arm_supervisor が何を見張っているか

MIT モードは衝突しても押し続けるので、追従誤差と電流を見張って軌道を止める最終防衛線。tracking_error と overcurrent の 2 トリップ

要点 #

MIT モード (τ = kp·(qref−q) + kd·(dqref−dq) + τ_ff) は衝突しても kp × Δq が飽和するまで電流を上げ続ける。物理的な衝突センサは無いので、supervisor が 状態量から異常を推定して軌道を止める。位置制御ループの外側の異常監視サーキットブレーカー。

数字 / コマンド #

入力: ruckig/joint_setpoint (指令), joint_states (実測), fibril_control_msgs/Feedback (モータ電流、オプション)

トリップ条件 (salamander_control/params/arm_supervisor.param.yaml):

|setpoint − 実測| > error_threshold を error_hold_sec 継続 → tracking_error
|current| > current_threshold を current_hold_sec 継続 → overcurrent
setpoint が setpoint_timeout より古い間は判定休止 (指令停止中の誤検知回避)

トリップ時:

trajectory_generator_node/stop (std_srvs/Trigger) を 1 回呼ぶ → Ruckig 減速停止
supervisor/fault (String, latched) に "tracking_error:<joint>" or "overcurrent:<joint>"
停止後は setpoint 更新止まる → 誤差判定が自動休止 → 再アーム

閾値は実行時に param 変更可 (ros2 param set /salamander/arm_supervisor_node ...)。

詳細 #

なぜこれが必要か #

MIT モードは

追従性が高い (feed-forward トルクと gain 応答)
衝突しても止まらない (指令 vs 実測のズレを電流で埋めようとし続ける)

の裏返し。位置制御ループ内では検知手段がないので、supervisor という外部監視が要る。

tracking_error と overcurrent の使い分け #

症状	tracking_error	overcurrent
衝突 (硬い)	○ (Δq が瞬時に大きくなる)	○ (kp × Δq で電流飽和)
拘束 (途中で止まる)	○	○
脱調 (encoder ズレ)	○ (実測が指令と乖離)	×〜△
過負荷 (重力コンプ不足)	△ (定常誤差)	○ (連続的に電流高い)
センサノイズ	△ (瞬間 spike で誤検知しうる → hold_sec で吸収)	×

両方持つ ことで検知漏れを減らす。

再アーム機構 #

トリップ → stop → setpoint 停止 (Ruckig が減速停止して以降 publish しない or 同じ値) → setpoint_timeout 超えて誤差判定休止 → fault を自動でクリアするわけではない が判定は止まる。次のリトライで setpoint が再度動き始めると判定再開。再衝突すれば再度トリップ (自己制限)。

実機チューニングのコツ #

通常動作の最大電流を先に測る (ros2 topic echo で feedback を回収)。それより 20-50% 上を current_threshold に。知らずに 0 のままにすると overcurrent は無効
error_threshold は URDF の関節速度 × dt を目安に。指令追従が遅い関節ほど大きめ
hold_sec は瞬間ノイズを弾く。50-200 ms 程度

診断への集約 #

robot_diagnostics_node が supervisor/fault を購読して /diagnostics に流す。 foxglove/rviz の diagnostic aggregator でトリップ理由が見える。