腕の逆運動学（IK）

本ガイドでは、Pinocchio の減衰最小二乗法（Damped Least Squares）を用いて、手首の目標姿勢を Elf3 の片腕 7 関節の関節角に変換します。左腕・右腕の両方のリソースを用意しています。本サンプルはオフラインでの逆運動学計算のみを行い、ROS の起動やモータへの指令は行いません。

1. サンプルリソースの入手

左右両腕のスクリプトと URDF を同じディレクトリに配置します（導入ユーザー様に HumansX より提供します）。

• 左腕：pinocchio_ik_solve.py と elf3_arm_l.urdf
• 右腕：pinocchio_ik_solve_r.py と elf3_arm_r.urdf

ディレクトリ構成は次のとおりです。

elf3_arm_ik/
├── elf3_arm_l.urdf
├── elf3_arm_r.urdf
├── pinocchio_ik_solve.py
└── pinocchio_ik_solve_r.py

両 URDF には表示用・干渉判定用ジオメトリの STL パスが含まれています。本サンプルは pin.buildModelFromUrdf() を呼び出して運動学モデルのみを構築するため、最小構成の IK ソルバーでは STL ファイルは不要です。

2. Pinocchio のインストール

Pinocchio が依存する NumPy が ROS 環境に影響しないよう、開発用 PC 上で独立した Python 環境を使用してください。

cd elf3_arm_ik
python3 -m venv .venv
source .venv/bin/activate
python -m pip install --upgrade pip
python -m pip install pin scipy

インストールを確認します。

python -c "import pinocchio as pin; print(pin.__version__)"

本ガイドは pin 4.1.0、numpy 2.2.6、scipy 1.15.3 で動作確認しています。互換性のある他のバージョンでも動作する可能性があります。

3. ソルバーの完全なコード

左腕の完全なコード

以下のコードは提供される pinocchio_ik_solve.py と同一です（コード内コメントは翻訳しています）。

import numpy as np
import pinocchio as pin
from scipy.spatial.transform import Rotation as R


if __name__ == "__main__":
    # URDF からロボットモデルを作成
    model = pin.buildModelFromUrdf("elf3_arm_l.urdf")
    data = model.createData()

    # 末端座標系を選択
    frame_id = model.getFrameId("l_wrist_z_link")

    # 目標位置と目標姿勢を設定
    target = pin.SE3(
        R.from_euler("xyz", [10, 0, 0], degrees=True).as_matrix(),
        np.array([0.1, 0, 0]),
    )

    # 初期関節角
    q = np.zeros(model.nq)

    max_iterations = 1000
    error_tolerance = 1e-5
    step_size = 0.1
    damping = 1e-6

    for i in range(max_iterations):
        # 順運動学：現在の末端姿勢を計算
        pin.forwardKinematics(model, data, q)
        pin.updateFramePlacements(model, data)
        current = data.oMf[frame_id]

        # SE(3) 姿勢誤差：現在の末端座標系から目標座標系への変換
        current_to_target = current.actInv(target)
        error = pin.log6(current_to_target).vector

        # 収束判定
        if np.linalg.norm(error) < error_tolerance:
            break

        # 末端のローカル座標系におけるヤコビ行列を計算
        jacobian = pin.computeFrameJacobian(
            model,
            data,
            q,
            frame_id,
            pin.ReferenceFrame.LOCAL,
        )

        # 末端ヤコビ行列を SE(3) 姿勢誤差のヤコビ行列に変換
        jacobian = -pin.Jlog6(current_to_target.inverse()) @ jacobian

        # 減衰最小二乗法で関節角の増分を計算
        velocity = -jacobian.T @ np.linalg.solve(
            jacobian @ jacobian.T + damping * np.eye(6),
            error,
        )

        # 関節角を更新し、URDF の関節範囲内に制限
        q = pin.integrate(model, q, velocity * step_size)
        q = np.clip(q, model.lowerPositionLimit, model.upperPositionLimit)

    print(q)

右腕にも同じソルバーを使用する

ソルバーの処理フローは左右で同一のため、完全なコードは 1 つだけ掲載しています。右腕には提供される pinocchio_ik_solve_r.py を使用するか、上記の左腕コードをコピーして次の 2 つの文字列のみを変更してください。

• elf3_arm_l.urdf を elf3_arm_r.urdf に変更
• l_wrist_z_link を r_wrist_z_link に変更

両スクリプトの処理手順は同じです。

1. 順運動学で現在の末端座標系の姿勢を計算します。
2. pin.log6() で 6 次元の SE(3) 姿勢誤差を求めます。
3. ローカル座標系でのフレームヤコビ行列を計算します。
4. 減衰最小二乗法で関節速度を計算します。

   v = -Jᵀ (J Jᵀ + λI)⁻¹ e

5. pin.integrate() で関節構成を更新し、URDF のリミット内にクリップします。

末端座標系の原点と向き

左腕の末端座標系は l_wrist_z_link、右腕は r_wrist_z_link です。本ガイドで提供する簡易 URDF では、次のようになっています。

