Springhead Users Manual

7.5 関節

拘束とは剛体と剛体の間に作用してその相対的運動に制約を加える要素です． 拘束のクラス階層をFig. 7.1に示します． まず拘束は関節と接触に分かれます．関節はユーザが作成しますが，接触は衝突判定結 果にもとづいて自動的に生成・削除されます． 関節はさらにいくつかの種類に分けられます．

細かな説明は後回しにして，まずは関節の作成方法から見ていきます．

関節の作成

以下ではもっとも使用頻度の高いヒンジの作成を例にとって関節の作成方法を説明します． ヒンジを作成するには次のようにします．

PHSolidIf* solid0 = phScene->GetSolids()[0];
PHSolidIf* solid1 = phScene->GetSolids()[1];


PHHingeJointDesc desc;
desc.poseSocket.Pos() = Vec3d( 1.0, 0.0, 0.0);
desc.posePlug.Pos()        = Vec3d(-1.0, 0.0, 0.0);
PHHingeJointIf* joint
     = phScene->CreateJoint(solid0, solid1, desc)->Cast();

作成したい関節の種類に応じたディスクリプタを作成し，これをPHSceneのCreateJoint関数に渡して関節を作成します． このとき，ディスクリプタとともに連結したい剛体のインタフェースも渡します． CreateJointはPHJointIf*を返しますので，作成した関節のインタフェースを得るにはCastで動的キャストします．

関節に関するPHSceneの関数を以下に示します．

NJointはシーン中の関節の個数を返します．GetJointはi番目の関節を取得します．

ソケットとプラグ

(a) before connection

(b) after connection

さて，上の例でディスクリプタに値を設定している箇所に注目してください．この部分で関節の取り付け位置を指定しています． Springheadでは，ソケットとプラグと呼ばれるローカル座標系を用いて関節の取り付け位置を表現します． ソケットとプラグとは，その名前 から連想するように，連結する剛体に取り付ける金具のようなものです． CreateJointの第引数の剛体にソケットがつき，第引数の剛体にプラグがつきます． ソケットとプラグがそれぞれの剛体のどの位置に取り付けられるかを指定するのがディスク リプタのposeSocketとposePlugです． 上の例ではソケットの位置が，プラグの位置がでした(Fig. 7.2(a))． この場合はFig. 7.2(b)のように剛体が連結されます． 後述するように，ヒンジはソケットとプラグのz軸を一致させる拘束です． したがって連結された剛体同士はソケットとプラグのz軸を回転軸として相対的に回 転することができます．

ソケットとプラグに関するディスクリプタとインタフェースを紹介します．

GetRelativePoseはソケット座標系から見たプラグ座標系の相対的な位置と向きを取得します． 同様に，GetRelativeVelocityはソケットからみたプラグの相対速度をソケット座標系で取得します． ここで第引数が並進速度，第引数が角速度です． GetConstraintForceはこの拘束が剛体に加えた拘束力を取得します(第引数が並進力，第引数がモーメント)． 具体的には，ソケット側剛体に作用した拘束力をソケット座標系で表現したものが得られます． プラグ 側剛体には作用反作用の法則によって逆向きの力が作用しますが，これを直接取得する関数は用意されていません．

関節の種類

Springheadで使用可能な関節の種類は

• • ヒンジ (PHHingeIf)

• • スライダ (PHSliderIf)

• • パスジョイント (PHPathJointIf)

• • ボールジョイント (PHBallJointIf)

• • バネ (PHSpringIf)

の5種類です．種類ごとに，自由度・拘束の仕方・変位の求め方が異なります．

ヒンジ

ヒンジは軸回転関節です． ヒンジは，Fig 7.3に示すようにソケットと プラグのz軸が一致するように拘束します． このときソケットのy軸とプラグのy軸の成す角(x軸同士でも同じことですが)が関節変位となります．

関節変位を取得するAPIは自由度関節(PH1DJointIf)で共通です．そのためヒンジに限らずスライダ・パスジョイントでも使用できます．

PH1DJointIfクラス

double GetPosition()

   

関節の変位を取得します．変位のはかり方は関節の種類に依存します．

スライダ

スライダは自由度の直動関節です． スライダは，Fig 7.4に示すようにソ ケットとプラグのz軸が同一直線上に乗り，かつ両者のx軸，y軸が同じ向きを向くように拘束します． このときソケットの原点からプラグの原点までが関節変位となります．

パスジョイント

パスジョイントはソケットとプラグの相対位置関係をパラメータの自由曲線で表現する関節です．詳しくは後述します．

T.B.D.

ボールジョイント

(a)	(b)

ボールジョイントは自由度の回転関節です． ボールジョイントはFig 7.5(a)に示す ようにソケットとプラグの原点が一致するように拘束します． ソケット座標系をプラグ座標系に変換するようなクォータニオンが変位となります．

一方で，ボールジョイントの変位はオイラー角の一種であるSwing-Twist座標系(Fig 7.5(b))で取得することもできます． ソケットとプラグのz軸同士がなす角をスイング角(Swing)，プラグのz軸をソケット のx-y平面への射影がソケットのx軸となす角をスイング方位角(Swing-Dir)，プラグのz軸周りの回転角度をツイスト角(Twist)と呼びます．Swing-Twist座標系は，後述するボールジョイントの関節可動範囲の指定に用います．

この2種類の変位は，それぞれに対応した関数で取得することができます． PHBallJointクラス

