More than 3 years have passed since last update.

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

Last updated at 2020-11-07Posted at 2020-11-07

#概要
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が使えるようになったら成功です。起動すると以下の画像のようになります。

#倒立振子のモデルプログラム
choreonoidではどのようにモデルを作るかを説明します。ここでは例として、以下のような外観の倒立振子を作ります。

ファイルの形式は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

ではコードの中身を詳細に見ていきましょう。
##最初の宣言
最初の宣言である

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

は、choreonoidに認識させるためのコードです。上記のように書けば問題ありません。

angleUnit: degree

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

##モデルコードの構成
コードの構成をおおざっぱに述べると以下のようになります。

Rail→Cart→Pendulumというような親子関係となります。
Rail、Cartを記述するプログラムは、

運動の拘束と物理パラメータ（質量、慣性モーメント等）を記述する部分
構成要素の姿勢を記述する部分
構成要素の形、寸法、色を記述する部分
の3つに分けられます。

また振子は、

回転軸
棒
の２つのモデルを組み合わせて作成します。

##Railのコード
以下のようなRailを作ります。

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

name: Rail

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

jointType: fixed

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

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軸方向に移動させるため、以下のように記述します。

translation: [ 0, 0, 0.02 ]

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

geometry: { type: Box, size: [5, 0.1, 0.04] }

と記述します。sizeの隣には、[x軸寸法　y軸寸法　z軸寸法]の順に寸法を入力します。
次に色ですが、色の指定の前に

appearande: &Rail_appearance

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

diffuseColor: [ 0.8, 0.8, 0.8 ]

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

specularColor: [ 0.6, 0.6, 0.6 ]

はRGBの鏡面反射係数の値です。値を0～1の間で指定します。

shininess: 0.6

は光沢度を指定します。値を0～1の間で指定します。

Cartのコード

以下のようなCartを作ります。

Cartの運動拘束・物理パラメータの記述

名前を以下のように指定します。

name: Cart

Cartの親をRailにしたいので、

parent: Rail

と記述します。

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

jointType: prismatic
jointId: 0
jointAxis: X

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

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

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移動させます。

translation: [ 0, 0, 0.065 ]

Cartの形状・寸法・色の記述

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

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を作ります。

Pendulumの運動拘束・物理パラメータの記述

名前を以下のように記述します。

name: Pendulum

cartをpendulumの親とします。

parent: Cart

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

translation: [ 0, 0, 0.09 ]

pendulumはy軸周りに回転してほしいので、

jointType: revolute
jointId: 1
jointAxis: Y

と記述します。このrevoluteで、回転運動を指定することができます。

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

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 ]

と記述して位置を決めます。

type: Transform

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

translation: [ 0, 0, 0.32065 ]

と記述します。
それから棒を図のように建てた姿勢にするため、

rotation: [ 1, 0, 0, 90 ]

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

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

elements:
   Shape:
      geometry: { type: Cylinder, height: 0.6413, radius: 0.01 }

と記述して棒の形と寸法を決めます。

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

appearance: *Pendulum_appearance

と記述します。

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

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

#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)

最初の宣言

以下のコードは最初の宣言です。

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

using namespace cnoid;

線形状態フィードバックゲインベクトル

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

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

左から順に、台車変位、振子角度、台車速度、振子角速度のフィードバックゲインです。
これらの値は最適レギュレータ理論で決めました。詳しい決め方はchoreonoidの使い方を解説するという趣旨からずれるため、割愛します。

リンクと変数の定義

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

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

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

前半２行

Link* joint_c;
Link* joint_p;

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

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

は、順番に

台車変位
台車速度
振子角度
振子角速度
力
を格納するための変数です。

リンクの初期設定

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

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を使って

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

joint_p = io->body()->link("Pendulum");

と書くことで、BodyファイルのCart、Pendulumをリンクとして扱えるようにします。
それから、

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

io->enableInput(joint_p, JOINT_ANGLE | JOINT_VELOCITY);

と書くことで、Cartから

台車変位
台車速度
を、Pendulumからは
振子角度
振子角速度
を取得できます。

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

