• 球：ロボットモデル(動くもの)
  半径を指定して球primitiveを作成(ただし後に作り直し：後述)。
• ロボット：ロボットモデル
  ロボットボディと４本のドライブローラからなる。
  ロボットボディは基盤に見立てた板と、球を支える板(直方体primitive)で構成。
  ローラは円筒primitiveを使って実装(ただし後に作り直し：後述)。
  ローラをトルク操作して、接触力で、球に押しつけてドライブする。
• 床：環境モデル(動かないもの)
  これがないと、全部が自由落下で落ちていく。
  ただの適当な薄い直方体を作る。

• 板(floor.wrl、環境)
  ただ、3.0x3.0x0.02[m]の板を作っただけ。rootジョイントでは一切パラメータを設定せず(fixedくらい)、shapeの側で、translation (0,0,-0.01)設定(デフォで上下がＺ軸)。
  よって、ロボット(球)の移動面はZ=0の面。
  
• 球(ball.wrl、ロボット、 _icosa.wrlをInline)
  半径 0.11[m]の球。
  質量3.8[kg]、慣性モーメントは対角に0.018(ともに実機より)。
  rootジョイントのtranslationを(0,0,0.11)にすることで、球の中心位置を板の上方に半径分、に設定。primitiveのtranslationは(0,0,0)のまま。primitive側で(0,0,0.11)にすると、重心は(0,0,0)、つまり球の下端にあるような、へんなモデルができるので注意(一度、失敗して起き上がりこぼしができた)。

  当初は標準プリミティブの球を使っていたが、ポリゴンのメッシュが緯度経度型で、赤道付近と極付近で接触特性が大きく異なること(面の大きさおよび頂点の構造)、メッシュ切りの細かさを指定できないことから、自前でVRML形状モデル(_icosa.wrl)を作成した。
  自前で作った形状モデルは、ロボットモデルの該当箇所で"Inline"で記述され、読み込み時に一緒に取り込まれるようであるため、同一ディレクトリに置いておくのがよさそう。逆に、読み込み時取り込みなので、形状モデルのwrlファイルのみを差し替えることが可能(形状は動特性には影響せず。接触判定と画面表示にのみ影響する印象)。
  

技術的に細かい補足：

• 開発元によれば、形状モデルはVRMLの「coord { point [...]}」(頂点規定)と「coordIndex [...]」(-1区切りでポリゴンの頂点セットを規定)のみあれば良い、とのこと。
• coordIndexでは頂点３個の番号(pointは０から始まる配列)をセットにして記述するが、回転方向で表裏が変わる。つまり、３点をＡＢＣとすると、「ＡＢＣ」と「ＡＣＢ」で表裏が変わる。裏になると、表示がおかしくなるので見た目で分かる。
• さらに、「color { color [...]}」と「colorIndex [(ポリゴン数)]」を加えることで、各ポリゴンに独立した色を設定している。なお、「colorPerVertex FALSE」が必須。これがないと、色は頂点ごとに指定したことになり、数が合わないばかりか、OpenHRPのモデルローダが落ちる。
• つくった形状モデルが正しく読み込めるかは、OpenHRPで「空のプロジェクト」にModelで一つ作成して、自動でできるrootジョイントにVRMLからのshape追加をしてみればいい。

ロボットボディは

• いじった経緯からＹ軸が上を向くように９０度回転させた座標系を持ち、原点は球の中心と同じ。
  （translationが(0,0,0.11)、rotationが(1,0,0,1.5708)つまり、(1,0,0)軸周りにπ/2回転）
  
• shape0とshape1はボディの板と、球を支える板(実機ではボールキャスタが使われているが面倒なので、摩擦ほぼゼロの板)。支え板の下面が座標 Y=0.11になるように、つまり球の上部と接する位置。
• 制御系で姿勢角度を得るために、RateGyroセンサ(GyroSensor)と、Accelerationセンサ(AccSensor)を搭載。 ともに、搭載位置は原点上方0.2[m]の位置。
• 質量8.7[kg]、慣性モーメント 対角全て0.11[kgm2] (※水平軸周りに0.11は実機実測値だが、鉛直軸はデータ無し。おそらくより小さい)、重心高さ0.23[m] (centerOfMass (0.0 0.23 0.0)、Ｙ軸が上方)

• スライダは、ロボットのroot座標系に対して、右図のように、Ｚ軸(青軸)が球の法線方向を向くようにrotationとtranslationを設定。
  また、スライド軸(青短太)をＺ軸と一致させた(jointAxis, rotationとtranslation適用後の座標軸に対して＝(0,0,1))。
  11/04/16現在で配布されているGrxUIには直動軸の表示に関して不具合が有るとのこと。そのためモデルをGrxUIで読み込んでも、SX1,RX1以外はこの通りに表示されない。GrxUIのソースビルドをしたのは、この不具合の補正のため。今後、修正版が配布されるとのこと。
  

  名前	translation	rotation	jointAxis
  SX1	(0,0,0.13)	(0,0,1,0)	(0,0,1)
  SX2	(0,0,-0.13)	(0,1,0,π)	(0,0,1)
  SY1	(0.13,0,0)	(0,1,0,π/2)	(0,0,1)
  SY2	(-0.13,0,0)	(0,1,0,-π/2)	(0,0,1)

  
  translationの0.13は球半径0.11とローラ半径0.02の合計である。
  なお、スライダ部分にはシミュレーション時のダミーとして質量0.1[kg]と慣性モーメント1e-4[kgm2](対角)を与えているが、固有の形状モデルは設定していない。
