物理エンジンで作るBio-likeness(2)

ようやくBulletでCSL”らしき"モーター制御の実装が出来ました。
”らしき”と書いたのは、一部推測および独自解釈しているためです。

Bulletでのモーター制御

いくつか方法がありますが、今回はヒンジ関節btHingeConstraintを用い、この関節を角速度を指定して回転させます。
enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse)
　enableMotor　　　：モーターの有効/無効
　maxMotorImpluse：最大トルク
　targetVelocity　　：目標角速度

CSLの式を目標角速度を得る形に変形する

前回紹介した参考論文に記載されているCSLの式のアウトプットはモーター電圧です。
そのため、上記関数へ適用するにはこの式を目標角速度を求める式に変形する必要があります。
u(t) = -gi*φ(t) + [gi*φ(t) + gf*u(t)]Z-1
　φ：モーター角
　u：モーターへの出力電圧
　gi：入力ゲイン (> 0)
　gf：フィードバックゲイン(> 1)
　z-1：単位遅延演算子

式を変形する

　u(t) = -gi*φ(t) + gi*φ(t-1) + gf*u(t-1)
　u(t) = gi*Δφ(t) + gf*u(t-1)

ここで、使用するモーターを電圧に対しトルクは一定で回転速度は比例するものと仮定します。
測定結果としての角度変化量をΔφ1(t)、電圧u(t)により変化させる目標値としての角度変化量をΔφ0(t)とすると
　u(t) = K*Δφ0(t)
　　※Kは比例定数

　K*Δφ0(t) = -gi*Δφ1(t)+gf*K*Δφ0(t-1)
　　Δφ0(t) = -gi/K*Δφ1(t)+gf*Δφ0(t-1)

　これで、入力：測定角速度Δφ1(t)と前回の目標角速度Δφ0(t-1)から、
　出力：目標角速度Δφ0(t)を求める形に変形できました。

BulletでCSL制御を実装

100msec(6Step)ごとに計測を行い、測定した角度の変化量が（外力が働かなければ）目標角速度の1/10になると想定して実装しました。

変形CSL関数

結果

　回転軸からの距離は3、おもりの重さは1/3

動作はかなりゆっくりですが、重力に反して倒立することを確認出来ました。

これでやっとスタートライン。
今後、ロボットのパーツとなる剛体を作成し、組み立てながらパラメータを調整します。