markdown ODEロボットシュミレーション勉强

Posted 2021-05-06

# ジオメトリの作成
## ジオメトリとは
衝突検出システムの中で最も基本となるオブジェクトでボディ単体の形やボディの集合の形を表す

## ジオメトリのAPI
### 球
+ 半径rの球ジオメトリをスペースspaceに作成し、そのID番号を返す(ジオメトリID)
```
dGeomID dCreateSphere(dSpaceID space, dReal r);
```
+ 球sphereの半径rを設定または取得する
```
void dGeomSphereSetRadius(dGeomID sphere, dReal r);
dReal dGeomSphereGetRadius(dGeomID sphere);
```
+ 球sphere内部の位置(x,y,z)の深さを取得する。位置が球の内部なら深さが正の値
球の外部なら負の値になり、球の表面なら0になる
```
dReal dGeomSpherePointDepth(dGeomID sphere, dReal x, dReal y, dReal z);
```

### 直方体
+ スペースspaceにx,y,z軸に沿った長さlx,ly,lzの直方体ジオメトリを生成し、そのID番号を返す
```
dGeomID dCreateBox(dSpaceID space, dReal lx, dReal ly, dReal lz);
```

+ 直方体boxの各辺の長さ(lx,ly,lz)を設定または、resultに取得する
```
void dGeomBoxSetLengths(dGeomID box, dReal lx, dReal ly, dReal lz);
void dGeomBoxGetLengths(dGeomID box, dVector3 result);
```

+ 直方体box内部の位置(x,y,z)の深さを取得
```
dReal dGeomBoxPointDepth(dGeomID box, dReal x, dReal y, dReal z);
```

### 平面
+ スペースspaceに平面ジオメトリを与えられた引数で生成し、そのID番号を返す
```
dGeomID dCreatePlane(dSpaceID space, dReal a, dReal b, dReal c, dReal d);
```
a,b,cは平面の方程式ax+by+cz=dのパラメータ

+ 平面planeのパラメータ(a,b,c,d)を設定またはresultに取得する
```
void dGeomPlaneSetParams(dGeomID plane, dReal a, dReal b, dReal c, dReal d);
void dGeomPlaneGetParams(dGeomID plane, dVector4 result);
```
+ 平面plane内部の位置(x,y,z)の深さを取得
```
dReal dGeomPlanePointDepth(dGeomID plane, dReal x, dReal y, dReal z);
```

### カプセル
+ スペースspaceに半径r、長さlのカプセルジオメトリを生成し、そのID番号を返す
```
dGeomID dCreateCapsule(dSpaceID space, dReal r, dReal l);
```
両端の半球は長さlに含まれていないため => 実際は**l + 2*r**
+ カプセルcapsuleの半径r、長さlを設定または取得する
```
void dGeomCapsuleSetParams(dGeomID capsule, dReal r, dReal l);
void dGeomCapsuleGetParams(dGeomID capsule, dReal *r, dReal *l);
```
+ カプセルcapsule内部の位置(x,y,z)の深さを取得する
```
dReal dGeomCapsulePointDepth(dGeomID capsule, dReal x, dReal y, dReal z);
```

### 円柱
+ スペースspaceに半径r,長さlの円柱ジオメトリを生成し、そのI番号を返す
```
dGeomID dCreateCylinder(dSpace space, dReal r, dReal l);
```

### 光線
+ スペースspaceに長さlの光線ジオメトリを生成し、そのID番号を返す
```
dGeomID dCreateRay(dSpaceID space, dReal l);
```
+ 光線rayの長さlを設定または取得する
```
void  dGeomRaySetLength(dGeomID ray, dReal l);
dReal dGeomRayGetLength(dGeomID ray);
```
+ 光線rayの始点(px,py,pz)と方向ベクトル(dx,dy,dz)を設定する
```
void dGeomRaySet(dGeomID ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz);
```
光線の相対座標系のz軸が方向ベクトルに沿うように回転行列が調整される、ただし、光線の長さは調整されない
+ 光線rayの始点startと方向dir(単位ベクトル)を取得する
```
void GeomRayGet(dGeomID ray, dVector3 start, dVector3 dir);
```

# ode物体の生成について
## ボディとは
剛体のこと  
剛体は力が加えられても変形しない大きさのある理想的な物体

3次元空間でボディを指定するためには位置と姿勢を決めなければならない

## ボディの生成
```
dBodyId dBodyCreate(dWorldID world);
```
worldにボディを作成する  
戻り値(dBodyID型)がボディを識別するためのID番号となる