• ローラーはそれぞれのスライダの座標系に対して、同一設定。translation, rotationとも既定値(0,0,0),(0,0,1,0)。 関節軸はＸ軸(赤軸)で(1,0,0)。
  質量0.1[kg]と慣性モーメント1e-4[kgm2](対角)をとりあえず設定。
  形状は、当初はprimitiveの円筒を用いたが、球と同様の理由により、VRMLモデルを自作した。
  _cylinder.wrlを追加(Inline)。 これもPerlスクリプト cylinder.plで生成した(半径0.02[m]、長さ0.10[m])。
  
• 関節番号は、ローラ RX1,RX2,RY1,RY2に０～３を、スライダ SX1,SX2,SY1,SY2に４～７を設定した。
  ※コントローラのことを考えると逆のほうが良かったかもしれない。

• floor-ball 設定：摩擦係数 0.5(デフォルト設定)
• floor-robot_body(root) 設定：摩擦係数 0.5(デフォルト設定)
  ※ロボットのモデルのボディ部分がroot
• ball-robot_body(R??) 設定：摩擦係数 0.5(デフォルト設定)
  ※ロボットのローラと球の摩擦設定
• ball-robot_body(root) 設定：摩擦係数 0.01
  ※ボディが球の上に位置するようにする、支持部分。実機ではボールキャスタで球が自由に回るように支持。

そのため、OpenHRPで制御系を作る場合は、

• コントローラブリッジを設定、起動してシミュれージョン側に入出力機能を付ける
• 制御プログラムをＲＴミドルウエアに基づいて作る(制御コンポーネント)
• （両者を繋ぐ、はコントローラブリッジに自動機能があるのでそれで十分）

すべてのポートは、TimedDoubleSeq型のデータ形式で、「時刻＋各値(double)の配列」という形です。 ジャイロと加速度センサは各１個で各々３軸なので、各々３要素の配列、ロボットは８関節(４ローラ＋４スライダ)あるのでacjointとcmforceは８要素の配列になります。値の順番は標準で関節に設定した番号順になります。
一方、cmjoint,cmspeed,cmaccelは中間に「:SX1,SX2,SY1,SY2:」と記載してあるので、４つのスライダの値に限定したもので、ローラは無関係になります。 スライダはIDが４～７でふってありますが、関節を限定すると、ここに記載した順番の配列になります。つまり、要素０がSX1、要素３がSY2の値になる４要素の配列です。

最後の--connectionによって、ブリッジが起動したときに、すでに動いているコンポーネントのポートと接続します。 なお、「--connection acaccel:BBcont0:acaccel」は「[ブリッジのacaccel]:[BBcont0:(の)acaccel]」という解釈です。 入出力の方向は関係なく、ブリッジ側：制御側、の順です。

ブリッジの起動は、GrxUIの「コントローラ」で設定します。バッチファイルを置いたところを実行ディレクトリにして、実行コマンドとしてバッチファイル名を指定します。コントローラ名は--server-nameで指定した名前です。 なお、このディレクトリにはブリッジの(かなり巨大な)ログが気づくと大量にたまります。 また、このディレクトリは、「プロジェクトのxmlファイルに対するの相対パス」で扱われるようです。 遠いディレクトリにおくと「../」が大量に続いたり、異なるドライブに分散させたりするとおかしくなるようです。 同一ディレクトリにシミュレーションモデルとコントローラのソースを置くのもどうかと思いますので、隣のディレクトリくらいにしておくといいかもしれません。もしくは、ブリッジのバッチファイルをモデルと同一ディレクトリにしておくか。

なお、このバッチファイルにエラーがあると、理由も分からずにシミュレーションの起動に失敗します。 おかしいと思ったら、コマンドプロンプトでそのままこのバッチファイルを実行して、エラーが出ないことを確認したほうがいいでしょう。 基本的には、ＲＴミドルウエアのコンポーネントの開発に従います。
コントローラ作成ガイドにあるように、rtc-templateを用いると、VC++でビルド可能なソリューション一式ができます。
テンプレートでできたものをただビルド、実行しても、動作するコンポーネントはできます(もちろん、なにもしません)。

あとは、この中に、制御のプログラムを書き込みます。最低限、実装すべき関数は

• onActivated()：ブリッジと接続されたあと、自動的にコンポーネントがActivateされると(動作状態になると)最初に呼ばれる。 ここで、制御に必要な初期化作業を行う。
• onDeactivated()：シミュレーションが停止するときに、自動的にDeactivateされると(動作停止)呼ばれる。 必要なら後始末(今は確認用のメッセージだけ)。
• onExecute()GrxUIで設定するのコントロール時間の間隔で呼び出されて、データを渡される制御処理本体。この関数で演算して、出力ポートのwrite()関数を呼べば、指令値が伝わる。

