IK ghost と解析 IK の失敗モード

teleop で ik_ghost が赤くなる原因。解析 IK (solve_ik_pitched) の 4 失敗と、URDF 可動域内でも失敗する数値・分岐由来の場面

要点 #

teleop は IK を通らない (joint_teleop → joint_target 直書き)。ik_ghost_node が実測 joint → FK で TCP → solve_ik_pitched に戻す監視ノード。ここで返る 4 status (out_of_reach / yaw_out_of_range / q1_out_of_range / q2_out_of_range) が IK の失敗種別。teleop で赤くなるのは主に elbow straight 近傍の数値誤差、 elbow branch のミスマッチ、wrist_pitch 補正後の外周越え。

数字 / コマンド #

solve_ik_pitched の失敗判定 (salamander_control/include/salamander_control/arm_kinematics.hpp):

status	条件
`OutOfReach`	余弦定理 `
`YawOutOfRange`	`atan2(y, x)` が `[yaw_min, yaw_max]` 外
`Q1OutOfRange`	解いた shoulder_pitch が URDF 域外
`Q2OutOfRange`	解いた beam_pitch が URDF 域外

topic: /salamander/ik_ghost/status (salamander_msgs/IkStatus) の reason に文字列。

詳細 #

ik_ghost の仕組み #

ik_ghost_node.cpp:

joint_states を購読、TF base_link → suction_link で現在 TCP を取得
tool_yaw を実測 joint から導出: tool_yaw = base_yaw + pad_mount_yaw + wrist_yaw、 tool_pitch = wrist_pitch (wrist_mount は水平)
solve_ik_pitched(TCP, tool_pitch, tool_yaw) を叩く
結果を ik_ghost/joint_state (ゴースト姿勢) と ik_ghost/status (色) に publish

teleop 自体には影響しない (関節指令は joint_teleop が直接 joint_target に書く)。 ghost は「今の姿勢に対して自律 IK は解けるか」を可視化する verification 用。

teleop で ghost が失敗する典型 5 パターン #

肘のばし切り (elbow straight) — beam_pitch → 0 で L1 と L2 が一直線。|c| = 1 ちょうど。FP で 1.0000001 に化けて OutOfReach。q2_min を -1° に緩めて吸収済み (逆側の僅かな反り返りをすくえる)。
beam_pitch max (URDF 0.5899 = 33.8°) — 4-bar 機構の実物可動域に URDF を合わせた。 teleop で max ちょうどを作ると solve_ik の解 q2 が max を僅かに超え Q2OutOfRange。
wrist_pitch != 0 で外周ぎり — solve_ik_pitched は tool_drop を tool_pitch と tool_yaw で回して wrist target を TCP - d に補正する。手先が既に L1+L2 ぎりぎりのときにピッチを立てると補正後 wrist target が外へ → OutOfReach。
base_yaw ≈ ±π — atan2 の値域 [-π, π] 境界の符号跳ね。teleop で base_yaw をぐるっと回して +3.14 に近づけると起きる (URDF が ±π ちょうどのため)。
elbow branch のミスマッチ — solve_ik_pitched(..., Branch::ElbowUp) が既定。 teleop で ElbowDown 相当の姿勢 (q1, q2 の符号組合せが反対) を作ると IK は「その TCP なら ElbowUp 解」を返し、実測 q2 と符号違い → Q2OutOfRange。

なぜ URDF 可動域内でも失敗するか #

解析 IK は 数値解ではなく閉形式。境界 (|c|=1, yaw=±π, q1/q2=lim) で FP 誤差に弱い
分岐選択が固定 (ElbowUp)。URDF 可動域は両分岐を許すが IK は片方だけ返す
URDF ↔ IK モデルの微妙な不整合 (4-bar 実物 vs 2R 近似の link 長)

実害 #

teleop は動く (joint 直書き、IK 通らない)
自律 (BT → arm_kinematics_node) で同じ TCP を狙うと同じく失敗 ⇒ BT が投げる target 座標を回避側にする、あるいは branch/tool_pitch を分けたリトライを組む
ghost の頻発は URDF 側の余裕 (q2_min -1° 等) で吸収済み。それでも赤が出る場合は上記 5 パターンのどれか