IKを用いたOpenArmの駆動について

[gunyarakun]

手先位置をFollowerの終端位置に反映するようにOpenArmを駆動したいです。
方法を教えてください。

[gunyarakun]

手先位置を指定してOpenArmを駆動するためには、手先の目標姿勢から関節角を求め（IK: Inverse Kinematics）、それをFollower側の各関節へ指令として送る、という流れになります。

IKにつきましては、以下の openarm_control パッケージの Kinematics クラスで提供しております。
https://github.com/enactic/openarm_control/blob/main/src/openarm_control/kinematics.py

使い方の流れは次のとおりです。

1. IKを有効にして初期化

Kinematics は IKParams を渡したときだけ IK が有効になります。

from openarm_control.kinematics import Kinematics, IKParams
kin = Kinematics(setup, IKParams())

2. 目標姿勢をセット

set_target(side, pose) に、対象の腕（"right" / "left"）と終端の目標姿勢を渡します。pose は float32[7] で [px, py, pz, qw, qx, qy, qz]（位置3 + クォータニオン4）です。クォータニオンの順序は wxyz、位置はベース座標系・単位mです。

kin.set_target("right", pose)  # pose: [px, py, pz, qw, qx, qy, qz]

3. 解いて関節角を得る

solve() を呼ぶと float32[16]（right[8] + left[8]、各腕が 7関節 + グリッパ1）が返ります。解が得られなかった場合は None です。

result = kin.solve()  # -> float32[16] or None

なお dt や反復回数は solve() の引数ではなく、初期化時の IKParams（dt, max_iters など）で指定します。

注意点

このIKは mink + MuJoCo による差分IK（QPベースの速度IK）です。解析的に一発で目標姿勢へ到達するのではなく、現在の関節配置から目標へ1ステップぶん近づける動作を返します。

そのため、

• 制御周期ごとにループで回す
• 各周期の頭で、現在のFollowerの関節状態を sync(values16) で内部configに反映してからset_target、そしてsolveする

という使い方になります。これを行わないと内部configと実機がずれ、挙動が不安定になりますのでご注意ください。

4. Follower（実機）へ送る

solve() の戻り値（各関節の目標値）を、そのまま OpenArm のFollowerドライバへ関節位置指令として送れば駆動します。

dora構成であれば、この一連の処理は dora-openarm-ik ノードとして提供しています（内部で上記 Kinematics を利用しています）。
https://github.com/enactic/dora-openarm-kinematics

target_right / target_left に目標姿勢を入力すると position_right / position_left（関節位置）が出力されますので、それを Follower ノードに接続すれば実機が駆動します。

dora-openarm-vr でも同じ構成（目標姿勢 → ik ノード → Follower）で動作しております。
https://github.com/enactic/dora-openarm-vr

config/dataflow-mujoco.yaml の ik ノードの position_* 出力を、MuJoCoビューアの代わりに follower-right / follower-left ノードへ差し替えると、そのまま実機駆動の構成になります。