choreonoidで倒立振子のシミュレーション


# 概要
choreonoidで倒立振子のシミュレーションをやってみたので、備忘録もかねて投稿します。
記事の構成は以下の通りです。
- シミュレーション環境
- 倒立振子のモデルプログラム
- コントローラのプログラム
- シミュレーションの実行

# シミュレーション環境
私の使っているPCはWindows10なので、
- Virtual Box
- Ubuntu 16.0.4 LTS
を使ってchoreonoidを使えるようにしました。
Ubuntu 16.0.4 LTSのインストールまでは下記の記事をご参考く
ださい。
https://qiita.com/pyon_kiti_jp/items/0be8ac17439abf418e48

Ubuntuが使えるようになったらchoreonoidのインストールをインストールします。
私は**choreonoid1.7.0 リリース版**をインストールしました。以下の公式サイトをご参考ください。
https://choreonoid.org/ja/manuals/1.7/install/build-ubuntu.html

Ubuntu上でchoreonoidが使えるようになったら成功です。起動すると以下の画像のようになります。
![scene00000000.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/839547/c6eeca4d-8597-64f5-afa2-2d19b99de768.png)

# 倒立振子のモデルプログラム
choreonoidではどのようにモデルを作るかを説明します。ここでは例として、以下のような外観の倒立振子を作ります。
![scene00000000.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/839547/d81c4fd8-ffb6-2aed-3a8f-34e5702d7ffb.png)


ファイルの形式は**yaml形式**です。コードの中身は以下の通りです。

```yaml
format: ChoreonoidBody
formatVersion: 1.0
angleUnit: degree
name: Cart_type_Inverted_pendulum

links:
   -
      name: Rail
      jointType: fixed
      mass: 500.0
      centerOfMass: [ 0, 0, 0 ]
      inertia: [ 1, 0, 0,
                 0, 1, 0,
                 0, 0, 1 ]
      elements:
         Transform:
            translation: [ 0, 0, 0.02 ]
            elements:
            -
               type: Shape
               geometry: { type: Box, size: [5, 0.1, 0.04] }
               appearance: &Rail_appearance
                  material:
                     diffuseColor: [ 0.8, 0.8, 0.8 ]
                     specularColor: [ 0.5, 0.5, 0.5 ]
                     shininess: 0.6
   -
      name: Cart
      parent: Rail
      jointType: prismatic
      jointId: 0
      jointAxis: X
      mass: 0.455
      centerOfMass: [ 0, 0, 0 ]
      inertia: [ 1, 0, 0,
                 0, 1, 0,
                 0, 0, 1 ]
      elements:
         Transform:
            translation: [ 0, 0, 0.065 ]
            elements:
            -
              type: Shape
              geometry: { type: Box, size: [0.3, 0.1, 0.05] }
              appearance: &Cart_appearance
                 material:
                    diffuseColor: [ 0, 0.6, 0 ]
                    specularColor: [ 0.3, 0.3, 0.3 ]
                    shininess: 0.6
   -
      name: Pendulum
      parent: Cart
      translation: [ 0, 0, 0.09 ]
      jointType: revolute
      jointId: 1
      jointAxis: Y
      mass: 0.21
      centerOfMass: [ 0, 0, 0.32 ]
      inertia: [ 0.028788598, 0,           0,
                 0,           0.028788598, 0,
                 0,           0,           0.028788598 ]
      elements:
         -
            type: Shape
            geometry: { type: Cylinder, height: 0.08, radius: 0.02 }
            appearance: &Pendulum_appearance
               material:
                  diffuseColor: [ 1, 0.9, 0.0 ]
                  specularColor: [ 0.3, 0.3, 0.3 ]
                  shininess: 0.6
         -
            type: Transform
            translation: [ 0, 0, 0.32065 ]
            rotation: [ 1, 0, 0, 90 ]
            elements:
               Shape:
                  geometry: { type: Cylinder, height: 0.6413, radius: 0.01 }
                  appearance: *Pendulum_appearance
```
ではコードの中身を詳細に見ていきましょう。
## 最初の宣言
最初の宣言である

```yaml
format: ChoreonoidBody
formatVersion: 1.0
angleUnit: degree
name: Cart_type_Inverted_pendulum
```
は、choreonoidに認識させるためのコードです。上記のように書けば問題ありません。

```yaml
angleUnit: degree
```
は、この先モデルの姿勢を回転させるときの単位です。degreeの方がわかりやすいでしょう。

