beam-shoulder カップリング制約 (salamander)

要点#

beam(q2) の上下限は shoulder(q1) に連動する動的制限。trajectory_generator_node.cpp が joint_target を受けたときにクランプする。URDF の固定 limit では表現できない平行リンクの物理停止を強制する。

実装場所#

salamander_control/src/trajectory_generator_node.cpp(IK 側 arm_kinematics は固定 q2 limit のみ。連動はこのノードだけ)。

• L91–92: パラメータ既定値 alpha_max = 1.4312 rad(82°)、beta_max = 1.1020 rad(63.14°)。trajectory_generator.param.yaml に未記載なのでコード default が実効値。値は機械的実測 (2026-09-09) 由来。arm_kinematics.hpp(IK 可解性判定)側の default も同値。
• L300–319: enforcement 本体。
• L455–456: 同制約の Ruckig limit 側コメント。

q2_max_coupled = q1 + alpha_max;   // 上限: q2 - q1 ≤ alpha_max (後傾で beam 下げすぎ防止)
q2_min_coupled = q1 - beta_max;    // 下限: q1 - q2 ≤ beta_max  (前傾で beam 上げすぎ防止)
if (q2 > q2_max_coupled) target[beam] = q2_max_coupled;
else if (q2 < q2_min_coupled) target[beam] = q2_min_coupled;

q1 = shoulder_pitch、q2 = beam_pitch。

実効値#

固定 limit と合成した実効範囲(alpha=82°, beta=63.14°):

• beam 上限 = min(q1 + 82°, 75°)(URDF beam upper = 75°)
• beam 下限 = max(q1 - 63.14°, 0°)(URDF beam lower = 0°)
• URDF: shoulder(q1) ∈ [-45°, 75°]、beam(q2) ∈ [0°, 75°]

読み方#

• coupling 上限が効くのは shoulder 後傾側(q1 < -7°)だけ。q1 = -45° で beam 上限 37.0°。
• coupling 下限が効くのは shoulder 前傾側(q1 > +63.14°)だけ。q1 = +75° で beam 下限 11.9°。
• 中央域(q1 = -7°〜+63.14°)では固定 URDF limit(0°〜75°)がそのまま効き、coupling は何もしない。
• alpha/beta の値は機械的実測 (2026-09-09) で決めた境界(上限 q1=-7°、下限 q1=+63.14°)を再現するように設定。

alpha_max / beta_max を変えたい場合は trajectory_generator.param.yaml に追記する。

関連#

• /docs/project/salamander/ik-and-trajectory-pipeline
• /docs/robotics/urdf-joint-limits
• /docs/robotics/urdf-parallel-linkage-mimic