実機立ち上げで決める値

要点: 順序は モータ設定 → 重力 FF → 関節ゼロ → 手先照合 → 機体設置 → カメラ TF → 動作定数。M2006 (MD 基板) は別系統で、feedback → 向き → gain → kp の順。関節ゼロは 0.3° 以内 (手先 4 mm) が必要。arm は RS05 (base_yaw / wrist_yaw) + RS00 (shoulder/beam) + M2006 (wrist_pitch)、ee は pad_stroke + wrist_pitch、aliner は 2 軸で計 M2006 は 4 軸。

一覧#

1. 減速比・回転方向#

正解は URDF (salamander_bringup/urdf/salamander.urdf.xacro)。

# salamander_control/params/arm_motors.param.yaml
motors:
  shoulder_pitch:
    gear_ratio: 3.0    # 関節 1 rad あたりのモータ rad
    invert: false      # true でモータ回転を反転

換算は robstride_adapter (driver.cpp: motor = load × G × sign、τ_motor = τ_load / G × sign)。motion_target と feedback は関節側。同じ値を写す先: isaac_motor_adapter.param.yaml gear_ratio、URDF effort = 14 N·m × G (test_urdf_consistency が突き合わせる)。kp/kd はモータ側なので関節側剛性は G²·kp。

測り方:

ros2 launch salamander_bringup salamander.launch.xml \
  motor_names:="[shoulder_pitch]" control:=false teleop:=false perception:=false usb_camera:=false
# motor_names default は [base_yaw, shoulder_pitch, beam_pitch, wrist_yaw] (wrist_pitch は M2006 なので除外)
ros2 launch salamander_bringup rviz.launch.xml

無通電で手で 90° 回し、RViz のモデルが同じ向きに 90° 動けば gear_ratio と invert が正しい。270° なら G=1 のまま。逆向きなら invert。初回の limits は torque 5 N·m / speed 3 rad/s (関節側) のまま。

3. 関節ゼロ点#

腕長 400 + 450 mm。shoulder_pitch 1° のずれで手先 850 × sin 1° = 15 mm。缶径 66 mm、sim では 16〜21 mm のずれで 1 本倒れた。目視の水平は 1〜2° の誤差があり不足。

• pitch 系: crank 鉛直面・beam 上面・link_hand 側面にデジタル角度計 (0.1°) を当て 0.0° で reset_encoder {offset: false, value: 0.0}
• yaw 系: 機械ストッパかピン穴に当てた位置で取る。当たり位置がゼロでなければ value にオフセット (rad)
• 最後に save_parameters。忘れると電源断で消える
• ゼロ姿勢を写真付きで robot_profile/ に残す

4. 手先照合#

field.yaml に座標がある点 (棚スロット中心) を 3 つ選び、関節 jog でパッドを触れさせ、TF base_link → suction_link と比較する。3 点とも同じ向きの残差 → tool_drop か機体設置。点ごとに向きが違う → 関節ゼロ。パッドの圧縮量 (数 mm) を先に疑う。

5→6 の順序#

calibrate_camera_mount.py は機体が field.yaml の位置にある前提で解く。設置 → 手先照合 → カメラ校正 の順。

7. 動作定数#

place_dz 0.16 release_dz 0.04 grasp_tol 0.02 は sim の値。manual_place:=true で走らせ、ログ confirmed, offset [x, y, z] mm を読む。毎回同じ向きなら field.yaml か URDF に戻す。

8. M2006 (MD 基板)#

エンドエフェクタ pad_stroke (prismatic [m]) と wrist_pitch (revolute [rad])、整頓機構 aliner_x / aliner_push は RoboMaster M2006 + C610 を部内 MD 基板 (can_md_controller) で回す。位置 PID は基板側、ROS は Target(POSITION) を joint 単位で渡す。wrist_pitch は M2006 出力に外付ギア 3:2 (motor 3 rev → joint 2 rev) が挟まる。

ros2 launch salamander_control actuator_driver.launch.xml arm:=false aliner:=false   # ee_md だけ
ros2 topic echo /salamander/arm/pad_stroke/motor/feedback
ros2 topic pub -1 /salamander/arm/pad_stroke/motor/target fibril_control_msgs/msg/Target "{type: 0, value: 0.1}"  # duty 0.1 = 1 A
ros2 service call /salamander/arm/pad_stroke/motor/reset_encoder fibril_control_msgs/srv/ResetEncoder "{offset: false, value: 0.0}"

決める値 (md_motors.param.yaml):

velocity.max (duty 上限) は 0.3 = 3 A (M2006 連続定格) から。8192 count/rev、36:1、2.1305e-5 rad/count。

値の置き場所#

測った日付・方法・値を robot_profile/ の 1 ファイルに書き、各 yaml はそこから写す。

9. IK / FK 検証 (関節ゼロ後、動作定数の前)#

FK は robot_state_publisher が URDF から算出する TF を読むだけで十分。

ros2 run tf2_ros tf2_echo base_link suction_link

field.yaml の既知 3 点にパッドを触れさせ、TF の xyz と field 座標を比較。3 点とも同方向にズレ → tool_drop か機体設置 (#5)、点ごとに違う方向 → 関節ゼロ (#3)。

IK は arm_cartesian_motion_node (arm_control.launch.xml cartesian:=true)。入力トピック:

• arm/target_position (PointStamped) — 手先位置
• arm/tool_yaw / arm/tool_pitch (Float64, latched) — 手先姿勢
• 出力: arm/joint_target (JointState) — IK 解

sim で先に検証、実機で照合の順:

# sim
ros2 launch salamander_bringup salamander_sim.launch.xml
ros2 topic pub --once /salamander/arm/tool_yaw std_msgs/msg/Float64 "{data: 0.0}"
ros2 topic pub --once /salamander/arm/tool_pitch std_msgs/msg/Float64 "{data: 0.0}"
ros2 topic pub --once /salamander/arm/target_position geometry_msgs/msg/PointStamped \
  "{header: {frame_id: base_link}, point: {x: 0.5, y: 0.0, z: 0.3}}"
ros2 topic echo /salamander/arm/joint_target -n 1        # IK 解
ros2 run tf2_ros tf2_echo base_link suction_link          # 到達後の実 TF

joint_target が妥当 (可動域内・非特異) で tf2_echo の値が target_position ± 5 mm なら IK と URDF は整合。実機で同じ target を投げ、feedback の実測 → tf2_echo が target と一致すればハード側 (invert / ゼロ) も整合。

切り分け:

10. posture プリセット (簡易 IK 検証)#

postures.param.yaml の home / rest は関節値直指定なので IK を通らない。IK 側の粗い検証には pad_stroke.param.yaml の grasp / place を使う (プリセット座標は field.yaml 由来)。

関連#

• /docs/project/salamander/gravity-friction-identification
• /docs/project/salamander/manual-final-placement
• /docs/project/salamander/teleop-controls