## モデルコードの構成
コードの構成をおおざっぱに述べると以下のようになります。
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/839547/6f9418f9-73aa-7c7f-239e-a0f2ca249080.jpeg" width=50%>
**Rail→Cart→Pendulum**というような親子関係となります。
Rail、Cartを記述するプログラムは、
- 運動の拘束と物理パラメータ（質量、慣性モーメント等）を記述する部分
- 構成要素の姿勢を記述する部分
- 構成要素の形、寸法、色を記述する部分
の3つに分けられます。

また振子は、
- 回転軸
- 棒
の２つのモデルを組み合わせて作成します。

## Railのコード
以下のようなRailを作ります。
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/839547/b62d6f87-6a5d-9b74-8f23-30922b5f6036.jpeg" width=50%>

### Railの運動拘束・物理パラメータの記述
最初に、

```yaml
name: Rail
```
と名前を付けます。これは後に親子関係を記述するのに使います。
Railは動かさないので、

```yaml
jointType: fixed
```
と記述します。
質量などの物理パラメータは、

```yaml
mass: 500.0
centertOfMass : [ 0, 0, 0 ]
inertia: [ 1, 0, 0,
           0, 1, 0,
           0, 0, 1 ]
```
と記述します。Railは動かさないので、これらの値は適当に決めても問題ありません。


### Railの姿勢の記述
今のままだとRailの下半分が埋まってしまうので、RailをZ軸方向に移動させます。
Railの高さ（厚さ）は4mmにしているので、2mmほどZ軸方向に移動させるため、以下のように記述します。

```yaml
translation: [ 0, 0, 0.02 ]
```

### Railの形状・寸法・色の記述
Railの形状は箱状なので、

```yaml
geometry: { type: Box, size: [5, 0.1, 0.04] }
```
と記述します。sizeの隣には、[x軸寸法　y軸寸法　z軸寸法]の順に寸法を入力します。
次に色ですが、色の指定の前に

```yaml
appearande: &Rail_appearance
```
と記述します。この部分を書くことで、同じ色を参照するときに記述を簡略化できます。
Railの色は、

```yaml
diffuseColor: [ 0.8, 0.8, 0.8 ]
```
と記述します。順番はRGB、つまり[Red, Green, Blue]となります。値を0～1の間で指定します。

```yaml
specularColor: [ 0.6, 0.6, 0.6 ]
```
はRGBの鏡面反射係数の値です。値を0～1の間で指定します。

```yaml
shininess: 0.6
```
は光沢度を指定します。値を0～1の間で指定します。

## Cartのコード
以下のようなCartを作ります。
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/839547/8d141538-02ff-5240-1711-e145e93ac1be.jpeg" width=50%>

### Cartの運動拘束・物理パラメータの記述
名前を以下のように指定します。

```yaml
name: Cart
```
Cartの親をRailにしたいので、

```yaml
parent: Rail
```
と記述します。

CartはX方向に並進運動してほしいので、

```yaml
jointType: prismatic
jointId: 0
jointAxis: X
```
と記述します。この**prismatic**で並進運動を指定することができます。
**jointId**でジョイントに番号付けします。これを書くことで、choreonoidの画面上で変位を数字で見ることができます。
**jointAxis**で、台車変位の正の向きを決めます。

物理パラメータはRailの時と同様に、

```yaml
mass: 0.455
centerOfMass: [ 0, 0, 0 ]
inertia: [ 1, 0, 0,
           0, 1, 0,
           0, 0, 1]
```
と記述します。Cartは回転しないので、慣性モーメントは適当に決めて問題ありません。

### Cartの姿勢の記述
Railの時と同様に、Cartが埋まらないようにZ軸方向に移動させます。
Railの厚さを4mm、Cartの厚さを5mmと指定したので、以下のように記述してz軸方向に6.5mm移動させます。

```yaml
translation: [ 0, 0, 0.065 ]
```

### Cartの形状・寸法・色の記述
Railの場合と同じように、Cartの形状・寸法・色を指定します。
Railとは別の色にしておきます。

```yaml
type: shape
geometry: { type: Box, size: [0.3, 0.1, 0.05] }
appearance: &Cart_appearance
   material:
      diffuseColor: [ 0, 0.6, 0 ]
      specularColor: [ 0.3, 0.3, 0.3 ]
      shininess: 0.6
```

## Pendulumのコード
以下のようなPendulumを作ります。
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/839547/625f8cc4-5dd1-dda0-3fc7-641b78607f7f.jpeg" width=50%>
### Pendulumの運動拘束・物理パラメータの記述
名前を以下のように記述します。

```yaml
name: Pendulum
```
cartをpendulumの親とします。