joint_c->setActuationMode(Link::JOINT_FORCE);
io->enableOutput(joint_c)

と記述し、振子は自由に動いてほしいので

joint_p->setActuationMode(Link::NO_ACTUATION);

と書きます。

状態変数の取得と制御入力の決定

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

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;
}

前半の

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;

で、台車に制御入力を加えます。

コントロールプログラムのコンパイル手順

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

コントロールプログラムの保存
Bodyファイルを、
\home\username\ダウンロード\choreonoid1.7.0\ext
上の
Inverted_pendulum
という名前のフォルダに保存したと思います。同じフォルダ上にコントロールプログラムをcart_type_controller.cppの名前で保存します。
テキストファイル作成
同フォルダにCMakeLists.txtを作り、そこに、
add-cnoid-simple-controller(cart_type_controller cart_type_controller.cpp)
と書いて保存します。
端末の起動・フォルダ移動・コマンドの実行
Ubuuntuの端末を起動し、cdコマンドを使って以下のフォルダに移動します。
\home\username\ダウンロード\choreonoid1.7.0
フォルダへ移動したら、cmake→makeとコマンドを実行します。
soファイルの生成
コンパイルが正しく実行されれば、
\home\username\ダウンロード\choreonoid1.7.0\simplecontroller
のフォルダ内に、
cart_type_controller.so
ができています。

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

choreonodの起動とワールドの作成
choreonoidを起動し、左上のタブの
「ファイル」→「新規」→「ワールド」
と進み、名前を付けて「作成」を押します。
ここでは名前をInverted_pendulumとしましょう。
「floor」と「Inverted_pendulum」のBodyファイル読み込み
floorと先ほど作成したInverted_pendulumのモデルを読み込みます。
floorは、
「ファイル」→「読み込み」→「ボディ」
と進み、choreonoidフォルダに移動して
\model\miscフォルダ内のfloor.bodyを読み込みます。このBodyファイルはextフォルダ内の「Inverted_pendulum」フォルダ内にコピーしておいてもいいでしょう。

次に倒立振子のモデルを読み込みます。floorと同じく
「ファイル」→「読み込み」→「ボディ」
と進み、choreonoidフォルダに移動し、
\ext\Inverted_pendulumと進み、「Cart_type_inverted_pendulum.body」を読み込みます。
コントロールプログラムの読み込み準備
コントロールプログラムを読み込ませる準備をします。左のウィンドウで倒立振子を選択状態（名前にオレンジ色が表示される状態）にし、左上の
「新規」→「シンプルコントローラ」
と進みます。名前は「stable_controller」としましょう。左のウィンドウの「Cart_type_Inverted_pendulum」の下に「stable_controller」ができます。
コントロールプログラム読み込み
コントローラプログラムを読み込みます。「stable_controller」をダブルクリックし、それから下ウィンドウの「コントローラモジュール」右隣の空欄をダブルクリックします。するとフォルダのマークが出てくるので、そのフォルダマークをクリックします。フォルダマーククリックするとsoファイルの選択画面が出るので、
choreonoidフォルダに移動→simplecontroller
と進み、コンパイルで作成した「Cart_type_controller.so」を選択して読み込みます。
AISTシミュレータの読み込み
「Inverted_pendulum」を選択し、
「ファイル」→「新規」→「AISTシミュレータ」
を選択し、「作成」を押します。
シミュレーション時間の設定
デフォルトではシミュレーション時間が無限時間となっているので、有限時間にしましょう。
AISTSimulatorをクリックし、その真下ウィンドウの、
時間範囲：指定時間
時間長：10.00
と設定を変えます。
シミュレーションの実行
倒立振子の安定化シミュレーションをやってみましょう。右のアニメーションウィンドウに表示されている倒立振子をマウスで動かし、右上の矢印マークの現在位置からのシミュレーションを開始を押します。すると安定化シミュレーションが実行されます。
せっかく作ったので、このシミュレータを保存しましょう。「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に投稿してみました。

間違い
表現の不備
改善コメント
質問
等がございましたら、是非ともご指摘お願いします。

You get articles that match your needs
You can efficiently read back useful information
You can use dark theme

What you can do with signing up