MuJoCoで逆運動学を使う｜IKで目標位置へ手先を届かせる動かし方

import mujoco
import numpy as np
 
XML = """
<mujoco>
  <option gravity="0 0 -9.81"/>
  <worldbody>
    <light pos="0 0 3"/>
    <geom type="plane" size="2 2 0.1" rgba="0.9 0.9 0.95 1"/>
    <body name="upper_arm" pos="0 0 1.0">
      <joint name="shoulder" type="hinge" axis="0 1 0"/>
      <geom type="capsule" fromto="0 0 0  0.35 0 0" size="0.03" mass="1.0"/>
      <body name="forearm" pos="0.35 0 0">
        <joint name="elbow" type="hinge" axis="0 1 0"/>
        <geom type="capsule" fromto="0 0 0  0.3 0 0" size="0.025" mass="0.6"/>
        <site name="hand" pos="0.3 0 0" size="0.02"/>
      </body>
    </body>
  </worldbody>
  <actuator>
    <motor name="shoulder_motor" joint="shoulder" ctrlrange="-20 20"/>
    <motor name="elbow_motor" joint="elbow" ctrlrange="-20 20"/>
  </actuator>
</mujoco>
"""
 
L1, L2 = 0.35, 0.3  # mujoco-robot-arm-control.mdxと同じ上腕・前腕の長さ
 
 
def inverse_kinematics(x, z, elbow_up=True):
    """手先座標(x, z)から関節角度(shoulder, elbow)を逆算する。届かない場合は腕を伸ばした姿勢に丸める"""
    y = -z  # MuJoCoのz軸は、2関節IKの教科書的なy軸と符号が逆（後述）
    cos_e = (x * x + y * y - L1 * L1 - L2 * L2) / (2 * L1 * L2)
    reachable = -1.0 <= cos_e <= 1.0
    cos_e = np.clip(cos_e, -1.0, 1.0)
    elbow = np.arccos(cos_e)
    if elbow_up:
        elbow = -elbow
    k1 = L1 + L2 * np.cos(elbow)
    k2 = L2 * np.sin(elbow)
    shoulder = np.arctan2(y, x) - np.arctan2(k2, k1)
    return shoulder, elbow, reachable

class PID:
    def __init__(self, kp, ki, kd, dt):
        self.kp, self.ki, self.kd, self.dt = kp, ki, kd, dt
        self.integral = 0.0
        self.prev_error = 0.0
 
    def update(self, target, current):
        error = target - current
        self.integral += error * self.dt
        derivative = (error - self.prev_error) / self.dt
        self.prev_error = error
        return self.kp * error + self.ki * self.integral + self.kd * derivative
 
 
def run_to_target(x, z, elbow_up, steps=5000):
    model = mujoco.MjModel.from_xml_string(XML)
    data = mujoco.MjData(model)
    hand_site = model.site("hand").id
    shoulder_origin = np.array([0, 0, 1.0])
 
    target_shoulder, target_elbow, reachable = inverse_kinematics(x, z, elbow_up)
    dt = model.opt.timestep
    shoulder_pid = PID(kp=60.0, ki=30.0, kd=8.0, dt=dt)
    elbow_pid = PID(kp=40.0, ki=20.0, kd=5.0, dt=dt)
 
    for _ in range(steps):
        data.ctrl[0] = shoulder_pid.update(target_shoulder, data.qpos[0])
        data.ctrl[1] = elbow_pid.update(target_elbow, data.qpos[1])
        mujoco.mj_step(model, data)
 
    hand = data.site_xpos[hand_site] - shoulder_origin
    hand_xz = (hand[0], hand[2])  # 平面運動なのでy成分(hand[1])は常に0
    err_mm = np.hypot(hand_xz[0] - x, hand_xz[1] - z) * 1000
    return reachable, hand_xz, err_mm
 
 
print("A（肘下げ）:", run_to_target(0.45, -0.25, elbow_up=False))
print("B（肘上げ）:", run_to_target(0.45, -0.25, elbow_up=True))

A（肘下げ）: (True, (0.450, -0.250), 0.3)
B（肘上げ）: (True, (0.450, -0.250), 0.4)

print("C（届かない目標）:", run_to_target(0.9, 0.0, elbow_up=False))

C（届かない目標）: (False, (0.650, -0.000), 250.0)