6軸CCD IKでアームが動いた
目標は「ロボットアームがカップの場所と置き場所の間を自律的に往復する」こと。 角度ベースの制御では解決できなかった問題を、IK（逆運動学）で突破した。
※注釈：デジタルツイン学習（R2R2R）の検証プロセス 本プロジェクトは、AIの判断を物理的動作へ変換する設計を検証するものです。現実空間をデジタルツイン化し、仮想空間での大量試行（Real-to-Render）から得た最適解を、実世界のモーター駆動（to-Real）へ還元する「R2R2Rパイプライン」の実装過程を記録・考察します。
朝の状態
前日までの実装は「Link1とLink2に手動で目標角度を設定する」方式だった。 Inspector上で角度を入力すれば動く。しかし問題があった。
PlaceTargetの座標とアームの角度が対応していない
角度を変えるたびにアームが地面に突っ伏す
どの角度が正解か、試行錯誤では終わらない
角度ベースのアプローチには構造的な限界があった。 「角度を指定する」のではなく「座標を指定してアームが勝手に動く」仕組みが必要だった。
IKへの方針転換
IK（Inverse Kinematics）とは、「ここに手を届かせたい」という座標を与えると、各関節の角度を自動計算する手法。
今回採用したのは CCD IK（Cyclic Coordinate Descent） 。 6軸全部を末端から根本に向かって順番に計算し、少しずつ目標座標に近づけていく。
シンプルで安定している。Unityとも相性がいい。
実装の核心
ArticulationBodyの回転軸は anchorRotation に記録されている。 ここから真の回転軸を取得し、平面に射影してからSignedAngleを計算する。
// ジョイントの真の回転軸をanchorRotationから取得
Vector3 jointAxis = targetJoints[i].anchorRotation * Vector3.right;
jointAxis = jointTransform.TransformDirection(jointAxis);
// jointAxisに垂直な平面に射影してから角度を計算
toEffector = Vector3.ProjectOnPlane(toEffector, jointAxis).normalized;
toTarget = Vector3.ProjectOnPlane(toTarget, jointAxis).normalized;
float angle = Vector3.SignedAngle(toEffector, toTarget, jointAxis);
drive.target += angle * 0.05f;
drive.target = Mathf.Clamp(drive.target, -180f, 180f);
射影をしないと、回転軸と無関係な方向への誤差が混入する。 0.05f の係数で1フレームあたりの移動量を制限し、発散を防ぐ。
インシデント：Stiffnessが1e+25に発散
CCD_ITERATIONSを1から3に増やした直後、全関節のStiffnessが 1e+25 に吹き飛んだ。
原因はコード内で drive.stiffness = 20000f を毎フレーム上書きしていたこと。 Drive TypeがTargetのときはStiffnessの書き込みが内部計算と干渉し、値が発散した。
修正方針：Stiffnessの上書きを完全に削除する。
ArticulationBodyのStiffnessはInspectorで設定した値をそのまま使う。 コードから触らない。これが正解だった。
復旧手順
Drive TypeをForceに変更 → Stiffnessが20000にリセットされる → Drive TypeをTargetに戻す。 link1〜link6の6個全部に対して実施。VCS Check-in。
結果
[IK] state=REACH_PLACE dist=0.005PlaceTargetへの到達距離 0.005m（5mm） を確認。 REACH_CUP → REACH_PLACE → REACH_CUP のループ動作も確認。
アームはくねくね動く。きれいではない。 でも「座標を与えたら自分で動く」は成立した。
今日の教訓
Stiffness/Dampingはコードから絶対に触らない。
Inspector設定値が真実。コードはdrive.targetとforceLimitだけ操作する。 これがR2R2Rプロジェクトの憲法であり、今日改めてその理由を身をもって理解した。
最終コード（RobotArmController.cs）
using UnityEngine;
public class RobotArmController : MonoBehaviour
{
// ── フィールド ────────────────────────────────────────
[Header("Joints")]
public ArticulationBody[] targetJoints; // Element0=link1 〜 Element5=link6
[Header("PickAndPlace")]
public Transform handlePoint; // カップの把持点
public GameObject placeTarget; // 置き場所
[Header("EndEffector")]
public Transform endEffector; // link6 配下の EndEffector
public float graspThreshold = 0.05f; // 到達判定距離 [m]
public float moveSpeed = 20f; // 未使用（CCD IK で直接制御）
// ── 内部状態 ──────────────────────────────────────────
private enum State { REACH_CUP, REACH_PLACE }
private State currentState = State.REACH_CUP;
private bool isCooldown = false;
private const int CCD_ITERATIONS = 1; // 1フレームあたりの反復回数
private Vector3 cachedHandlePosition;
private Vector3 cachedPlacePosition;
private bool positionsCached = false;
// ── Unity ライフサイクル ──────────────────────────────
void Start()
{
if (targetJoints == null) return;
// 全 joint の xDrive を初期化（damping は触らない）
foreach (var joint in targetJoints)
{
if (joint == null) continue;
var drive = joint.xDrive;
drive.driveType = ArticulationDriveType.Target;
drive.forceLimit = 1000000f;
// target は現在値を維持
joint.xDrive = drive;
}
Debug.Log($"[START] endEffector={(endEffector == null ? "NULL" : endEffector.name)}");
Debug.Log($"[START] handlePoint={(handlePoint == null ? "NULL" : handlePoint.name)}");
Debug.Log($"[START] placeTarget={(placeTarget == null ? "NULL" : placeTarget.name)}");
cachedHandlePosition = handlePoint.position;
cachedPlacePosition = placeTarget.transform.position;
positionsCached = true;
}
void LateUpdate()
{
if (targetJoints == null || targetJoints.Length < 6) return;
if (endEffector == null) return;
// ── ターゲット座標を決定 ──────────────────────────
Vector3 targetPosition = GetTargetPosition();
// ── CCD IK（1回反復） ─────────────────────────────
for (int iter = 0; iter < CCD_ITERATIONS; iter++)
{
// link6(index5) → link1(index0) の順に処理
for (int i = targetJoints.Length - 1; i >= 0; i--)
{
if (targetJoints[i] == null) continue;
Transform jointTransform = targetJoints[i].transform;
// ジョイントの真の回転軸をanchorRotationから取得
Vector3 jointAxis = targetJoints[i].anchorRotation * Vector3.right;
jointAxis = jointTransform.TransformDirection(jointAxis);
// ローカル座標系でベクトルを計算
Vector3 toEffector = endEffector.position - jointTransform.position;
Vector3 toTarget = targetPosition - jointTransform.position;
// jointAxisに垂直な平面に射影
toEffector = Vector3.ProjectOnPlane(toEffector, jointAxis).normalized;
toTarget = Vector3.ProjectOnPlane(toTarget, jointAxis).normalized;
float angle = Vector3.SignedAngle(toEffector, toTarget, jointAxis);
// Debug可視化
Debug.DrawRay(jointTransform.position, jointAxis * 0.1f, Color.red);
Debug.DrawRay(jointTransform.position, toEffector * 0.1f, Color.green);
Debug.DrawRay(jointTransform.position, toTarget * 0.1f, Color.blue);
var drive = targetJoints[i].xDrive;
drive.forceLimit = 1000000f;
drive.target += angle * 0.05f;
drive.target = Mathf.Clamp(drive.target, -180f, 180f);
targetJoints[i].xDrive = drive;
targetJoints[i].SetDriveTarget(ArticulationDriveAxis.X, drive.target);
}
}
// ── ステート遷移判定 ──────────────────────────────
float dist = Vector3.Distance(endEffector.position, targetPosition);
Debug.Log($"[IK] state={currentState} dist={dist:F3}");
if (dist <= graspThreshold && !isCooldown)
{
isCooldown = true;
Invoke(nameof(ResetCooldown), 1.5f);
switch (currentState)
{
case State.REACH_CUP:
currentState = State.REACH_PLACE;
Debug.Log("[RobotArm] → REACH_PLACE");
break;
case State.REACH_PLACE:
currentState = State.REACH_CUP;
Debug.Log("[RobotArm] → REACH_CUP");
break;
}
}
}
private void ResetCooldown() { isCooldown = false; }
// ── 現在ステートに応じたターゲット座標を返す ─────────
private Vector3 GetTargetPosition()
{
switch (currentState)
{
case State.REACH_CUP:
return cachedHandlePosition;
case State.REACH_PLACE:
return cachedPlacePosition;
default:
return transform.position;
}
}
}
