FF スタック実装 (RC27)

要点: FF スタック (一般設計) を RC27 の CAN MD 系に実装済み。FW 側 (CanMotorMbed) と Host 側 (ros2_can_toolbox) の両方に対応 API がある。

実装済み branch#

API 対応#

一般設計の各項に対して:

Trajectory 入口 (can_md_controller)#

can_md_controller に ~/motor{i}/trajectory (trajectory_msgs/JointTrajectoryPoint) subscribe を追加済み。100 Hz の trajectory msg を流すと、motor->sendTrajectoryPoint(p,v,a) が呼ばれる。

positions/velocities/accelerations の [0] を採用 (単軸なので配列先頭固定)。accel_ff_gain は param から都度読む。

電流零点校正 (V7 拡張)#

feature/ff-stack に追加 (0cc220c + 161a136):

• FW: MotorWorker::_current_offset + BaselineState state machine。current_sense() は raw - offset を返す
• 起動方式: on-demand (service call)、auto boot はしない (boot 時に motor 静止の保証ができないため)
• 静止判定: DUTY mode かつ duty limiter output ~0 かつ encoder speed ~0。動かされたら abort、前回 offset 維持
• Config: CURRENT_BASELINE_START (value = 秒、default 0.5、cap 5)、CURRENT_OFFSET (読み書き)
• CURRENT_POLARITY (161a136 で追加): HW/配線反転を吸う。実測で +duty → -current になっていたので、current.polarity: -1.0 を設定すると physically 正しい sign convention (+duty → +current) に揃う。default 1.0、1.0 以外の時のみ MD へ送信

Host 側 (feature/ff-stack-api の f2a422d):

• Toolbox: Motor::triggerCurrentBaseline(sec), setCurrentOffset(v)
• can_md_controller service: ~/motor{i}/calibrate_current_baseline (std_srvs/Trigger)。呼ばれると FW に kick、duration + 100ms sleep で success 返却
• can_md_controller param: current.baseline_duration_s (0.5)、current.offset (0 以外なら起動時に setCurrentOffset)

呼び出し順の推奨:

# 1. motor を止める
ros2 topic pub -1 /test/motor/target fibril_control_msgs/Target '{type: 0, value: 0.0}'

# 2. baseline
ros2 service call /test/md_single/motor0/calibrate_current_baseline std_srvs/srv/Trigger '{}'

# 3. 以後 feedback.current は 0 中心

一般設計は 電流センサの零点校正。

sendTrajectoryPoint#

軌道の 1 サンプル (pos, vel, acc) を渡すと、上記 API を組み合わせて FW に流す:

motor.sendTrajectoryPoint(pos, vel, acc);
// 内部で:
//   sendPosition(pos)
//   sendVelocityFF(vel)
//   sendDutyFF(accel_ff_gain * acc + gravity_model(pos))

End-to-end 確認手順#

推奨: 統合 launch + calibrator#

# 1. motor を立ち上げる
ros2 launch motor_test_bringup ff_homing_test.launch.xml

# 2. 校正 (どの stage を回すかは param で指定)
ros2 launch motor_test_bringup motor_calibrator.launch.xml
# 部分だけ回すなら override:
#   stages.friction:=false stages.accel_ff_gain:=false
# → /tmp/calibration_result.yaml に snippet 出力

motor_calibrator_node は param 駆動で 4 stage (current_baseline / friction / accel_ff_gain / gravity) を順に実行 (2b33ae1)。詳細は params/motor_calibrator.param.yaml 参照。

低レベル: 個別コマンドで叩く場合#

# 1. 起動
ros2 launch motor_test_bringup single_motor.launch.xml

# 2. 電流校正 (motor 停止確認 → service call)
ros2 topic pub -1 /test/motor/target fibril_control_msgs/Target '{type: 0, value: 0.0}'
ros2 service call /test/md_single/motor0/calibrate_current_baseline std_srvs/srv/Trigger '{}'

# 3. PID gain
ros2 param set /test/md_single motor0.velocity.kp 0.3
ros2 param set /test/md_single motor0.position.kp 5.0
ros2 param set /test/md_single motor0.use_cascade_position true

# 4. FF なしで sin 追従を bag
ros2 bag record /test/motor/trajectory /test/motor/feedback -o baseline_no_ff &
ros2 run motor_test_bringup traj_probe.py --shape sin --amplitude 3.0 --period 2.0 --duration 10

# 5. friction を有効化 (friction_sweep.py で測って):
ros2 param set /test/md_single motor0.friction.coulomb 0.02
ros2 param set /test/md_single motor0.friction.viscous 0.005

# 6. accel_ff_gain (最後)
ros2 param set /test/md_single motor0.accel_ff_gain 0.001

traj_probe.py は velocities[0] を自動で送るので velocity_ff は param 設定不要で自動有効。

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

短期#

1. V7 PR 3 本の merge: CanMotorMbed/feature/ff-stack → main、ros2_can_toolbox/feature/ff-stack-api → main、motor_test/feature/single-motor-launch → main。相互参照。旧 10-byte 系 PR (#225/226/216/217) は既に close 済
2. 実走回帰: motor_test bench で Ruckig 10 rad/s trajectory を bag 取り、位相遅れが 15-21 ms に入るかの再現確認 (10-byte 版 rec_0eY53h1bh4xbfTA9 との比較)
3. 候補確認: setFrictionCompensation で低速 (0.5 rad/s) の追従誤差が縮む、FF 送信停止で 100 ms で FF が自然消滅する (watchdog)
4. 電流符号の FW/HW 修正 — 完了 (161a136)。current.polarity: -1.0 で吸える
5. motor_calibrator に URDF safety guard — 完了 (c6063ef)。joint_name 指定で motor_test::tuning::UrdfGuard(Inside) が有効化、逸脱で stage abort
6. FW-side position bound: LIMIT_PROFILE (V5) は toolbox で deprecated。後継の設計・実装が必要 (別 issue、host バグの二重防御用)

中期#

5. gravity_model の param 化: 現状 host 側 callback のみ。yaml で式 (A*cos(θ+φ) 等) を書けるようにするなら拡張が要る
6. sendTrajectoryPoint を実機構で回す: bench (motor_test) → br_linear 機構 → BT integration の順
7. Kt 換算の集約: 現状は各所で個別。accel_ff_gain = J/Kt を機構ごとに 1 箇所に持つ。RoboMaster は duty=1.0 → 10 A の正規化のみ ([[robomaster-normalization]])

長期#

8. mech_calibrator ノード (パラメータ自動同定): 計画は別ノート。起動シーケンスで自動的に friction + accel_ff_gain まで求める
9. 軌道 generator の統合: br_route_follower が pos/vel/acc を吐く (現状 pos だけ)、または独立の trajectory_generator (Ruckig ラッパ)

関連#

• 一般設計: FF スタック
• 一般手法: PID と FF 項の自動チューニング
• 計画: PID/FF 自動チューニング (RC27)