## 質量パラメータの設定
### 質量パラメータの初期化
```
void dMassSetZero(dMass *mass);
```
質量パラメータmassを0に初期化する

### 質量パラメータの計算
ロボットシュミレーションのための質量、慣性テンソル、重心位置など  
重要なパラメータをまとめdMass構造体で定義している
```
typedef struct dMass{
  dReal mass; // ボディの全質量
  dVector4 c; // ボディ座標系での重心位置
  dMatrix3 I; // ボディ座標系での慣性テンソル(3*3行列)
} dMass;
```

## 基本形状で重心位置や慣性テンソルを自動計算するAPI
### 球
```
void dMassSetSphere(dMass *mass, dReal d, dReal r);
void dMassSetSphereTotal(dMass *mass, dReal m, dReal r);
```
基本Totalの方を使うのが楽  
使用例  
```
dMass mass;
dReal m = 10; //構造体にまとめるといい(質量)
dReal r = 1;  //構造体にまとめるといい(半径)
dBodyID ball = dBodyCreate(world); //ballのなかにIDが入る

dMassSetZero(&mass);             //重心をリセット
dMassSetSphereTotal(&mass, m, r);//massのパラメータの自動計算
dBodySetMass(ball, &mass);       //ballというボディに現在のmassデータをセットする
dBodySetPosition(ball, x,y,z);   //ballというボディの位置を与える
```
これが基本的な流れ => ほかのものにも適応できる

### カプセル
```
void dMassSetCapsule(dMass *mass, dReal d, int dir, dReal r, dReal l);
void dMassSetCapsuleTotal(dMass *mass, dReal m, int dir, dReal r, dReal l);
```
**dirについて**  
長軸方向を表す
+ 1 : x軸方向
+ 2 : y軸方向
+ 3 : z軸方向

### 円柱
```
void dMassSetCylinder(dMass *mass, dReal d, int dir, dReal r, dReal l);
void dMassSetCylinderTotal(dMass *mass, dReal m, int dir, dReal r, dReal l);
```
基本カプセルと同じ

### 直方体
```
void dMassSetBox(dMass *mass, dReal d, dReal lx, dReal ly, dReal lz);
void dMassSetBoxTotal(dMass *mass, dReal m, dReal lx, dReal ly, dReal lz);
```

## 質量パラメータの設定
```
void dBodySetMass(dBodyID body, const dMass *mass);
```
ボディbodyに質量パラメータmassを設定する
```
void dBodyGetMass(dBodyID body, dMass *mass);
```
ボディbodyの質量パラメータmassを取得する

## 質量パラメータの調整
```
void dMassAdjust(dMass *mass, dReal new_m);
```
質量パラメータmassの全質量が質量new_mに一致するように質量パラメータを調整

```
void dMassTranslate(dMass *mass, dReal x, dReal y, dReal z);
```
ボディの位置は通常、重心位置となる。そして、それが相対座標系の原点になる  
その原点を(x,y,z)と変位させたいときの質量パラメータmassを計算する

```
void dMassRotate(dMass *mass, const dMatrix3 R);
```
相対座標系で回転行列Rだけ回転させたときの質量パラメータmassを計算する

```
void dMassAdd(dMass *a, const dMass *b);
```
質量パラメータbを質量パラメータaに加える

## 位置の設定
```
void dBodySetPosition(dBodyID body, dReal x, dReal y, dReal z);
```
ボディbodyの位置を絶対座標系(x,y,z)[m]に設定する

## 姿勢の設定
### 回転行列を使用する場合
+ 回転行列Rを単位行列に設定
```
void dRSetIdentity(dMatrix3 R);
```

+ 回転軸ベクトル(ax,ay,az)を中心にangle[rad]回転(反時計回り)したときの回転行列Rを取得
```
void dRFromAxisAndAngle(dMatrix3 R, dReal ax, dReal ay, dReal az, dReal angle);
```

+ ボディbodyに回転行列Rの姿勢を設定
```
void dBodySetRotation(dBodyID body, const dMatrix3 R);
```

+ 回転行列Rをクオータニオンqに変換する
```
void dRtoQ(const dMatrix3 R, dQuaternion q);
```

+ オイラー角 phi, theta, psi から回転行列Rを取得する
```
void dRFromEulerAngles(dMatrix3, dReal phi, dReal theta, dReal psi);
```

+ 2つのベクトル(ax,ay,az), (bx,by,bz)から回転行列Rを取得する
**これらのベクトルには回転後の相対座標系のx,y軸ベクトルを入れる**
```
void dRFrom2Axes(dMatrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz);
```

### クオータニオンを使う場合
+ クオータニオンqを無回転に設定する
```
void dQsetIdentity(dQuaternion q);
```

