Springhead Users Manual

7.6 関節系の逆運動学

逆運動学(IK)は，剛体関節系において剛体が目標位置に到達するよう関節を制御する機能です．

Springheadでは，関節系のヤコビアンを用いたIK機能が使用可能です． 物理シミュレーションの1ステップごとに関節系のヤコビアンを計算し，それに基づいて剛体を目標位置・姿勢に近づけるような各関節の角速度を計算します． シミュレーションを続けることで，最 終的に剛体が目標位置・姿勢となった状態が得られます．

Springhead上の剛体関節系に対してIKを使用するには，少々下準備が必要です． 次のように3つの剛体が直線状につながった関節系を例にとって解説します．

IKを使用するには，まずIKに用いるための関節を「アクチュエータ」として登録する必要があります． 

// given PHSceneIf* phScene
// given PHSolidIf* solid1, solid2, solid3
// given PHHingeJointIf* joint1 (solid1 <-> solid2)
// given PHHingeJointIf* joint2 (solid2 <-> solid3)


PHIKHingeActuatorDesc descIKActuator;


PHIKHingeActuatorIf* ikActuator1
  = phScene->CreateIKActuator(descIKActuator);
ikActuator1.AddChildObject(joint1);


PHIKHingeActuatorIf* ikActuator2
  = phScene->CreateIKActuator(descIKActuator);
ikActuator1.AddChildObject(joint2);

PHIKHingeActuatorIfはPHHingeJointIfに対応するアクチュエータクラスです．

次に，関節系の親子関係を登録します．親アクチュエータに，子アクチュエータを登録します． 

ikActuator1.AddChildObject(ikActuator2);

また，IKを用いて到達させる先端の剛体を「エンドエフェクタ」として登録する必要があります． 

PHIKEndEffectorDesc descEndEffector;


PHIKEndEffectorIf* ikEndEffector1
  = phScene->CreateIKEndEffector(descEndEffector);
ikEndEffector1.AddChildObject(solid3);

最後に，剛体関節系の親子関係において，エンドエフェクタの直接の親にあたるアクチュエータに対し，エンドエフェクタを登録します． 

ikActuator2.AddChildObject(ikEndEffector1);

この例では solid1 -(joint1)->solid2 -(joint2)->solid3 のように関節が接続されていますから，関節系の末端である solid3 をエンドエフェクタにした場合，直接の親にあたるアクチュエータは joint2 に対応するアクチュエー タ，すなわち ikActuator2 ということになります．

ここまでの作業で，生成されたオブジェクトの関係は以下のようになっているはずです．

これで下準備は終わりです．

目標位置をセットし，IKエンジンを有効にするとIKが動き始めます． 

// solid3 goes to (2, 5, 0)
ikEndEffector1->SetTargetPosition(Vec3d(2, 5, 0));


phScene->GetIKEngine()->Enable(true);


...
phScene->Step(); // IK is calculated in physics step
...
IKエンジン

IKの計算は，PHSceneが持つIKエンジン(PHIKEngine)によって実現されています．

IKエンジンはデフォルトでは無効となっています． 

phScene->GetIKEngine()->Enable(true);

を実行することで有効となります．GetIKEngine()は，PHSceneが持つIKエンジンを取得するAPIです．

SpringheadにおけるIKの計算原理は，関節系のヤコビ行列（ヤコビアン）に基づきます．全アクチュエータの関節角度に微小変化量 \( \varDelta \bm {\theta } \) を与えた時の，全エンドエフェクタの位置の微小変化量 \( \varDelta \bm {r} \) は，関節系のヤコビアン \( J \) を用いて

\[ \varDelta \bm {r} = J \varDelta \bm {\theta } \]

と表されます．毎ステップごとに関節系ヤコビアン\( J \)および目標位置に向かう微小変位\( \varDelta \bm {r} \)を計算し，上記の線形連立方程式を解くことで各関節に与える角速度を求めます．

線形連立方程式の求解にはガウス=ザイデル法による繰り返し解法を用いています．そのため1ステップあたりの繰り返し計算の回数によって計算速度と計算精度のトレードオフがあります．繰り返しの回数は， 

// 20 iteration per 1 physics step
phScene->GetIKEngine()->SetNumIter(20);

のようにして設定することができます．

PHSceneIfクラス

PHIKEngineIf* GetIKEngine()

   

IKエンジンを取得します．

PHIKEngineIfクラス

Enable(bool b)

   

IKエンジンの有効・無効を切り替えます．引数がtrueならば有効化し，falseならば無効化します．

SetNumIter(int n)

   

IKの繰り返し計算回数を1ステップあたりn回にセットします．

アクチュエータ

Springheadでは，IKに使用する各関節をアクチュエータと呼びます．IKは，アクチュエータを駆動させて剛体を目標位置に到達させます．

＜オブジェクト関係図＞

IKエンジンはアクチュエータを複数保持し，各アクチュエータが各関節を保持します．アクチュエータオブジェクト一つにつき，関節が一つ対応します． アクチュエータオブジェクトの具体的な役割は，関節の状態をIKエンジンに伝え，IKの計算のうち関節ヤコビアンの 計算など関節ごとに行う部分を実行し，IKの計算結果に従って関節を動かす事です．

アクチュエータクラスの種類と作成

本稿執筆時点では，IK用アクチュエータとして使用できるのはヒンジとボールジョイントのみです． それぞれに対応したアクチュエータクラスがあります．

