FixedJointで「掴む」を実装した

CCD IKでアームが動くようになったLOG90に続き、LOG91では「掴む」を実装した。

ロボットアームがコーヒーカップの位置に到達したとき、FixedJointを動的に生成してカップをアームに固定する。設置ポイントに到達したらFixedJointをDestroyして解放する。これだけのことだが、UnityのArticulationBodyを使う環境ではやり方を間違えると即崩壊する。

実装した把持の仕組み

ステートマシンを2ステートから4ステートに拡張した。

REACH_CUP → GRASP → REACH_PLACE → PLACE → REACH_CUP（ループ）

把持のキーポイントは3つ。

FixedJointを動的に生成する transform.SetParent()によるParentingは使わない。ArticulationBodyの階層を動的に変えるとシミュレーションが崩壊するリスクがあるため、FixedJointで物理的に繋ぐ方式を選んだ。

graspJoint = cupObject.AddComponent<FixedJoint>();
graspJoint.connectedBody = endEffector.GetComponent<Rigidbody>();
graspJoint.breakForce  = Mathf.Infinity;
graspJoint.breakTorque = Mathf.Infinity;

距離0.05m以下を把持トリガーにする IKがカップの把持点（HandlePoint）に到達したかどうかを距離で判定する。今日の実測では2mm精度で到達を確認。

設置時はDestroyするだけ PlaceTargetに到達したらDestroy(graspJoint)の一行で解放。物理シミュレーションがそこから先を引き受ける。

動作確認ログ

Consoleに以下のログが順番に流れることを確認した。

[RobotArm] → GRASP
[RobotArm] GRASP: FixedJoint 生成完了
[RobotArm] → REACH_PLACE
[RobotArm] → PLACE
[RobotArm] PLACE: FixedJoint 破棄完了
[RobotArm] → REACH_CUP（ループ）

到達精度：

• REACH_CUP（カップへ）: dist = 0.002m（2mm）

• REACH_PLACE（設置点へ）: dist = 0.008m（8mm）

教訓

FixedJointのconnectedBodyにはRigidbodyが必要。

EndEffectorにRigidbodyがない場合、connectedBodyがnullになりFixedJointが機能しない。FixedJointのログは出るのにカップが動かない、という症状になる。これは明日（LOG92）解決する課題として持ち越した。

現時点の動作は「アームが左右のターゲットを往復する」状態。把持の完全実装はLOG92へ。

憲法は守られた

今日もStiffness（20,000）とDamping（2,000）はコードから一切触っていない。xDrive.targetとforceLimitの操作のみ。LOG90のStiffness発散インシデントの教訓が生きている。