+ 回転軸ベクトル(ax,ay,az)を中心にangle[rad]回転(反時計回り)したときのクオータニオンqを取得
```
void dQFromAxisAndAngle(dQuaternion q, dReal ax, dReal ay, dReal az, dReal angle);
```

+ ボディbodyにクオータニオンqを設定する
```
void dBodySetQuaternion(dBodyID body, const dQuaternion q);
```

+ クオータニオンqを回転行列Rに変換する
```
void dQMultiply0(dQuaternion qa, const dQuaternion qb, const dQuaternion qc);
void dQMultiply1(dQuaternion qa, const dQuaternion qb, const dQuaternion qc);
void dQMultiply2(dQuaternion qa, const dQuaternion qb, const dQuaternion qc);
void dQMultiply3(dQuaternion qa, const dQuaternion qb, const dQuaternion qc);
```
dQMultiplyの後の数字はほかの掛算関数と同様に
  - 0 : qa = qb * qc  : qc回転したあとに、qb回転する純パン
  - 1 : pbの逆  
  - 2 : qcの逆
  - 3 : 両方逆のクオータニオンを使って計算する

+ 姿勢q、角速度ベクトルwからqの時間微分を計算し、その値dqを返す
```
void dWtoDQ(const dVector3 w. const dQuaternion q, dVector4 dq);
```

## 速度の設定
```
void dBodySetLinerVel(dBodyID, dReal x, dReal y, dReal z);
void dBodySetAngularVel(dBodyId, dReal x, dReal y, dReal z);
```
ボディの速度(x軸成分, y軸成分, z軸成分)[m/s],
角速度(x軸まわり, y軸まわり, z軸まわり)[rad/s]を設定する

## 力、トルクの設定
```
void dBodySetForce(dBodyID body, dReal x, dReal y, dReal z);
void dBodeySetTorque(dBodyID body, dRal x, dReal y, dReal z);
```

## ボディの破壊
```
void dBodyDestroy(dBodyID body);
```
ボディbodyを破壊するが、ボディに接続されたジョイントは機能しなくなるが**破壊はされない**

# ODEの使い方
## デフォルトのキー操作

1. Ctrl + t : テクスチャのON/OFF
2. Ctrl + s : 影のON/OFF
3. Ctrl + x : 強制終了
4. Ctrl + p : 一時停止のON/OFF
5. Ctrl + o : 1ステップの実行


##　シュミレーションの再実行するための操作
1. 必要なオブジェクトの破壊
  + ジョイントの破壊 : ```dJointDestroy()```
  + ボディの破壊     : ```dBodyDestroy()```
  + ジオメトリの破壊 : ```dGeomDestroy()```

2. 接触点グループの破壊 : ```dJointGroupDestroy()```
3. 接触点グループの生成 : ```dJointGroupCreate()```
4. 必要なオブジェクトの生成
  + ボディの生成  : ```dBodyCreate()```
  + 質量、位置、姿勢の設定(必要があれば初期値を変更) :  
```
  dBodySetMass()
  dBodySetPosition()
  dBodySetRotation()
```
  + ジオメトリの生成 : ```dCreateBox()``` など
  + ボディとジオメトリの対応付け : ```dGeomSetBody()```
  + ジョイントの生成と設定 : ```dJointCreateHinge()```　など

# 描画のクオリティ

## 球の描画クオリティを設定
```
dsSetSphereQuality(n);
```
デフォルトではn=1

## カプセルの描画クオリティを設定
```
dsSetCapsuleQuality(n);
```
デフォルトではn=3

## ODE
物理演算シュミレーター  
GUIにOpenGl等を使用することができる

## ダウンロード
### ode自体のダウンロード
自分のbitbucket codepracticeの中にある**ode-0.13.tar.bz2**を他の場所にコピーし、  
解凍する

###事前に必要なパッケージ
```
sudo apt-get install automake libtool freeglut3-dev
```

## ode解凍後の作業
1. 解凍したディレクトリに移動
2. ```./boostrap```  を実行
3. ```./configure --with-trimesh=opcode --enable-new-trimesh --enable-shared  --enable-release --with-x --enable-double-precision --with-libccd```  を実行する
4. ```make``` 長いので待つ
5. ```sudo make install```

以上でodeを使用する環境が整った

##drawstufを使用する場合
1. ```sudo cp -r include/drawstuff /usr/local/include/```
2. ```sudo cp drawstuff/src/.libs/libdrawstuff.* /usr/local/lib```
3. ```sudo ldconfig```


これでgitのODE環境の作成が完了した