Quaterniond GetPosition()

   

ソケット座標系をプラグ座標系に変換するようなクォータニオンを返します．

Vec3d GetAngle()

   

Swing-Twist座標系で表現された関節変位を返します．

バネ

剛体間を連結するダンパ付きバネです．ソケット座標系とプラグ座標系が一致するときが自然状態で，位置の変位・姿勢の変位に比例して自然状態に戻すような力・モーメントを発生します．並進運動に作用するバネ・ダンパ係数と，回転運動に作用するバネ・ダンパ係数 はディスクリプタによってそれぞれ設定できます．

有効化と無効化

有効な拘束は拘束力を生じます．無効化された拘束は存在しないのと同じ状態になりますが， 削除するのと異なりいつでも再度有効化することができます． 作成直後の拘束は有効化されています．

関節制御

自由度関節の場合

関節を駆動する力は次式で与えられます．

ここで，はそれぞれ関節変位と関節速度でGetPosition，GetVelocityで取得できます． その他の記号とデ ィスクリプタ変数との対応は以下の通りです．

	spring
	damper
	targetPosition
	targetVelocity
	offsetForce

上の式はバネ・ダンパモデルとPD制御則の二通りの解釈ができます． 前者としてとらえるならはバネ係数，はダンパ係数，はバネの自然長，は基準速度となります． 後者としてとらえる場合ははPゲイン，はDゲイン，は目標変位，は目標速度となります． また，は関節トルクのオフセット項です． 上の式で得られた関節トルクは最後にfMaxの範囲に収まるようにクランプされます．

ボールジョイントの場合

ヒンジと同様に，バネダンパモデル・PD制御を実現します． ボールジョイントの変位はクォータニオンで表されるため，目標変位targetPositionはクォータニオンで，目標速度targetVelocityは回転ベクトルで与えます．

可動域制限

CreateLimitは可動範囲制約オブジェクトのディスクリプタを引数にとります． 自由度関節の可動範囲制約の場合，Vec2d rangeが可動域を表します．range[0]が可動域の下限， range[1]が上限です．range[0] <range[1]が満たされているときに限り可動範囲制約が有効となります． デフォルトではrange[0] >range[1]となる値が設定されていて，可動範囲制約は無効となっています．

関節の変位が可動範囲限界に到達したとき，範囲を超過しないように可動範囲制約の拘束力が作用します． このとき，関節変位を範囲内に押し戻す力はバネ・ダンパモデルで計算されます． このバネ係数とダンパ係数はそれぞれディスクリプタのspring， damperで指定します．

可動範囲用のspring，damperは初期値でも十分大きな値が設定されていますが，関節制御において非常に大きなバネ・ダンパ係数を用いると可動範囲制約のバネ・ダンパが負けてしまうことがあります．その場合には関節制御より大きな係数を 適切に再設定すると，可動範囲内で関節を制御する事ができるようになります．

自由度関節の場合

PH1DJointLimitDescクラス

Vec2d range

   

可動範囲を表します．range[0]が下限，range[1]が上限です．

double spring

   

double damper

   

可動範囲を制限するためのバネ・ダンパモデルの係数です．

PH1DJointLimitIfクラス

IsOnLimit()

   

現在の関節姿勢が可動範囲外にある時にtrueを返します．この関数がtrueを返すような時，関節には可動域制約を実現するための拘束力が発生しています．

ボールジョイントの場合

ボールジョイントの可動範囲はFig 7.5(b)に示すSwing-Twist座標系によって指定します．

ボールジョイントに対しては2種類の可動範囲制約を使用することができます．

• • ConeLimitは円錐形の可動範囲制約で，主に関節のスイング角を一定範囲内に制約します．

• • SplineLimitは自由曲線形の可動範囲制約で，プラグ座標系z軸の可動範囲を閉曲線で指定することができます．

ここではConeLimitについて説明します(SplineLimitについては後述します)．

PHBallJointConeLimitDescクラス

Vec2d limitSwing

   

スイング角の可動範囲です．概念的には，関節が一定以上に折れ曲がらないようにする制約です(スイング角の下限を設定する事もできるので，実際には一定以上にまっすぐにならないようにする機能も有しています)．

limitSwing[0]が下限，limitSwing[1]が上限です．limitSwingを取得・設定するためのAPIは

PHBallJointConeLimitIf::[Set|Get]SwingRange(range)

です．

limitSwing[0] >limitSwing[1]となる時は無効化されます．デフォルトではlimitSwing[0] >limitSwing[1]となる値がセットされています．

Vec2d limitTwist

   

ツイスト角の可動範囲です．概念的には，関節が一定以上にねじれないようにするための制約です．

limitTwist[0]が下限，limitTwist[1]が上限です．limitTwistを取得・設定するためのAPIは

PHBallJointConeLimitIf::[Set|Get]TwistRange(range)

です．

limitTwist[0] >limitTwist[1]となる時は無効化されます．デフォルトではlimitTwist[0] >limitTwist[1]となる値がセットされています．

double spring

   

double damper

   

可動範囲を制限するためのバネ・ダンパモデルの係数です．自由度関節の場合と同じです．

PHBallJointConeLimitIfクラス

IsOnLimit()

   

現在の関節姿勢が可動範囲外にある時にtrueを返します．自由度関節の場合と同じです．

ボールジョイントの自由曲線可動域

パスジョイント

弾塑性変形バネダンパ