```yaml
parent: Cart
```

高さ4mmのRail、高さ5mmのCartの上にpendulumを取り付けるので、以下のように記述してz軸方向に9mm移動します。

```yaml
translation: [ 0, 0, 0.09 ]
```
pendulumはy軸周りに回転してほしいので、

```yaml
jointType: revolute
jointId: 1
jointAxis: Y
```
と記述します。この**revolute**で、回転運動を指定することができます。

物理パラメータはRail、Cartと同様に、

```yaml
mass: 0.21
centerOfMass: [ 0, 0, 0.32 ]
inertia: [ 0.028788598, 0,           0,
           0,           0.028788598, 0,
           0,           0,           0.028788598 ]
```
とします。上の図で示した振子は細長い棒と太くて短い円柱の組み合わせで成立していますが、重心・慣性モーメントのパラメータは棒のみを考慮して決定しました。
###回転軸の様相、形状
回転軸の形状を作ります。以下のように記述し、図で示したような円柱を作ります。
まずは

```yaml
elements:
   -
```
と記述し、そのあとに

```yaml
type: Shape
geometry: { type: Cylinder, height: 0.08, radius: 0.02 }
```
と記述して所望寸法の円柱を作ります。その後、

```yaml
appearance: &Pendulum_appearance
material:
   diffuseColor: [ 1, 0.9, 0.0 ]
   specularColor: [ 0.3, 0.3, 0.3 ]
   shininess: 0.6
```
と記述して色を指定します。

### 棒の様相、形状
棒の形状を記述する前に、

```yaml
type: Transform
translation: [ 0, 0, 0.32065 ]
rotation: [ 1, 0, 0, 90 ]
```
と記述して位置を決めます。

```yaml
type: Transform
```
は、棒の位置を決める前の初期宣言です。
それから棒の長さの半分（320.65mm）だけZ軸方向に動かすため、

```yaml
translation: [ 0, 0, 0.32065 ]
```
と記述します。
それから棒を図のように建てた姿勢にするため、

```yaml
rotation: [ 1, 0, 0, 90 ]
```
と記述します。カッコ内の要素は順に、[ x軸, y軸, z軸,　回転量 ]という意味です。
つまり今回は、**x軸周りに90度回転する**という意味になります。

棒の位置と姿勢を決めたら、

```yaml
elements:
   Shape:
      geometry: { type: Cylinder, height: 0.6413, radius: 0.01 }
```
と記述して棒の形と寸法を決めます。

最後に色を決めますが、今回は回転軸と同じ色にしたいので、

```yaml
appearance: *Pendulum_appearance
```
と記述します。

以上となります。
このプログラムは、
\home\username\ダウンロード\choreonoid-1.7.0\ext
上に**Inverted_pendulum**という名前のフォルダを作り、そこに
Cart_type_inverted_pendulum.body
という名前で保存しましょう。

# コントローラのプログラム
倒立振子の安定化プログラムの全体像は以下の通りです。

```cpp
#include <cnoid/SimpleController>
#include <cnoid/Link>
#include <vector>

using namespace cnoid;

const double F[] = {1.25, 28.637, 1.6504, 4.2012};

class cart_type_controller : public cnoid::SimpleController
{
	Link* joint_c;
	Link* joint_p;

	double xc;
	double dxc;
	double qp;
	double dqp;
	double f;

public:
	
	virtual bool initialize(SimpleControllerIO* io) override
	{
		joint_c = io->body()->link("Cart");
		io->enableInput(joint_c, JOINT_DISPLACEMENT | JOINT_VELOCITY);
		joint_c->setActuationMode(Link::JOINT_FORCE);
		io->enableOutput(joint_c);

		joint_p = io->body()->link("Pendulum");
		io->enableInput(joint_p, JOINT_ANGLE | JOINT_VELOCITY);
		joint_p->setActuationMode(Link::NO_ACTUATION);

		return true;
	}

	virtual bool control() override
	{
		xc = joint_c->q();
		dxc = joint_c->dq();

		qp = joint_p->q();
		dqp = joint_p->dq();

		f = F[0]*xc + F[1]*qp + F[2]*dxc + F[3]*dqp;

		joint_c->u() = f;

		return true;

	}
};

CNOID_IMPLEMENT_SIMPLE_CONTROLLER_FACTORY(cart_type_controller)
```
## 最初の宣言
以下のコードは最初の宣言です。

```cpp
#include <cnoid/SimpleController>
#include <cnoid/Link>
#include <vector>

using namespace cnoid;
```