OnExecute関数の冒頭に while(1)で囲まれた、値の取得部分があります。 本来は、単にisNewしてreadすればよさそうなのですが、書き方によっては、前回のシミュレーションの最後の状態が読めたり、不思議な現象が再現性良く現れたので、その対策として、時刻０が読めるまではから読みするようにしています。 コントローラの開発(実行、デバッグ)のルーチンワーク：

1. GrxUIを起動、プロジェクトを読み込む。
2. モデルの修正をしたら、モデルの保存。モデルの初期状態などの修正は、プロジェクトの保存の必要もあり。 そのたびに、再読込をしておいたほうがいい。
3. VC++でコントローラの.slnを読み込み。Ｆ５でビルド実行。
4. シミュレーション実行。
5. 修正する場合は、シミュレーション終了後・止めてから、コントローラを[×]。 修正、再実行、シミュレーション開始。「再スタートしますか」には「はい」を選択したほうが、たぶんいい。

• BBcont.h：クラス定義のprotected:の中、DataIn(Out)Port declaration のところで、
  　　TimedDoubleSeq m_なんとか;
  　　In(Out)Port をセットで追加する。
• BBcont.cpp：コンストラクタの継承初期化部分に
  　　m_なんとかIn(Out) ("なんとか", m_なんとか),
  を追加する。（ダブルクオートの中がポート名）。
• BBcont.cpp：BBcont::onInitialize()に
  　　addIn(Out)Port("なんとか",m_なんとかIn(Out));
  を追加。また、同関数の後部に、
  　　m_なんとか.data.length(想定している要素数);
  を追加する。

• 姿勢角度の計算
• 球の転動量＝ローラの回転の計算
• 倒立振子制御によるローラトルクの算出

姿勢角度の計算は、加速度出力とレートジャイロの出力から計算しています。 加速度は重力加速度方向をある程度検出できる一方で、ロボットの振動の影響を受けます。特に、OpenHRPの加速度センサは帯域に制限がないようで、かなり過激な波形がでます(実際の加速度センサは、センサの帯域や取り付け方法などによってほどほどになまっている)。そのまま使えません(本来は、水平成分と垂直成分をatan2にいれれば何とかなりそうですが垂直成分も変動が大きいので、9.8決め打ちにしています)。
ジャイロはそのまま積分すると、動き方によっては角度にずれが出ます。実在のレートジャイロに比べると、ドリフトしないのは理想的ですが。 そこで、この二つを、実在のBallIP同様に合成ジャイロの手法で、合成して、傾斜角度を求めています。 具体的には、加速度センサから計算した傾斜角から低域(ほぼ直流分)を取り出し、ジャイロの積分値から広域を取り出し、合算しています。

球の転動量はローラの回転角度で代用しています。ローラと球の間にはすべりが明らかにあり、同方向に回転する２本のローラは期待では同一回転をするはずですが、明らかに異なる角度で回っています。 ので、ここでは、単純に、同方向の２本のローラの回転の平均値を使いました。

制御、DoControl()では傾斜角、角速度、球移動位置、移動速度に対して、K1～K4のゲインでＰＤ制御をかけています。
なお、ゲインは、実験的？に決めたので、根拠はありません。
また、制御に味をだす？ため、目標値を時間とともに振るようなデモコードが入れてあります。 ローラの回転を伝えるには、ローラを球に押しつけないと摩擦力が不足します。 これを固定の構造では、上述の通りシミュレーションがうまくいかなかったため、スライダを加えて、一定の力で押しつけるようにしました。 ただし、単に押しつけ力を発生させるのでは、二つの問題があります。
一つは物理的に明らかな現実にもある問題で、力でおすだけだと、微妙な力のずれや外力などでペアのローラが一緒に原点からずれていくという現象です(結果、ロボットと球がずれる)。本来はこの対策はまじめに実装する必要がありますが、もう一つの影響が大きいので、あわせて対処しています。
二つ目はローラのめり込み現象です。ただ、押しつけるだけなら問題なくとも、その状態でローラをまわすと、ローラがどんどんめり込んでいく現象が見られました。原因は不明ですが、おそらく、接触力の計算値が、ぎりぎり接触を維持するのに不足して、僅かずつ沈んでいく、という感じなのではないかと思います。

この現象に対処するために、いくつか試して現状で採用した方法は、「推力を設定しつつ、位置を強制する」です。 現行のシミュレータ(コントローラブリッジ)では、ハイゲインではない、トルク(力)モードであっても、位置、速度、加速度を設定できるようです。 具体的には、ブリッジのin-portで、TORQUEだけではなく、VALUE、VELOCITY、ACCELERATION を設定し、そこに制御側から値を書き込みます。 onExecuteの後半に実装してあり、

• 速度cmspeed、加速度cmaccelの値をゼロにする(ゼロにしないと、速度が蓄積して、矯正しきれなくなる)。
• 変位cmjointが一定量(D)以上ずれたら、強制的に位置を書き換える(acjointはローラ、スライダともなので要素番号が4-7、cmjointはスライダだけで0-3にある)。