gravity_compensation_node の内部モデル

URDF を Pinocchio で build (mimic=true で 4-bar 縮約) → g(q) = generalizedGravity。payload は円柱 inertia を tool 側 joint に加算した別 model で切替

要点 #

Pinocchio で URDF から build。mimic=true で crank_parallel / beam_parallel を縮約 して独立 DoF 3 個の model に落とす。computeGeneralizedGravity(model, data, q) で g(q) を出し、 gravity/joint_command.effort に書く。下流の bridge が MIT モードの τ_ff に流す。

払い出し (payload) 側は 円柱 inertia を tool 側 joint に加算した別 model を事前 build しておき、gripper/holding (Bool) で参照ポインタを切替。

数字 / コマンド #

subscribe  ruckig/joint_setpoint   (JointState pos[, vel])
subscribe  gripper/holding         (std_msgs/Bool)
publish    gravity/joint_command   (JointState pos + vel + effort)

param (gravity_compensation.param.yaml):

enable: true/false — false で effort=0 素通し (比較用)
scale: 1.0 — 実機ギア向き対策の符号/大きさ調整。実行時変更可
payload.mass / radius / length / com / parent_joint / attached

単一の真実:

arm 各リンク mass/inertia/origin: URDF (salamander.urdf.xacro)
payload: payload.mass = 2 × field.yaml workpiece_mass_kg (test_field_logic GravityPayloadMassMatchesWorkpieceMass が検査)

詳細 #

model 構築 #

pinocchio::urdf::buildModelFromXML(urdf, model_, /*verbose=*/false, /*mimic=*/true);

mimic=true が肝。URDF の

<joint name="crank_parallel">
  <mimic joint="shoulder_pitch" multiplier="-1.0" offset="0.0" />
</joint>

を Pinocchio が model 構築時に縮約する。手で拘束式を書かなくても独立 DoF 3 個 (base_yaw shoulder_pitch beam_pitch) の model が出る。

g(q) 計算 #

Eigen::VectorXd g = pinocchio::computeGeneralizedGravity(model, data, q);

内部は RNEA (Recursive Newton-Euler Algorithm) を dq=0, ddq=0 で走らせている。

τ_gravity(q) = ∂U/∂q = Σ_i J_i(q)^T · m_i · g_world

出力は各独立関節の重力トルク [N·m]。3 × 1 ベクトルを URDF joint 順から入力 JointState.name 順に写して effort に書く。

payload の載せ方 #

pinocchio::Inertia payload = pinocchio::Inertia::FromCylinder(mass, radius, length);
pinocchio::SE3 placement(Identity, Vector3d(com_x, com_y, com_z));
model_loaded_.inertias[jid] += placement.act(payload);

FromCylinder で対称テンソル (main-axis is z) を作り、SE3.act(inertia) で tool フレーム基準の重心オフセット (デフォルト com=[0,0,-0.07] = pad の下 7cm) に移して、parent_joint (デフォルト beam_parallel = tool 側リンク) の inertia に加算。

別 model を事前 build することで切替コストがゼロ (ポインタ差し替え)。

holding 変化への即応 #

payload_sub_ = create_subscription<Bool>("gripper/holding", 10, [](msg){
  if (msg->data != payload_attached_) {
    payload_attached_ = msg->data;
    if (last_setpoint_) on_setpoint(*last_setpoint_);  // 即再計算
  }
});

次の setpoint 到着まで待たない ことが重要。掴む/離す瞬間は setpoint stream が止まっている (静止中) ことが多く、待つと缶の重さ分アームが垂れる。最後の setpoint を使って即 τ_ff を更新する。

enable / scale の実行時変更 #

ros2 param set /gravity_compensation_node enable false で FF を切って実機挙動を比較。 scale も同様。変更検知でも last_setpoint を使って即再計算。

制限 #

静止トルクのみ (dq=0, ddq=0)。動的な慣性項・コリオリは含まない。速い動きで追従誤差が乗るが、Ruckig の 3 rad/s 域なら無視可
摩擦補償なし — motor 側の friction は motor_ff_tuner (別ノード) が別途 FF する。参照 [[gravity-friction-identification]]
holding は単一 Bool — 1 缶だけ掴んだ状態も 2 缶ぶんの補償 = 0.065 kg 過補償。許容
wrist_pitch > 0 で缶を立てた状態 の合成重心移動は近似で据え置き (TODO(hw): pitch 依存 com が要れば追加)

URDF 変更が自動追従する #

Pinocchio が robot_description (param) から build するので、URDF の mass/inertia を変えると gravity_comp も自動で追従する。yaml に mass を書き写す必要なし。今日 (2026-09-05) の crank_link 0.70 → 0.40、beam_link 0.48 → 0.40 も自動反映。