Homing 実装 (RC27)

要点: Homing の一般設計 (計算 param 校正 / 状態機械) を motor_test 側に実装した。まず bench で 1 モータ検証、機構が付いたら rc27_control 側に移送する順。

実装済み (motor_test)#

なぜ motor_test 側から: bench 先行検証 の方針。1 モータで挙動が固まってから rc27_control (機構ノード) に移送する。

Python から C++ 移行: motor/control 系は基本 C++ の方針 (memory feedback)。既存 homing_calibrate.py / homing_state.py / friction_sweep.py / current_calibrate.py / traj_probe.py は参照用として残す。新規は C++。

Param (3 必須、12 default)#

/rc27/lift_homing:
  ros__parameters:
    target_topic:   /rc27/lift/target
    feedback_topic: /rc27/lift/feedback
    velocity_home:  0.5        # 壁に押し当てる速度 [rad/s] (VELOCITY mode。2026-09 に duty_home から改名)
    # timeout_ms:   5000       # default 5000ms. URDF stroke/min_speed×3 で決めるのは呼ぶ側
    # backoff:      0.05       # default 0.05 で通る想定
    # reset_encoder_on_done: true  # 壁到達で feedback.position を 0 にする
    # joint_name:   "front_lift"   # URDF joint 名 (空文字で over-travel guard 無効)
    # urdf_safety_margin: 0.1      # URDF limit の外側の余裕 (limit 到達を許す)

mode 移行: 初期実装は DUTY (電流指令) だったが、RoboMaster MD の firmware が電流を regulate するため壁到達時に電流スパイクが出ず、S4 の over-current 検出が構造的に成立しない。2026-09 に VELOCITY 指令に切替 (current-based-endstop-detection)。旧 duty_home は velocity_home にリネーム。値の意味も duty[-1,1] から rad/s (m/s) に変わっている。

Default 埋め込み: timeout_ms=5000, ramp_time_ms=200, move_baseline_ms=100, v_min_hold_ms=50, start_gate_ms=100, k_current_margin=3.0, baseline_ms=150, reset_encoder_on_done=true, joint_name="", urdf_topic="/robot_description", urdf_wait_s=5.0, urdf_safety_margin=0.1.

URDF over-travel guard (Outside mode)#

joint_name を指定すると motor_test_urdf_utils の RobotDescription を使って URDF joint limit を監視。逸脱時 OVER_TRAVEL として FAULT。Homing は意図的に壁に到達するので、bounds は [lower - margin, upper + margin] の Outside (limit 到達を許す)。margin default 0.1 で過走行 sanity check 用。

内部は motor_test::tuning::UrdfGuard (旧名 calibration は既存 ADC 校正と混同するので rename、将来 fibril_tuning へ移送予定)。

stroke を持たない設計 (2cfbe9d): URDF joint <limit lower/upper> が機構範囲の原本。homing yaml に stroke を持つと duplicate になり drift の元。timeout_ms は呼ぶ側 (URDF から stroke を拾って ×3) で決める。

Param 名は単位を含まない (linear 軸で意味が変わらない)。encoder.gain が機構側 rad or m を吐く思想。

テスト launch (推奨)#

一連の flow を 1 launch にまとめた ff_homing_test.launch.xml が使える (motor_test/feature/single-motor-launch 3945fb8):

# homing yaml 空で起動 (motor + rosbridge + bag、homing 未待機)
ros2 launch motor_test_bringup ff_homing_test.launch.xml

# 校正後、yaml を指定して再起動 (homing service 待機)
ros2 launch motor_test_bringup ff_homing_test.launch.xml \
  homing_yaml:=/path/to/homing_test.yaml

手順は motor_test_bringup/params/ff_homing_test.README.md を参照 (8 段階: 起動 → 電流校正 → PID/cascade → homing 校正 → homing 実行 → traj_probe → FF ON/OFF 比較 → bag 分析)。

使い方 (低レベル)#

校正 (機構ごとに 1 回)#

安全に動く方向を --direction で指定して、始点が limit じゃない位置から:

ros2 run motor_test_bringup homing_calibrate.py \
  --direction +1 --stroke 3.0 --mech-name lift

出力に homing_lift.yaml snippet が出るのでコピペ。

実行#

# Python 版 (単発 or service)
ros2 run motor_test_bringup homing_state.py --ros-args \
  --params-file /path/to/homing_lift.yaml -p run_once:=true

# C++ 版 (本命)
ros2 run motor_test_bringup homing_state_node --ros-args \
  --params-file /path/to/homing_lift.yaml -p run_once:=true

# 常駐 + service
ros2 service call /homing_state/trigger std_srvs/srv/Trigger '{}'

実測から分かったこと (2026-08-16、無負荷 RoboMaster)#

無負荷 (机上、機構なし) で 1 モータ回した結果:

分かった問題点#

1. 電流の符号が duty と逆: +duty → -current, -duty → +current。FW 側で CURRENT_POLARITY として吸えるようにした (161a136)。使い方: ros2 param set /test/md_single motor0.current.polarity -1.0 で +duty → +current の物理慣例に揃う
2. 無負荷では duty→current の線形性が取れない (R²=0.77): 加速電流と負荷電流が混ざる。機構が付いてから stopper に押し当てて再測する必要がある
3. sigma_off = 0.0000 になったケース: 50 ms の baseline だとサンプル 5 個で退化。baseline_ms=150 に上げて対処済み
4. min_speed 測定に加速中が入ってバイアス: 修正済 (末尾 --speed-tail=0.3 s の平均のみ使う)

次にやりたいこと (優先順)#

短期 (機構待ちなくできる)#

1. FW 側で電流符号を修正 — 完了 (161a136)。current.polarity param で -1.0 に設定して吸える
2. URDF joint limit を homing に流す — 完了 (aef168b + 8a56a3f)。motor_test_urdf_utils の RobotDescription を motor_test::tuning::UrdfGuard 経由で subscribe、joint_name 指定で over-travel guard 有効
3. homing_state の S0 を短縮 / 削除: 電流零点校正 が FW 側で効いてる前提なら σ_off を毎回測る意味が薄い。σ_move はまだ要るので S3 は残す
4. current.offset を yaml で復元: 校正結果を yaml に書ける形にする。既に can_md_controller の param として存在 (current.offset) → 起動時 setCurrentOffset で復元、毎回 service 呼ばなくて済む
5. FW-side position bound: LIMIT_PROFILE (V5) は toolbox で deprecated。後継は未設計。host の bug で範囲外指令が出ても FW でクランプする仕組みが要る (別 issue)

中期 (実機構が付いてから)#

4. 実機構で再校正: 無負荷値は捨てて lift / grip / arm ごとに homing_calibrate を回す。stroke_rad を図面値に、velocity_home は「押し当て速度 (slow/normal/fast)」的な 3 段階 UX にしても良い (VELOCITY モード移行後は「押し当て強さ」ではなく「速度」で分岐する)
5. homing_state を rc27_control に移送: feedback-mechanism-nodes-in-rc27-control に従って、機構ノードとして rc27_control 配下に置く。yaml は rc27_control/config/homing_{lift,grip,arm}.yaml
6. S_LIMIT_AT_START の自動確認: 現状は state 返して stop。「逆方向に低 duty で 300ms 戻して動きが見えたら limit 確定」を script 内で自動化 (現在は呼び出し側 or 手動)
7. BT との統合: HomingSkill として BT ノード化。SetBool でトリガ、DONE / FAULT を返す

長期#

8. mech_calibrator ノード: 起動シーケンスで自動的に homing + friction 校正まで走る。計画別ノート
9. 同時多軸 homing: 複数機構を並列に homing する場合の資源競合 (電流ピーク、電源電圧降下) の抑制

関連#

• 一般設計:
  • Homing 用パラメータ校正
  • Homing の状態機械
  • 電流ベースの homing
  • 電流センサの零点校正
• RC27 内:
  • FF スタック実装 (RC27) — 電流校正は FF と同じ V7 branch
  • PID/FF 自動チューニング計画 (RC27)