controller(controllers)
controllers: のnode YAMLのkeyです.controllerは入力(または方策)をfollowerの指令に変えます.controllerごとの使い方は Controllers を参照してください.
共通のkey
| key | 型 | 既定値 | 単位 | 説明 |
|---|---|---|---|---|
type |
文字列 | 必須 | — | joint,parts,pose_ik,policy か module:Class |
followers |
文字列のリスト | 必須 | — | 動かすfollowerのnode名(followerとcontroller) |
command_deadline_ms |
数値 | 必須 | ms | 指令の有効期限.過ぎるとfollowerは保持する |
follower_timeout_ms |
数値 | 150 |
ms | followerの状態がこれより古くなると,全followerへの指令を止める |
followerとcontroller
各controllerは followers: に,動かすfollowerを並べます.1台のfollowerを動かすcontrollerはちょうど1つです.
| controller | 動かすfollower |
|---|---|
joint |
ちょうど1台(leaderの関節を1台のfollowerの関節へ対応させる) |
pose_ik |
腕1台と,任意でgripper 1台(手に持った機器の姿勢をIKで腕の関節へ) |
parts |
ちょうど1台.内側のcontrollerがそれぞれ一部の関節を動かす |
policy |
全followerを設定の順に.記録した action と observation.state が全followerをつないだ形なので,方策の出力も同じ形です.出力を並び順に分けて各followerへ送ります |
followerが複数あるcontrollerは,全followerの状態がそろうまで指令を送りません.どれかのfollowerが FAULT や ESTOP になるか,状態が follower_timeout_ms より古くなると,そのcontrollerは全followerへの指令を止めます.指令が途切れたfollowerは,保持している目標を保ちます.controllerは,位置を合わせ直すまで次の指令を送りません.policy controllerは,followerの状態が古くなると,その回のepisodeの方策の実行を止めます.画面のcontrollerの行に,止めた理由(stopped_by)と,状態を待っているfollower(waiting_state)が出ます.
joint
leaderの関節を1台のfollowerの関節へ対応させます.入力は関節の入力(rakuda_mini など)です.始めに位置を合わせ(leaderとfollowerの差が alignment_tolerance 以内になるまで待つ),そのあと速さを制限して追従します.
| key | 型 | 既定値 | 単位 | 説明 |
|---|---|---|---|---|
input |
文字列 | 必須 | — | 入力のnode名 |
alignment_tolerance |
数値 | 必須 | followerの単位 | 追従を始める前と,episodeを始めるたびに許すleaderとfollowerの差 |
max_speed |
数値 | 必須 | followerの単位/s | 遠隔操作の目標を1周期に動かせる幅(単位) |
input_timeout_ms |
数値 | 必須 | ms | 入力がこれより古くなると指令を止める |
unit |
文字列 | — | — | 書くなら入力とfollowerの単位.全関節に scale があれば入力の単位は違ってよい |
joints |
リスト | 同じ名前の関節どうし | — | 関節の対応(下記) |
couplings |
リスト | [] |
— | 他の関節の変位を目標に足す(下記) |
home |
mapping | — | — | 保存した姿勢(home) |
feedback |
mapping | — | — | 力の逆送(下記) 模擬のみ |
関節の対応(joints)
入力がfollowerと同じ関節名と単位を持つ(Rakuda-miniとRakuda2)なら,書かなくてよく,各follower関節は同じ名前の入力関節に従います.名前,向き,ずれが違うときは,followerの関節の順に1関節1項目で書きます.follower = sign × scale × leader + offset です.
| key | 型 | 既定値 | 単位 | 説明 |
|---|---|---|---|---|
leader |
文字列 | 必須 | — | 入力の関節名 |
follower |
文字列 | 必須 | — | followerの関節名(followerの順) |
sign |
1 か -1 |
必須 | — | 向き |
offset |
数値 | 必須 | followerの単位 | ずれ |
scale |
正の数 | 1.0 |
followerの単位/入力の単位 | 歯車比や単位の違い.feedback とは併用できない |
関節の相殺(couplings)
couplings は,他のfollower関節の reference からの実測変位を,対応させたあとの目標に足します(速度の制限の前).胴体を回したときに頭のcameraの向きを保つ,といった使い方です.leaderの control(関節の固定など)とは独立した設定です.Leaderの設定例 を参照してください.
| key | 型 | 既定値 | 単位 | 説明 |
|---|---|---|---|---|
joint |
文字列 | 必須 | — | 目標に足すfollower関節.重複できない |
source |
文字列 | 必須 | — | 変位を測るfollower関節(joint と別) |
scale |
数値 | 必須 | — | 変位に掛ける係数 |
reference |
数値 | 必須 | followerの単位 | 何も足さない source の位置 |
lead_ms |
数値 | 0 |
ms | source の速度×この時間だけ先を見る(joint の位置制御の遅れの補い).0〜500 |
# nodes/controller.yaml
type: joint
input: leader
followers: [follower]
alignment_tolerance: 300
max_speed: 2048
command_deadline_ms: 100
input_timeout_ms: 150
couplings:
- {joint: head_yaw, source: torso_yaw, scale: -1.0, reference: 2048, lead_ms: 70}
parts 模擬のみ
type: parts のcontrollerを使うと,Rakuda2(1本のbusに17関節)のように関節の多いfollowerを,複数の入力で,それぞれ別の関節について動かせます.腕ごとに手持ちのdeviceを使う,片腕だけleaderで動かす,といった構成です.followerが受け取る指令は,今までどおり1つのcontrollerからの1本です.安全の検査,記録する action,datasetは変わりません.確かめたのは模擬のdeviceだけです.
followerは自分の部分に名前を付けます(followerの parts).
| key | 型 | 既定値 | 単位 | 説明 |
|---|---|---|---|---|
parts |
pathのリスト | 必須 | — | 内側のcontrollerのfile.このfileのdirectoryが基準.名前はfile名(拡張子なし) |
input_timeout_ms |
数値 | 150 |
ms | 入力がこれより古くなると指令を止める |
# nodes/controller.yaml
type: parts
followers: [follower]
command_deadline_ms: 100
parts: [right_hand.yaml, left_hand.yaml] # このfileの隣のcontrollerのfile
# nodes/right_hand.yaml:controllerの種類は自由.followers は部分(<follower>.<部分>)
type: joint
input: leader
followers: [follower.right_arm]
alignment_tolerance: 300
max_speed: 2048
command_deadline_ms: 100
input_timeout_ms: 150
- 内側のcontrollerは,自分の部分を1台のfollowerとして扱います(単位と周期は同じ,可動域はその関節のもの).それぞれ自分の入力に追従します.同じ入力に複数の内側のcontrollerが追従しても構いません.
- 1つの関節を動かせる内側のcontrollerは1つまでです.どの部分も動かさない関節は,followerが保持している位置に留まります.
- followerへの指令は全関節の1本なので,ある入力の指令には,ほかの部分の目標も入ります.入れるのは,その部分の目標がまだ有効な間だけです(指令してから内側のcontrollerの
command_deadline_ms以内で,そのcontrollerがそのあと指令を止めていないこと).入力が止まった部分,手を離した部分,IKが届かなかった部分には,followerが保持している目標を入れます.古い目標へ動き続けることはありません. - followerが
FAULTかESTOPになると,すべての部分が揃え直してから指令を再開します. - 内側のcontrollerには,まだ
homeとfeedbackを書けません.partsのcontrollerを方策のteleop:に使うこともできません. - controllerの行は,どれかの部分が指令を送っていれば
followingです.partsに内側のcontrollerごとの状態が出ます.
pose_ik 開発中
手に持った機器の姿勢を,IKで腕の関節に変えます.入力は姿勢の入力(pika_sense)です.操作者がclutchを入れた時点の入力の姿勢と腕の先端の姿勢を基準に,そこからの手の動きを先端の動きにします(相対の対応).使い方は Pika SenseとIK を参照してください.
| key | 型 | 既定値 | 単位 | 説明 |
|---|---|---|---|---|
input |
文字列 | 必須 | — | 姿勢の入力のnode名 |
arm |
文字列 | 必須 | — | IKを解く腕のfollower(または部分).運動学のmodelが必要 |
gripper |
文字列 | — | — | 顎の開きで動かす1関節のfollower(または部分) |
tool_offset |
{xyz, rpy} |
flange | m,rad | flangeから見た工具の座標系 |
workspace |
{min, max} |
なし | m | 腕の土台の座標系での目標の範囲.外れると指令しない.省略すると関節の範囲とIKだけで制限する |
max_input_jump |
{position_m, rotation_rad} |
必須 | m,rad | 1 sampleでこれより跳んだ入力は,見失ったとみなして操作を止める |
ik |
mapping | 下の表 | — | IKの設定 |
gripper_map |
{scale, offset} |
{scale: 1.0, offset: 0.0} |
— | gripper = scale × 顎の角度 + offset |
max_speed |
{follower: value} |
必須 | followerの単位/s | followerごとの速さの上限 |
input_timeout_ms |
数値 | 必須 | ms | 入力がこれより古くなると操作を止める |
home |
{follower: home} |
— | — | 保存した姿勢(home) |
ik のkey |
既定値 | 単位 |
|---|---|---|
iterations |
30 |
回 |
damping |
0.001 |
— |
position_tolerance_m |
0.005 |
m |
rotation_tolerance_rad |
0.05 |
rad |
policy
学習した方策がcontrollerになります.全followerを設定の順に動かし(followerとcontroller),入力は持ちません.keyは 方策のcontroller にまとめています.
力の逆送(feedback) 模擬のみ
実機では未検証
followerが受けた外力をleaderへ返す経路です.模擬(mock)の試験だけを通しています.設計と今後の段階は bilateral制御の設計 を参照してください.
設定に estimators を足し(推定器),controllerの feedback で推定器を参照します.templateでは,option bilateral が estimators.effort を追加し,leader・controller・followerを逆送用のnode fileに置き換えます.辺は書きません.各nodeの参照から次の経路を導きます.
leader ──sample──► controller ──command──► follower ──state──► estimator
▲ │ ▲ │
└──── feedback ──────┘ └────────────── estimate ──────────┘
feedback は,followerが1台の joint controllerでだけ使えます.scale のある関節の対応とは併用できません.leader側の設定は control の feedback です.
| key | 型 | 既定値 | 単位 | 説明 |
|---|---|---|---|---|
estimator |
文字列 | 必須 | — | 推定器のnode名 |
deadline_ms |
数値 | 必須 | ms | 推定の元になったfollowerの状態の時刻からの期限 |
unit |
文字列 | 必須 | — | <leaderの単位>/<推定器の単位>(例:mA/mA) |
gain |
{follower joint: gain} |
必須 | — | 逆送する関節と係数(0以上) |
feedback:
estimator: effort
deadline_ms: 100
unit: mA/mA # leaderの単位 / 推定器の単位
gain: # followerの関節名 → 係数(0以上)
r_arm_el_yaw: 0.1
leaderへの電流は −gain × sign × 残差 です(sign は joints の対応の符号).controllerは,推定が有効で接触中,followerが ACTIVE,controllerが following のときだけ,適用の希望(gate)を立てます.期限は,推定の元になったfollowerの状態の時刻から deadline_ms です.推定が遅れて届き,その時点で期限を過ぎていれば送らず,controllerのstatusの feedback_error に理由を出します.leaderは,feedback: を書いた電流制御のgroup(gravity_comp,impedance)の関節だけで受け取ります.
読み込み時に,参照先の存在,推定器とcontrollerのfollowerの一致,単位,leader側の受け入れを確かめます.合わない設定は起動前に拒否されます.couplings の対象の関節にも,joints の対応どおりに返します.Python API(InProcessSession)は推定器を扱いません.
推定器(estimators) 模擬のみ
estimators: は最上位の役割の1つです.推定器は,followerが申告する力の量(Rakuda2は currents_mA,単位mA)から,関節ごとの残差(実測 − 自由運動での予測)を出します.外力は残差と逆向きにかかります.
estimators:
effort: nodes/effort.yaml
推定器(nodes/effort.yaml):
| key | 型 | 既定値 | 単位 | 説明 |
|---|---|---|---|---|
type |
文字列 | 必須 | — | none(常に推定なし.経路の確認用)または model_residual |
follower |
文字列 | 必須 | — | 状態を読むfollowerのnode名 |
joints |
文字列のリスト | 必須 | — | 推定するfollower関節 |
models |
pathのリスト | 必須(model_residual) |
— | model_residual の重力model(腕ごと).出力関節を合わせると joints とちょうど一致すること |
smoothing_alpha |
数値 | 1.0 |
— | 指数移動平均の係数(0より大きく1以下.1=平滑化なし) |
contact_on |
数値 | 0 |
推定器の単位 | 残差のL1 normがこれ以上で接触とみなす |
contact_off |
数値 | 0 |
推定器の単位 | 接触中は,これを下回ると接触でなくなる(hysteresis).contact_off ≤ contact_on.どちらも0なら常に接触 |
record |
真偽値 | false |
— | true で推定をdatasetに記録する.recorder.max_age_ms にこの推定器の名前が必要 |
model_residual は 残差 = 実測電流 − g(q) です.g(q) はfollowerで同定した重力modelで,学習は要りません.摩擦・慣性・位置の追従誤差は残差に残るので,自由に動かしているときも0になりません.推定器nodeのstatusの residual_l1(接触の判定と比べる値)を,自由に動かしたときと押したときで見て,contact_on をその間に置きます.重力modelは,leaderと同じ identify-gravity で,followerのcalibration(calibration/follower_<side>.yaml)から作ります.
uv run roboloom calibrate-joints .roboloom/rakuda2/calibration/follower_right.yaml
uv run roboloom identify-gravity .roboloom/rakuda2/calibration/follower_right.yaml \
--out .roboloom/rakuda2/calibration/effort_gravity_right.yaml --collect
followerの腕は,XM540とXM430が混在するため関節ごとに別々に同定します(shared_parameters: false).leaderより多くの姿勢が要ります.同定中はfollowerのnodeを止めておきます.
record: true の推定器は,observation.effort_ext(外力=−残差,推定器の単位),observation.effort_ext.valid,observation.contact を記録します.推定がないframeは0です.記録する推定器が複数あるときは,設定の順につなげます.names は observation.effort_ext が <follower>.<joint>,ほかの2つが推定器の名前です.単位の違う推定器は1本のvectorにできないので,読み込み時に拒否します.