shoulder-beam のカップリング可動域

要点#

q1 (shoulder) と q2 (beam) の相対角に上下 2 つの物理停止がある。URDF の per-joint limit では書けないので、IK (arm_kinematics.hpp::solve_ik) と trajectory_generator_node の joint_target post-clamp、両方に同じ制約を置く。 teleop の joint jog は IK を通らないので trajectory_generator 側の clamp が防波堤。

数字 / コマンド#

上限: q2 − q1 ≤ alpha_max  (default 1.40 rad = 80°)  — shoulder 後傾で beam 下げすぎ
下限: q1 − q2 ≤ beta_max   (default 0.87 rad = 50°)  — shoulder 前傾で beam 上げすぎ

ROS param で調整:

ros2 param set /arm/trajectory_generator alpha_max 1.40
ros2 param set /arm/trajectory_generator beta_max  0.87

同名 param が arm_kinematics_node / arm_cartesian_motion_node にも要る (未実装なら コード改修必要)。IK 側で越えたら IkStatus::CoupledLimit を返し、motion_core が early-fail する。

per-joint limit (URDF):

q1 (shoulder_pitch): [-1.35, +1.35]  rad
q2 (beam_pitch):     [ 0.00, +1.309] rad  (= 0° to 75°, 2026-09-09 で 33.8→75° に拡張)

詳細な背景#

parallelogram linkage の 2 モータ (shoulder / beam) は base で並んでいて、 それぞれのモータ角がリンクの絶対姿勢角になる。独立には見えるが、機構ハウジング内で 互いに干渉するため、絶対角の差 q2 − q1 と q1 − q2 の両方向で物理停止がある。

上限 α: q2 − q1 ≤ 80°#

shoulder が後傾 (q1 が負) すると beam が下げられる方向にハウジングと干渉する。 実測:

• q1 = −45° (rest) で q2 max ≈ 33.8° → q2 − q1 = 78.8°

rest 姿勢 (q1=−45°, q2=+33.8°) をぎり通す 80° を default に。狭くしすぎると rest 姿勢が IK 通らなくなる。

下限 β: q1 − q2 ≤ 50°#

shoulder が前傾 (q1 が大) すると beam が引っ張られて上げられる方向に干渉する。 2026-09-09 手動 bag 実測:

50° margin だと q1 = +80° で q2 min = +30° (実測 32.7° と一致)。

実装箇所#

• salamander_control/include/salamander_control/arm_kinematics.hpp ArmParams::alpha_max / beta_max。solve_ik の per-joint limit チェックの後に q2 − q1 > alpha_max と q1 − q2 > beta_max を判定して CoupledLimit を返す
• salamander_control/src/trajectory_generator_node.cpp Ruckig の後段で joint_target を post-clamp。beam を上下どちらかの coupled 上限に 切り詰めて WARN を吐く。IK を通らない teleop joint jog / posture の防波堤

やっていない事#

• URDF に書く方法はない。URDF joint limit は independent per-joint しか表現できない
• DLS IK / 特異点回避は Phase 1 では入れていない。IK 解析式が境界で CoupledLimit を返すので motion_core が early-fail する。将来 cart twist jog を入れる時に検討
• α / β の実測は手動 rosbag で取っただけ。厳密な物理限界は当てていない。運用で margin 不足が出たら param で狭める

関連#

• IK と軌道の 2 段構え
• IK の失敗モード
• パッケージ配置