ロボットを自在に制御しよう！

ロボットの構造を決定しよう！

　前回は、デモプログラムを利用したロボットシミュレーションについて説明してきました。今回は、前回の最後のほうで少し触れたロボットの構造について、そして関節や3次元描画、ロボットの制御について説明していきます。

　ロボットは、図1-1の「ボックス型」「ボール型」「カプセル型」のように3種類で構成されることになります。そのため、構築プロセスも積み木のように、各物体の種類（形状）・サイズ・初期配置を決定するだけのとても単純なものです。

　ここでは、ボックス型物体1個とボール型物体3個で構成されるデモバギー「demo_buggy.cpp」を例に、ロボットの構造の作成方法を説明していきます。

　物体を生成するための前準備として、各物体情報を収納する変数「static dBodyID」型と「static dGeomID」型の宣言が必要となります。デモバギーの場合「demo_buggy.cpp」の頭の部分に位置する「static dBodyID body[4];」「static dGeomID box[1];」「static dGeomID sphere[3];」となります。

　dBodyID型は物体の「位置」「角度」「重量」情報を収納する変数であり、生成する物体の個数分の変数を作る必要があります（4変数：body[4]）。一方、dGeomIDは「形状」情報を有する変数であるため、dBodyID型と同様に4変数ありますが、バギー本体のボックス型物体1個「box[1]」と、バギーの車輪となるボール型3個「sphere[3]」というように分類されます。

　次に、メインルーチンに記述されている物体生成のプログラムコードについて説明します。

　まず「dBodyCreate」で物体1個をシミュレーション空間に生成し、それぞれに以下の情報を入力します。
　「dBodySetPosition」で位置情報
　「dBodySetQuanternion」で角度情報
　「dBodySetBox/dBodySetSphere→dMassAdjust→dBodySetMass」で重量分布（サイズ＆重量）

　ここで一番難しいと思われるのは、物体の角度情報を入力するときに使われる四元数（クオータニオン）です。

　四元数とは、三次元空間上における物体の角度を示す方法です。物体の回転軸ベクトルを（ax,ay,az）とし、その軸周りで回転している角度をAngleとおく場合、その物体の角度情報を4つの変数(cos(angle/2), sin(angle /2)*ax, sin(angle /2)*ay, sin(angle /2)*az）で示します。

　そのため、四元数を使って「void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az,　dReal angle);」において回転軸ベクトル情報と角度情報を入力する、ということになるのです。

　次に、物体の「種類」「サイズ」情報を設定します。ここで気をつけるのは、物体の形状ごとに異なる設定コードとなるため、ボックス型の場合はdGeom型変数box[1]を使用して、直方体3辺の長さを入力するということです。対してボール型の場合は、dGeom型変数sphere[3]を使用し、半径を入力します。

　なお、これらのサイズは重量分布の設定時に入力したサイズと同じである必要があります。

　そして、物体生成の最後に、「body」情報と「box/sphere」情報を「dGeomSetBoy」に基づいて関連付けします。

ロボットの動きに必要な関節構造を決定しよう！

　ODEでは、図1-2に示されるように多くの関節タイプが準備されています。ODEのユーザーガイド（http://www.ode.org/ode-latest-userguide.html）の「7.3. Joint parameter setting functions」に各関節の構造がわかりやすい概念図で紹介されていますのでご確認ください。

　関節の生成方法は、物体の生成方法と類似しています。まず関節情報は「static dJointID」型変数に収納されます。デモバギーにおいては、ボックス型本体に接続する3個の車輪となりますので「static dJointID joint[3];」というように3変数の宣言がされています。

　物体の生成の直後に記述される関節生成コードについては下記をご覧ください（***は関節名）。

1）「dJointCreate***」で関節の種類を設定。デモバギーではヒンジ2関節を設定。
2）関節は2物体があって生成可能となるので、「dJointAttach」により2物体を選択（物体のdBody型の変数を指定）。デモバギーでは、body[0]とbody[1]、body[0]とbody[2]、body[0]とbody[3]の3種類の組み合わせ。
3）関節の位置座標を「dJointSet***Anchor」により入力。デモバギーではボール型物体の中心部を回転軸としているため、ボール型物体の位置座標を「dBodyGetPosition」により読み取り、入力。
4）軸角度ベクトル「dJointSet***Axis」の設定。なお、ヒンジ2関節は2自由度関節なので、次の2軸の設定が必要です。
　1自由度目は車輪を左右旋回するための軸としてZ軸周りの回転軸に設定し、2自由度目は車輪を回転させるためY軸周りの回転軸として設定します。