逆運動学

タエチャスクナッチャノン 05241011

はじめに

Parametric Curveで WebGL の基本的な描画方法を学びましたが、今回は 3D での実装を行うため、ボイラープレートコードの用意を不要にするため、Three.js を使用します。

左側にオレンジ色の箱が表示されていると思います。これは以下のコードだけで表示できます。

import { Canvas } from "@react-three/fiber";
import { OrbitControls, Box } from "@react-three/drei";

export function InverseKinematics() {
  return (
    <div style={{ width: "100%", height: "100%" }}>
      <Canvas camera={{ position: [3, 3, 3] }}>
        <ambientLight intensity={0.5} />
        <directionalLight position={[10, 10, 5]} intensity={1} />

        <Box args={[1, 1, 1]} position={[0, 0.5, 0]}>
          <meshStandardMaterial color="orange" />
        </Box>

        <Grid args={[100, 100]} />

        <OrbitControls enablePan={true} enableZoom={true} enableRotate={true} />
      </Canvas>
    </div>
  );
}

非常に、簡単だったと思います。

目標

逆運動学、または Inverse InverseKinematics (IK) とは、特定の位置に複数の関節を持つロボットアームなどを配置するための技術であるため、今回はロボットアームを作成し、ボールを掴むようにします。

まずは、ロボットアームの基本的な構造を作成しましょう。

ロボットアームの作成

5つの関節を持つロボットアームを作成します。各関節は異なる色で表示され、長さはすべて 0.5 にする。各関節は、自分の子を tip に配置し、次々の関節を再帰的に形成します。

const colors = ["orange", "blue", "green", "red", "purple"];
const lengths = [0.5, 0.5, 0.5, 0.5, 0.5];

function RobotArm({ target }: { target?: Vector3 }) {
  const { scrollPassed } = useScrollDetectorState();

  const [rotations, setRotations] = useState(
    colors.map(() => new Quaternion()),
  );

  const jointRefs = useRef([
    ...colors.map(() => createRef<Mesh>()),
    createRef<Mesh>(),
  ]);

  return (
    <group>
      <RobotJoint
        colors={colors}
        lengths={lengths}
        rotations={rotations}
        refs={jointRefs.current}
      />
    </group>
  );
}

function RobotJoint({
  colors,
  lengths,
  rotations,
  refs,
}: {
  colors: string[];
  lengths: number[];
  rotations: Quaternion[];
  refs: React.RefObject<Mesh | null>[];
}) {
  const length = lengths[0];
  const color = colors[0];
  const rotation = rotations[0];
  const ref = refs[0];

  return (
    <group ref={ref} quaternion={rotation}>
      <Box args={[0.2, length, 0.2]} position={[0, length / 2, 0]}>
        <meshStandardMaterial color={color} />
      </Box>

      <group position={[0, length, 0]}>
        {lengths.length > 1 && (
          <RobotJoint
            colors={colors.slice(1)}
            lengths={lengths.slice(1)}
            rotations={rotations.slice(1)}
            refs={refs.slice(1)}
          />
        )}
        <mesh ref={refs.slice(1).length === 1 ? refs.slice(1)[0] : undefined}>
          <sphereGeometry args={[0.1, 32, 32]} />
          <meshStandardMaterial color={color} />
        </mesh>
      </group>
    </group>
  );
}

最後の ref は、最後ロボット関節のエンドエフェクター (tip) を表します。

ターゲット

次に、ターゲットを作成します。ターゲットは、ロボットアームが掴むべき位置を示すためのものです。

function Target() {
  return (
    <mesh position={[0, 0.5, 2]}>
      <sphereGeometry args={[0.2, 32, 32]} />
      <meshStandardMaterial color="yellow" />
    </mesh>
  );
}

ただし、DragControls を普通に使うと、OrbitControls と競合してしまうため、setDragging を使って、ドラッグ中は OrbitControls を無効にするようにします。

export function InverseKinematics() {
  const [dragging, setDragging] = useState(false);

  return (
    // ...
        <Target setDragging={setDragging} />
        <OrbitControls
          enabled={!dragging}
        />
  );
}

export function Target({
  setDragging,
}: {
  setDragging: (dragging: boolean) => void;
}) {
  return (
    <DragControls
      onDragStart={() => setDragging(true)}
      onDragEnd={() => setDragging(false)}
    >
      // ...
    </DragControls>
  );
}

逆運動学の実装

逆運動学のコアは、ターゲットに対して回転させ、複数回反復して、各関節の回転を更新するだけで非常に簡単ですが、

const updateRotations = useCallback(() => {
  if (!target) return;

  const endIndex = colors.length - 1;
  const end = jointRefs.current[jointRefs.current.length - 1].current;
  if (!end) return;

  const newRotations = rotations.map((r) => r.clone());

  for (let iter = 0; iter < MAX_ITERATIONS; iter++) {
    for (let i = endIndex; i >= 0; i--) {
      const endEffector = end.getWorldPosition(new Vector3());
      const joint = jointRefs.current[i].current;
      if (!joint) continue;
      const ev = joint.worldToLocal(endEffector.clone()).normalize();
      const tv = joint.worldToLocal(target.clone()).normalize();
      const q = new Quaternion().setFromUnitVectors(ev, tv);
      newRotations[i].multiply(q);
      joint.quaternion.copy(newRotations[i]);
    }
  }

  setRotations(newRotations);
}, [target, rotations]);

useEffect(() => {
  if (target) {
    updateRotations();
  }
}, [target]);

ただし、これではロボットアームの関節が無条件で回転してしまうため、関節の回転を制限する必要があります。まずは、ヒンジ関節を実装します。

const invq = joint.quaternion.clone().invert();
const axis = i === 0 ? new Vector3(0, 1, 0) : new Vector3(0, 0, 1);
const parent = axis.clone().applyQuaternion(invq);
q.setFromUnitVectors(axis, parent);
newRotations[i].multiply(q);
joint.quaternion.copy(newRotations[i]);

ベース関節だけは Y 軸を中心に回転し、それ以外の関節は Z 軸を中心に回転するようにします。次は、関節を -PI/2 から PI/2 の範囲に制限します。

if (i !== 0) {
  const euler = joint.rotation;
  const clamped = new Vector3()
    .setFromEuler(euler)
    .clamp(
      new Vector3(-Math.PI / 2, -Math.PI / 2, -Math.PI / 2),
      new Vector3(Math.PI / 2, Math.PI / 2, Math.PI / 2),
    );
  newRotations[i].setFromEuler(new Euler(clamped.x, clamped.y, clamped.z));
  joint.quaternion.copy(newRotations[i]);
}

これで、関節の回転が制限され、ロボットアームがターゲットに向かって正しく動くようになります。

Reference: https://rodolphe-vaillant.fr/entry/114/cyclic-coordonate-descent-inverse-kynematic-ccd-ik

以上は Shader Toy での実装だそうですが、pseudo code として参考にしました。