// given PHSceneIf* phScene


PHIKHingeActuatorDesc descIKActuator;
PHIKHingeActuatorIf* ikActuator
     = phScene->CreateIKActuator(descActuator);

アクチュエータを作成するには，PHSceneIfのCreateIKActuator関数を用います．引数はアクチュエータのディスクリプタです．PHIKHingeActuatorDesc型のディスクリプタを渡すとヒンジ用のアクチュエータが作成され， PHIKBallActuatorDesc型のディスクリプタを渡すとボールジョイント用のアクチュエータが作成されます．

作成された時点では，アクチュエータは関節と対応付けがされていません．アクチュエータの子要素に関節を登録することで対応付けが行われます． 

// given PHHingeJointIf* joint
ikActuator->AddChildObject(joint);
アクチュエータの親子関係の登録

次のように二股に分岐したリンクを例にとります：

計算上，IKで駆動する関節系は木構造でなければなりません． Springheadでは，アクチュエータの親子関係を作ることで関節の木構造を設定します．

// given PHIKActuator ikActuator1, ikActuator2
ikActuator1->AddChildObject(ikActuator2);

AddChildObjectを呼び出すと，アクチュエータに対し「子要素」となるアクチュエータを登録することができます． これを全てのアクチュエータに対して行うことでアクチュエータの木構造が設定されます．このときアクチュエータの親子関係は，前出の図 の右側のようになります．

関節のダンパ係数とIK

関節の運動は，IK機能によって計算された目標関節角速度を関節にSetTargetVelocityすることで実現します． 目標速度に関する関節の振る舞いは，関節のdamperパラメータによって変化します．一般にdamperが大きいほど関節 は固くなり，外乱の影響を受けづらくなります．この性質はそのままIKの振る舞いにも受け継がれます．

重み付きIK

通常，IKは全ての関節を可能な限り均等に使用して目標を達成するよう計算されます． 一方，キャラクタの動作に用いる場合などで，手先を優先的に動かし胴体はあまり動かさない，といった重み付けが要求される場面があります．

SpringheadのIKには，このような重み付けを設定することができます． 

// given PHIKActuator ikActuator1, ikActuator2
ikActuator1->SetBias(2.0);
ikActuator2->SetBias(1.0);

SetBiasは，指定した関節をあまり動かさないように設定する関数です．Biasには\( 1.0 \)以上の値を設定します．大きな値を設定した関節ほど，IKによる動作は小さくなります．デフォルトではどのアクチュエー タも\( 1.0 \)となっており，全関節が均等に使用されます．

エンドエフェクタ

剛体・関節系を構成する剛体の一部を，「エンドエフェクタ」に指定することができます． エンドエフェクタには目標位置・姿勢を指示することができます．IKエンジンは，エンドエフェクタ剛体が指定された目標位置・姿勢を達成するようアクチュエータを制御します．

エンドエフェクタの作成

エンドエフェクタはPHSceneIfのCreateIKEndEffectorを用いて作成します．引数にはPHIKEndEffectorDescを渡します． 

// given PHSceneIf* phScene


PHIKEndEffectorDesc descEndEffector;


PHIKEndEffectorIf* ikEndEffector
  = phScene->CreateIKEndEffector(descEndEffector);

アクチュエータ同様，エンドエフェクタも作成時点では剛体との対応を持ちません．AddChildObjectにより剛体を子要素として登録する必要があります． 

// given PHSolidIf* solid


ikEndEffector.AddChildObject(solid);

次に，エンドエフェクタ剛体を親剛体に連結しているアクチュエータに対し，エンドエフェクタを子要素として登録します． 

// given PHIKActuatorIf* ikActuatorParent


ikActuatorParent.AddChildObject(ikEndEffector);

こうすることでエンドエフェクタはアクチュエータ木構造の葉ノードとなり，IKの計算に使用できるようになります．

なお，エンドエフェクタは一つの関節系に対して複数作成することができます．この場合，IKは複数のエンドエフェクタが可能な限り同時に目標位置・姿勢を達成できるようアクチュエータを制御します．また，エンドエフェクタは関節系の先端剛体に限りません．

目標位置の設定

エンドエフェクタの目標位置はSetTargetPositionによって指定します． 

// solid3 goes to (2, 5, 0)
ikEndEffector->SetTargetPosition(Vec3d(2, 5, 0));

エンドエフェクタに目標姿勢を指示し，エンドエフェクタが特定の姿勢をとるように関節系を動作させることもできます． 

ikEndEffector->SetTargetOrientation( Quaterniond::Rot(’x’, rad(30)) );
ikEndEffector->EnableOrientationControl(true);

目標姿勢はQuaterniondで設定します．姿勢制御はデフォルトでは無効になっており，使用するにはEnableOrientationControlを呼んで有効化する必要があります．

EnablePositionControlおよびEnableOrientationControlを用いると，位置制御・姿勢制御の両方を個別に有効・無効化することができます． 

// 位置制御あり，姿勢制御なし（デフォルト）
ikEndEffector->EnablePositionControl(true);
ikEndEffector->EnableOrientationControl(false);

// 位置制御なし，姿勢制御あり
ikEndEffector->EnablePositionControl(false);
ikEndEffector->EnableOrientationControl(true);

// 位置制御あり，姿勢制御あり
ikEndEffector->EnablePositionControl(true);
ikEndEffector->EnableOrientationControl(true);