## 線形状態フィードバックゲインベクトル
線形状態フィードバックゲインベクトルは以下のように定義します。

```cpp
const double F[] = {1.25, 28.637, 1.6504, 4.2012};
```
左から順に、台車変位、振子角度、台車速度、振子角速度のフィードバックゲインです。
これらの値は最適レギュレータ理論で決めました。詳しい決め方はchoreonoidの使い方を解説するという趣旨からずれるため、割愛します。

## リンクと変数の定義
以下のように記述して、リンクと変数を定義します。

```cpp
class cart_type_controller : public cnoid::SimpleController
{
	Link* joint_c;
	Link* joint_p;

	double xc;
	double dxc;
	double qp;
	double dqp;
	double f;
```
前半２行

```cpp
Link* joint_c;
Link* joint_p;
```
は、リンクを格納するための変数です。リンクとして扱うことで、変位や速度といった情報を取り出すことができます。
後半の5行

```cpp
double xc;
double dxc;
double qp;
double dqp;
double f;
```
は、順番に
1. 台車変位
2. 台車速度
3. 振子角度
4. 振子角速度
5. 力
を格納するための変数です。

## リンクの初期設定
次のコードは、リンク初期設定を行うプログラムです。

```cpp
virtual bool initialize(SimpleControllerIO* io) override
{
	joint_c = io->body()->link("Cart");
	joint_c->setActuationMode(Link::JOINT_FORCE);
	io->enableOutput(joint_c);
	io->enableInput(joint_c, JOINT_DISPLACEMENT | JOINT_VELOCITY);

	joint_p = io->body()->link("Pendulum");
        io->enableInput(joint_p, JOINT_ANGLE | JOINT_VELOCITY);
	joint_p->setActuationMode(Link::NO_ACTUATION);

	return true;
}
```
先ほど定義したjoint_c、joint_pを使って

```cpp
joint_c = io->body()->link("Cart");
```

```cpp
joint_p = io->body()->link("Pendulum");
```
と書くことで、BodyファイルのCart、Pendulumをリンクとして扱えるようにします。
それから、

```cpp
io->enableInput(joint_c, JOINT_DISPLACEMENT | JOINT_VELOCITY);
```

```cpp
io->enableInput(joint_p, JOINT_ANGLE | JOINT_VELOCITY);
```
と書くことで、Cartから
- 台車変位
- 台車速度
を、Pendulumからは
- 振子角度
- 振子角速度
を取得できます。

次にCartとPendulumの駆動方式を定義します。Cartには制御入力を加えたいので、

```cpp
joint_c->setActuationMode(Link::JOINT_FORCE);
io->enableOutput(joint_c)
```
と記述し、振子は自由に動いてほしいので

```cpp
joint_p->setActuationMode(Link::NO_ACTUATION);
```
と書きます。

## 状態変数の取得と制御入力の決定
以下のプログラムで、状態変数の取得と制御入力の決定を行います。

```cpp
virtual bool control() override
{
	xc = joint_c->q();
	dxc = joint_c->dq();

	qp = joint_p->q();
	dqp = joint_p->dq();

	f = F[0]*xc + F[1]*qp + F[2]*dxc + F[3]*dqp;

	joint_c->u() = f;

	return true;
}
```
前半の

```cpp
xc = joint_c->q();
dxc = joint_c->dq();

qp = joint_p->q();
dqp = joint_p->dq();
```
で、倒立振子の状態変数（台車変位、振子角度など）を取得します。その後の

```cpp
f = F[0]*xc + F[1]*qp + F[2]*dxc + F[3]*dqp;
```
で制御入力を計算し、

```cpp
joint_c->u() = f;
```
で、台車に制御入力を加えます。

## コントロールプログラムのコンパイル手順
上で解説したプログラムは以下の手順でコンパイルします。

1. コントロールプログラムの保存
Bodyファイルを、
\home\username\ダウンロード\choreonoid1.7.0\ext
上の
Inverted_pendulum
という名前のフォルダに保存したと思います。同じフォルダ上にコントロールプログラムを**cart_type_controller.cpp**の名前で保存します。

2. テキストファイル作成
同フォルダに**CMakeLists.txt**を作り、そこに、
add-cnoid-simple-controller(cart_type_controller cart_type_controller.cpp)
と書いて保存します。

3. 端末の起動・フォルダ移動・コマンドの実行
Ubuuntuの**端末**を起動し、**cdコマンド**を使って以下のフォルダに移動します。
\home\username\ダウンロード\choreonoid1.7.0
フォルダへ移動したら、**cmake**→**make**とコマンドを実行します。