• 末端座標系の原点は、手首の X・Y・Z 回転軸の共通交点にあり、手のひらの中心や指先ではありません。wrist_y_joint と wrist_z_joint はいずれも <origin xyz="0 0 0" rpy="0 0 0"> のため、手首の 3 関節は同一の原点を共有します。
• base_link は簡易片腕モデルのルートです。原点はその腕の第 1 肩軸の中心にあり、胴体や地面に置かれたロボット全体のベース座標系ではありません。ロボット全体のモデルで使用する場合は、姿勢を座標変換してから使用してください。
• 7 関節すべてが 0 のとき、末端座標系の原点はこの base_link において (0.256, 0, -0.256) m です。肩から肘まで -Z 方向に 0.256 m、続いて肘から手首まで +X 方向に 0.256 m 進みます。左右の簡易 URDF のゼロ位置ジオメトリは同一です。
• すべての固定関節の rpy は 0 です。そのためゼロ姿勢では、末端座標系の +X/+Y/+Z 軸は base_link の対応する軸と完全に一致します。Elf3 の ROS 本体座標系の規約では、+X が前方、+Y がロボットの左方向、+Z が上方向です。関節が動くと、末端座標系の軸は手首とともに回転します。

target.translation は、簡易 base_link で表した末端座標系原点の目標位置（単位：メートル）です。target.rotation は、base_link に対する末端座標系の軸の目標姿勢です。サンプルの R.from_euler("xyz", [10, 0, 0], degrees=True) は、末端座標系を X 軸まわりに 10 度回転させることを要求するもので、単に手首の 1 関節に 10 度を加えるものではありません。

4. 実行と結果の解釈

両スクリプトとも相対パスで URDF を開くため、リソースを置いたディレクトリで実行してください。

cd elf3_arm_ik
source .venv/bin/activate

# 左腕
python pinocchio_ik_solve.py

# 右腕
python pinocchio_ik_solve_r.py

両スクリプトは同じ目標姿勢を使用しており、現時点では次のように出力されます。

[-0.18420714  0.14130363 -0.02609032 -0.95993     0.14621128  1.309
 -0.14385886]

左腕の配列順序は elf3_arm_l.urdf で定義されています。

右腕の出力も同じ 7 つの数値で、elf3_arm_r.urdf により次の関節に対応付けられます。

r_shoulder_y_joint, r_shoulder_x_joint, r_shoulder_z_joint,
r_elbow_y_joint, r_wrist_x_joint, r_wrist_y_joint, r_wrist_z_joint

実運用のコードでは、ループ後に誤差を再計算し、許容誤差未満の場合にのみ結果を採用してください。収束しない場合は、目標姿勢、初期関節構成、ステップサイズ、または減衰係数を調整してください。

5. 目標姿勢の変更

目標位置は次のように変更します。

target_position = np.array([x, y, z])  # metres, in base_link

目標姿勢は次のように変更します。

target_rotation = R.from_euler(
    "xyz",
    [roll, pitch, yaw],
    degrees=True,
).as_matrix()

両者を組み合わせて Pinocchio の姿勢を作成します。

target = pin.SE3(target_rotation, target_position)

7 自由度の腕は 6 次元の末端姿勢に対して冗長であるため、同じ目標に複数の解が存在し得ます。初期値 q、減衰係数、ステップサイズが結果に影響します。連続した軌道では、関節の飛びを避けるため、前回の解を次回の初期関節構成として使用してください。

6. 結果を関節オーバーライドとして使用する

IK の結果は関節目標値にすぎません。シミュレーションで検証するには、actuators_cmds_override（31 自由度制御 API ガイドの「ノーコードでの頭部関節オーバーライド」を参照）を使用して、指定した左腕関節をオーバーライドします。配信レートはオーバーライドのタイムアウトを満たす必要があります。現在のデフォルトは 0.2 s です。

ros2 topic pub -r 20 \
  /simulation/actuators_cmds_override \
  communication/msg/ActuatorCmds \
  '{
    actuators_name: [
      "l_shoulder_y_joint", "l_shoulder_x_joint",
      "l_shoulder_z_joint", "l_elbow_y_joint",
      "l_wrist_x_joint", "l_wrist_y_joint", "l_wrist_z_joint"
    ],
    pos: [
      -0.18420714, 0.14130363, -0.02609032, -0.95993,
      0.14621128, 1.309, -0.14385886
    ],
    kp: [54.224, 54.224, 16.747, 54.224, 16.747, 16.747, 16.747],
    kd: [3.452, 3.452, 1.066, 3.452, 1.066, 1.066, 1.066]
  }'

Ctrl+C を押して連続配信を停止すると、オーバーライドはタイムアウトし、現在の状態へ滑らかに戻ります。次のコマンドで明示的に解除することもできます。

ros2 topic pub --once \
  /simulation/actuators_cmds_override \
  communication/msg/ActuatorCmds '{}'

実機では /hardware/actuators_cmds_override を使用します。右腕を制御する場合は、コマンド内の 7 つの l_ 関節名を対応する r_ 関節名に置き換えてください。