5. soファイルの生成
コンパイルが正しく実行されれば、
\home\username\ダウンロード\choreonoid1.7.0\simplecontroller
のフォルダ内に、
**cart_type_controller.so**
ができています。

#シミュレーションの実行
最後に、倒立振子のシミュレーションのやり方を説明します。

1. choreonodの起動とワールドの作成
choreonoidを起動し、左上のタブの
「ファイル」→「新規」→「ワールド」
と進み、名前を付けて「作成」を押します。
ここでは名前を**Inverted_pendulum**としましょう。
![No_1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/839547/76766b07-befb-cef4-f15c-63b4d4db9375.png)

2. 「floor」と「Inverted_pendulum」のBodyファイル読み込み
 **floor**と先ほど作成した**Inverted_pendulum**のモデルを読み込みます。
floorは、
「ファイル」→「読み込み」→「ボディ」
と進み、choreonoidフォルダに移動して
\model\miscフォルダ内の**floor.body**を読み込みます。このBodyファイルはextフォルダ内の「Inverted_pendulum」フォルダ内にコピーしておいてもいいでしょう。
![floor読み込み.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/839547/c8af23a4-29fb-da94-c9d6-2bdfe2d7faf0.png)
次に倒立振子のモデルを読み込みます。floorと同じく
「ファイル」→「読み込み」→「ボディ」
と進み、choreonoidフォルダに移動し、
\ext\Inverted_pendulumと進み、「Cart_type_inverted_pendulum.body」を読み込みます。
![Inverted_pendulum読み込み.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/839547/d3ff3c1a-b517-2515-42f1-ef93dde85404.png)

3. コントロールプログラムの読み込み準備
コントロールプログラムを読み込ませる準備をします。左のウィンドウで倒立振子を選択状態（名前にオレンジ色が表示される状態）にし、左上の
「新規」→「シンプルコントローラ」
と進みます。名前は「stable_controller」としましょう。左のウィンドウの「Cart_type_Inverted_pendulum」の下に「stable_controller」ができます。
![soファイル読み込み準備.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/839547/cc7de265-fcfd-1059-ac77-07a54be85558.png)

4. コントロールプログラム読み込み
コントローラプログラムを読み込みます。「stable_controller」をダブルクリックし、それから下ウィンドウの「コントローラモジュール」右隣の空欄をダブルクリックします。するとフォルダのマークが出てくるので、そのフォルダマークをクリックします。フォルダマーククリックするとsoファイルの選択画面が出るので、
choreonoidフォルダに移動→simplecontroller
と進み、コンパイルで作成した「Cart_type_controller.so」を選択して読み込みます。
![soファイル読み込み.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/839547/fbe011df-6bd8-b9d5-80cf-e26a97d3da14.png)


5. AISTシミュレータの読み込み
「Inverted_pendulum」を選択し、
「ファイル」→「新規」→「AISTシミュレータ」
を選択し、「作成」を押します。
![asitシミュレータ.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/839547/87876bd3-2a74-22c2-dc29-d60a368c569f.png)


6. シミュレーション時間の設定
デフォルトではシミュレーション時間が無限時間となっているので、有限時間にしましょう。
AISTSimulatorをクリックし、その真下ウィンドウの、
時間範囲：指定時間
時間長：10.00
と設定を変えます。
![シミュレーション時間設定.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/839547/6fc59b68-4c1f-c06e-cfff-f55c5a247764.png)

7. シミュレーションの実行
倒立振子の安定化シミュレーションをやってみましょう。右のアニメーションウィンドウに表示されている倒立振子をマウスで動かし、右上の矢印マークの**現在位置からのシミュレーションを開始**を押します。すると安定化シミュレーションが実行されます。
![シミュレーションの実行.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/839547/c9673c4c-010b-a321-f94c-0a4495a44d92.png)

8. せっかく作ったので、このシミュレータを保存しましょう。「ext」フォルダ内の「Inverted_pendulum」に保存すればわかりやすいでしょう。「Inverted_pendulum.cnoid」の名前で保存します。

# 参考URL
この記事を書くにあたって参考にしたサイトを以下に示します。

- choreonoid1.7.0のマニュアル
https://choreonoid.org/ja/manuals/1.7/index.html

- choreonoidでの倒立振子のシミュレーション動画
https://www.youtube.com/watch?v=e_NCZI3TCqw

# 終わりに
今回、初めてQiitaに投稿してみました。
- 間違い
- 表現の不備
- 改善コメント
- 質問
等がございましたら、是非ともご指摘お願いします。