![[PukiWiki]](image/Concept_logo.jpg "[PukiWiki]"){kind=logo}
Up:[[Tutorial]] Previous：[[車輪移動ロボット]] Next: [[距離センサ]]
----
#contents
*車両のダイナミクス [#j7cc4426]
前節では，動力学計算をしない状態での車両の制御を説明しましたが，ここでは車両の物理シミュレーションを行うサンプルを紹介します。
**ダンベルモデル [#f84e822a]
***モデルの作成 [#q02438d0]
まずロボットの形状モデルなどが置かれたディレクトリに移動し、以下のファイルを作成します。
$ cd ~/sigverse-<version>/share/sigverse/data/shape/
$ emacs Dumbel.x3d
#highlight(cpp){{
<?xml version="1.0" encoding="UTF-8"?>
<X3D profile="Immersive" version="3.0">
<Scene>
<ProtoDeclare name='Joint' >
<ProtoInterface>
<field accessType='inputOutput' type='SFVec3f' name='center' value='0.0 0.0 0.0' />
<field accessType='inputOutput' type='MFNode' name='children' />
<field accessType='inputOutput' type='MFFloat' name='llimit' />
<field accessType='inputOutput' type='SFRotation' name='limitOrientation' value='0.0 0.0 1.0 0.0' />
<field accessType='inputOutput' type='SFString' name='name' value='' />
<field accessType='inputOutput' type='SFRotation' name='rotation' value='0.0 0.0 1.0 0.0' />
<field accessType='inputOutput' type='SFVec3f' name='scale' value='1.0 1.0 1.0' />
<field accessType='inputOutput' type='SFRotation' name='scaleOrientation' value='0.0 0.0 1.0 0.0' />
<field accessType='inputOutput' type='MFFloat' name='stiffness' value='0.0 ,0.0 ,0.0' />
<field accessType='inputOutput' type='SFVec3f' name='translation' value='0.0 0.0 0.0' />
<field accessType='inputOutput' type='MFFloat' name='ulimit' />
<field accessType='inputOutput' type='MFFloat' name='dh' value='0.0 ,0.0 ,0.0 ,0.0' />
<field accessType='inputOutput' type='SFString' name='jointType' value='' />
<field accessType='inputOutput' type='SFFloat' name='jointId' value='-1.0' />
<field accessType='inputOutput' type='SFVec3f' name='jointAxis' value='1.0 0.0 0.0' />
</ProtoInterface>
<ProtoBody>
<Transform>
<IS>
<connect nodeField='children' protoField='children' />
<connect nodeField='center' protoField='center' />
<connect nodeField='rotation' protoField='rotation' />
<connect nodeField='scale' protoField='scale' />
<connect nodeField='scaleOrientation' protoField='scaleOrientation' />
<connect nodeField='translation' protoField='translation' />
</IS>
</Transform>
</ProtoBody>
</ProtoDeclare>
<ProtoDeclare name='Segment' >
<ProtoInterface>
<field accessType='initializeOnly' type='SFVec3f' name='bboxCenter' value='0.0 0.0 0.0' />
<field accessType='initializeOnly' type='SFVec3f' name='bboxSize' value='-1.0 -1.0 -1.0' />
<field accessType='inputOutput' type='SFVec3f' name='centerOfMass' value='0.0 0.0 0.0' />
<field accessType='inputOutput' type='MFNode' name='children' />
<field accessType='inputOutput' type='SFNode' name='coord' />
<field accessType='inputOutput' type='MFNode' name='displacers' />
<field accessType='inputOutput' type='SFFloat' name='mass' value='0.0' />
<field accessType='inputOutput' type='MFFloat' name='momentsOfInertia' value='0.0 ,0.0 ,0.0 ,0.0 ,0.0 ,0.0 ,0.0 ,0.0 ,0.0' />
<field accessType='inputOutput' type='SFString' name='name' value='' />
<field accessType='inputOnly' type='MFNode' name='addChildren' />
<field accessType='inputOnly' type='MFNode' name='removeChildren' />
</ProtoInterface>
<ProtoBody>
<Group>
<IS>
<connect nodeField='children' protoField='children' />
<connect nodeField='addChildren' protoField='addChildren' />
<connect nodeField='removeChildren' protoField='removeChildren' />
<connect nodeField='bboxCenter' protoField='bboxCenter' />
<connect nodeField='bboxSize' protoField='bboxSize' />
</IS>
</Group>
</ProtoBody>
</ProtoDeclare>
<ProtoDeclare name='Humanoid' >
<ProtoInterface>
<field accessType='initializeOnly' type='SFVec3f' name='bboxCenter' value='0.0 0.0 0.0' />
<field accessType='initializeOnly' type='SFVec3f' name='bboxSize' value='-1.0 -1.0 -1.0' />
<field accessType='inputOutput' type='SFVec3f' name='center' value='0.0 0.0 0.0' />
<field accessType='inputOutput' type='MFNode' name='humanoidBody' />
<field accessType='inputOutput' type='MFString' name='info' />
<field accessType='inputOutput' type='MFNode' name='joints' />
<field accessType='inputOutput' type='SFString' name='name' value='' />
<field accessType='inputOutput' type='SFRotation' name='rotation' value='0.0 0.0 1.0 0.0' />
<field accessType='inputOutput' type='SFVec3f' name='scale' value='1.0 1.0 1.0' />
<field accessType='inputOutput' type='SFRotation' name='scaleOrientation' value='0.0 0.0 1.0 0.0' />
<field accessType='inputOutput' type='MFNode' name='segments' />
<field accessType='inputOutput' type='MFNode' name='sites' />
<field accessType='inputOutput' type='SFVec3f' name='translation' value='0.0 0.0 0.0' />
<field accessType='inputOutput' type='SFString' name='version' value='1.0' />
<field accessType='inputOutput' type='MFNode' name='viewpoints' />
</ProtoInterface>
<ProtoBody>
<Transform>
<IS>
<connect nodeField='bboxCenter' protoField='bboxCenter' />
<connect nodeField='bboxSize' protoField='bboxSize' />
<connect nodeField='center' protoField='center' />
<connect nodeField='rotation' protoField='rotation' />
<connect nodeField='scale' protoField='scale' />
<connect nodeField='scaleOrientation' protoField='scaleOrientation' />
<connect nodeField='translation' protoField='translation' />
</IS>
<Group>
<IS>
<connect nodeField='children' protoField='viewpoints' />
</IS>
</Group>
<Group>
<IS>
<connect nodeField='children' protoField='humanoidBody' />
</IS>
</Group>
</Transform>
</ProtoBody>
</ProtoDeclare>
<ProtoInstance name='Humanoid' DEF="HRP1" containerField='children'>
<fieldValue name='info' value='"Test Joint"' />
<fieldValue name='name' value='sample' />
<fieldValue name='version' value='1.1' />
<ProtoInstance name='Joint' DEF="JOINT0" containerField='humanoidBody'>
<fieldValue name='name' value='JOINT0' />
<fieldValue name='jointType' value='free' />
<fieldValue name='jointId' value='1.0' />
<ProtoInstance name='Joint' DEF="JOINT_CENTER" containerField='children'>
<fieldValue name='name' value='JOINT_CENTER' />
<fieldValue name='jointId' value='2.0' />
<fieldValue name='jointType' value='fixed' />
<ProtoInstance name='Segment' DEF="LINK_CENTER" containerField='children'>
<fieldValue name='name' value='LINK_CENTER' />
<fieldValue name='mass' value='1.0' />
<Transform>
<Shape>
<Appearance>
<ImageTexture url="tumiki_moku.jpg"/>
</Appearance>
<IndexedFaceSet DEF="link1" coordIndex="0 3 2 1 -1 4 5 6 7 -1 4 0 1 5 -1 5 1 2 6 -1 6 2 3 7 -1 7 3 0 4 -1" texCoordIndex="0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1" solid="false">
<TextureCoordinate point="0.0 0.0, 0.0 1.0, 1.0 1.0, 1.0 0.0," />
<Coordinate DEF="Coaster_c01GeoPoints" point="-1.0 0.0 -1.0, 1.0 0.0 -1.0, 1.0 0.0 1.0, -1.0 0.0 1.0, -1.0 10.0 -1.0, 1.0 10.0 -1.0, 1.0 10.0 1.0, -1.0 10.0 1.0," />
</IndexedFaceSet>
</Shape>
</Transform>
</ProtoInstance>
<ProtoInstance name='Joint' DEF="JOINT_LWHEEL" containerField='children'>
<fieldValue name='name' value='JOINT_LWHEEL' />
<fieldValue name='translation' value='0.0 5.0 0.0' />
<fieldValue name='jointAxis' value='1.0 0.0 0.0' />
<fieldValue name='jointId' value='2.0' />
<fieldValue name='jointType' value='rotate' />
<ProtoInstance name='Segment' DEF="LINK_LWHEEL" containerField='children'>
<fieldValue name='name' value='LINK_LWHEEL' />
<fieldValue name='mass' value='1.0' />
<Transform>
<Shape>
<Appearance>
<ImageTexture url="book_A.jpg"/>
</Appearance>
<IndexedFaceSet DEF="pCylinderShape1_ifs" coordIndex=" 0 1 21 20 -1 1 2 22 21 -1 2 3 23 22 -1 3 4 24 23 -1 4 5 25 24 -1 5 6 26 25 -1 6 7 27 26 -1 7 8 28 27 -1 8 9 29 28 -1 9 10 30 29 -1 10 11 31 30 -1 11 12 32 31 -1 12 13 33 32 -1 13 14 34 33 -1 14 15 35 34 -1 15 16 36 35 -1 16 17 37 36 -1 17 18 38 37 -1 18 19 39 38 -1 19 0 20 39 -1 1 0 40 -1 2 1 40 -1 3 2 40 -1 4 3 40 -1 5 4 40 -1 6 5 40 -1 7 6 40 -1 8 7 40 -1 9 8 40 -1 10 9 40 -1 11 10 40 -1 12 11 40 -1 13 12 40 -1 14 13 40 -1 15 14 40 -1 16 15 40 -1 17 16 40 -1 18 17 40 -1 19 18 40 -1 0 19 40 -1 20 21 41 -1 21 22 41 -1 22 23 41 -1 23 24 41 -1 24 25 41 -1 25 26 41 -1 26 27 41 -1 27 28 41 -1 28 29 41 -1 29 30 41 -1 30 31 41 -1 31 32 41 -1 32 33 41 -1 33 34 41 -1 34 35 41 -1 35 36 41 -1 36 37 41 -1 37 38 41 -1 38 39 41 -1 39 20 41 -1" texCoordIndex=" 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 3 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1 0 1 2 -1" solid="false" normalIndex=" 0 1 1 0 -1 1 2 2 1 -1 2 3 3 2 -1 3 4 4 3 -1 4 5 5 4 -1 5 6 6 5 -1 6 7 7 6 -1 7 8 8 7 -1 8 9 9 8 -1 9 10 10 9 -1 10 11 11 10 -1 11 12 12 11 -1 12 13 13 12 -1 13 14 14 13 -1 14 15 15 14 -1 15 16 16 15 -1 16 17 17 16 -1 17 18 18 17 -1 18 19 19 18 -1 19 0 0 19 -1 20 20 21 -1 22 20 21 -1 23 22 21 -1 24 23 21 -1 20 24 21 -1 20 20 21 -1 25 20 21 -1 26 25 21 -1 20 26 21 -1 20 20 21 -1 25 20 21 -1 25 25 21 -1 27 25 21 -1 27 27 21 -1 28 27 21 -1 29 28 21 -1 30 29 21 -1 20 30 21 -1 20 20 21 -1 20 20 21 -1 31 31 32 -1 31 33 32 -1 33 33 32 -1 33 34 32 -1 34 35 32 -1 35 36 32 -1 36 31 32 -1 31 31 32 -1 31 31 32 -1 31 37 32 -1 37 37 32 -1 37 31 32 -1 31 31 32 -1 31 38 32 -1 38 39 32 -1 39 40 32 -1 40 41 32 -1 41 31 32 -1 31 31 32 -1 31 31 32 -1">
<TextureCoordinate point="0.0 0.0, 0.0 1.0, 1.0 1.0, 1.0 0.0," />
<Coordinate DEF="pCylinderShape1_coord" point="5.68363 -1.27612 -1.84672, 4.83479 -1.27612 -3.51268, 3.51268 -1.27612 -4.83479, 1.84672 -1.27612 -5.68363, 4.221E-8 -1.27612 -5.97613, -1.84672 -1.27612 -5.68363, -3.51268 -1.27612 -4.83479, -4.83479 -1.27612 -3.51268, -5.68363 -1.27612 -1.84672, -5.97613 -1.27612 -1.0152E-7, -5.68363 -1.27612 1.84672, -4.83479 -1.27612 3.51268, -3.51268 -1.27612 4.83479, -1.84672 -1.27612 5.68363, 5.9312E-8 -1.27612 5.97612, 1.84672 -1.27612 5.68363, 3.51268 -1.27612 4.83479, 4.83479 -1.27612 3.51268, 5.68363 -1.27612 1.84672, 5.97612 -1.27612 0.0, 5.68363 1.27612 -1.84672, 4.83479 1.27612 -3.51268, 3.51268 1.27612 -4.83479, 1.84672 1.27612 -5.68363, 4.221E-8 1.27612 -5.97613, -1.84672 1.27612 -5.68363, -3.51268 1.27612 -4.83479, -4.83479 1.27612 -3.51268, -5.68363 1.27612 -1.84672, -5.97613 1.27612 -1.0152E-7, -5.68363 1.27612 1.84672, -4.83479 1.27612 3.51268, -3.51268 1.27612 4.83479, -1.84672 1.27612 5.68363, 5.9312E-8 1.27612 5.97612, 1.84672 1.27612 5.68363, 3.51268 1.27612 4.83479, 4.83479 1.27612 3.51268, 5.68363 1.27612 1.84672, 5.97612 1.27612 0.0, 0.0 -1.27612 0.0, 0.0 1.27612 0.0, "/>
<Normal DEF="pCylinderShape1_normal" vector="0.95106 0.0 -0.30902, 0.80902 0.0 -0.58779, 0.58779 0.0 -0.80902, 0.30902 0.0 -0.95106, -1.0561E-7 0.0 -1.0, -0.30902 0.0 -0.95106, -0.58779 0.0 -0.80902, -0.80902 0.0 -0.58779, -0.95106 0.0 -0.30902, -1.0 0.0 -1.5087E-8, -0.95106 0.0 0.30902, -0.80902 0.0 0.58779, -0.58779 0.0 0.80902, -0.30902 0.0 0.95106, 9.8065E-8 0.0 1.0, 0.30902 0.0 0.95106, 0.58779 0.0 0.80902, 0.80902 0.0 0.58779, 0.95106 0.0 0.30902, 1.0 0.0 7.8452E-7, 0.0 -1.0 0.0, 0.0 -1.0 1.1976E-8, 0.0 -1.0 2.1603E-8, 0.0 -1.0 2.6484E-8, 0.0 -1.0 4.8808E-9, 0.0 -1.0 4.3206E-8, 0.0 -1.0 4.3206E-8, 0.0 -1.0 6.8583E-9, 0.0 -1.0 4.3206E-8, 0.0 -1.0 0.0, 0.0 -1.0 -4.3206E-8, 0.0 1.0 0.0, 0.0 1.0 -5.4945E-9, 0.0 1.0 4.3206E-8, 0.0 1.0 -4.8808E-9, 0.0 1.0 -2.6484E-8, 0.0 1.0 -2.1603E-8, 0.0 1.0 -4.3206E-8, 0.0 1.0 -6.8583E-9, 0.0 1.0 1.4745E-8, 0.0 1.0 -2.1603E-8, 0.0 1.0 -4.3206E-8, "/>
</IndexedFaceSet>
</Shape>
</Transform>
</ProtoInstance>
</ProtoInstance>
<ProtoInstance name='Joint' DEF="JOINT_RWHEEL" containerField='children'>
<fieldValue name='name' value='JOINT_RWHEEL' />
<fieldValue name='translation' value='0.0 -5.0 0.0' />
<fieldValue name='jointAxis' value='1.0 0.0 0.0' />
<fieldValue name='jointId' value='2.0' />
<fieldValue name='jointType' value='rotate' />
<ProtoInstance name='Segment' DEF="LINK_RWHEEL" containerField='children'>
<fieldValue name='name' value='LINK_RWHEEL' />
<fieldValue name='mass' value='1.0' />
<Transform>
<Shape>
<Appearance>
<ImageTexture url="book_A.jpg"/>
</Appearance>
<IndexedFaceSet USE="pCylinderShape1_ifs" />
</Shape>
</Transform>
</ProtoInstance>
</ProtoInstance>
</ProtoInstance>
</ProtoInstance>
</ProtoInstance>
</Scene>
</X3D>
}}
このサンプルは2つのシリンダを関節で接続したモデルです。
同じ形状ファイルをクライアント側のSIGViewerのインストール先に置きます。
***Jointの親子関係 [#fed078fe]
作成したモデルのJointの親子関係は以下のようになっています。
Humanoid sample（モデルのルート）
+ Joint JOINT0 (ルートジョイント、3軸における位置、回転の6つの自由度をもつ)
+ Joint JOINT_CENTER :Segment LINK_CENTER (固定ジョイント、自由度0)
+ Joint JOINT_LWHEEL :Segment LINK_LWHEEL(ヒンジジョイント、自由度1）
+ Joint JOINT_RWHEEL :Segment LINK_RWHEEL(ヒンジジョイント、自由度1）
このモデルではLINK_LWHEELがJOINT_LWHEELによって接続され、JOINT_RWHEELがLINK_RWHEEL
によって接続されています。
LINK_LWHEELとLINK_RWHEELはそれぞれシリンダ形状となっていて、2つのシリンダが回転することによって車輪運動が実現されます。
***設定ファイル [#e3db8144]
エージェント設定ファイルと世界ファイルを作成します。
$ cd ../xml
$ emacs DumbelTest.xml
$ emacs DumbelWorld.xml
#highlight(cpp){{
DumbelTest.xml
<?xml version="1.0" encoding="utf8"?>
<define-class name="robot_test" inherit="Agent.xml">
<x3d>
<filename>dumbel.x3d</filename>
</x3d>
</define-class>
$ emacs DumbelWorld.xml
DumbelWorld.xml
<?xml version="1.0" encoding="utf8"?>
<world name="myworld">
<gravity x="0.0" y="-9.8" z="0.0"/>
<instanciate class="DumbelTest.xml">
<set-attr-value name="name" value="Robot1"/>
<set-attr-value name="dynamics" value="true"/>
<set-attr-value name="x" value="5.0"/>
<set-attr-value name="y" value="4.5"/>
<set-attr-value name="z" value="5.0"/>
<set-attr-value name="qw" value="0.707"/>
<set-attr-value name="qx" value="0.0"/>
<set-attr-value name="qy" value="0.0"/>
<set-attr-value name="qz" value="0.707"/>
<set-attr-value name="language" value="c++"/>
<set-attr-value name="implementation" value="./NewWorld/DumbelController.so"/>
</instanciate>
</world>
}}
ダイナミクスをtrueにしています。
***コントローラ [#u4ff78dd]
次に車輪を回転させるコントローラを作成します。
$ emacs DumbelController.cpp
#highlight(cpp){{
#include "Controller.h"
/**
* 車両モデルのサンプル
*/
class DumbelController : public Controller {
public:
void onInit(InitEvent &evt);
double onAction(ActionEvent&);
private:
int time;
int state;
enum {
STATE_INIT = 0,
STATE_STEP1,
STATE_STEP2,
STATE_STEP3,
STATE_STEP4
};
enum {
TIME_0 = 0,
TIME_1 = 15,
TIME_2 = 30
};
};
void DumbelController::onInit(InitEvent &evt) {
SimObj *my = getObj(myname());
state = STATE_INIT;
time = 0;
}
double DumbelController::onAction(ActionEvent &evt) {
SimObj *my = getObj(myname());
time++;
switch(state) {
case STATE_INIT:
setWheelProperty(
(char*)"LINK_LWHEEL", // char *leftWheelName,
0.0, // double leftMotorConsumption,
1000.0, // double leftWheelMaxSpeed,
0.0001, // double leftWheelSpeedUnit,
0.0, // double leftSlipNoise,
0.0, // double leftEncoderResolution,
100.0, // double leftMaxForce,
(char*)"LINK_RWHEEL", // char *rightWheelName,
0.0, // double rightMotorConsumption,
1000.0, // double rightWheelMaxSpeed,
0.0001, // double rightWheelSpeedUnit,
0.0, // double rightSlipNoise,
0.0, // double rightEncoderResolution,
100.0 // double rightMaxForce
);
state = STATE_STEP1;
break;
case STATE_STEP1:
differentialWheelsSetSpeed(5.0,5.0);
state = STATE_STEP2;
break;
case STATE_STEP2:
if(time > TIME_1) {
state = STATE_STEP3;
}
break;
case STATE_STEP3:
// 逆方向に回してみる
differentialWheelsSetSpeed(-5.0,-5.0);
state = STATE_STEP4;
break;
case STATE_STEP4:
if(time > TIME_2) {
state = STATE_STEP1;
time = 0.0;
}
break;
}
return 1.0;
}
extern "C" Controller * createController() {
return new DumbelController;
}
}}
setWheelProperty()で車両の特性を設定します。各引数の意味は下記の通りとなっています。
第1引数 : 左側の車輪のリンクオブジェクト名
第2引数 : 左側の車輪の消費電力[W]（2011/4/12時点未対応）
第3引数 : 左車輪の最大角速度[rad/s]
第4引数 : 速度の精度[rad/s]
第5引数 : シミュレーションステップごとに加えられる一様分布に従うノイズ
第6引数 : 1rad回転するごとに加えられるノイズの回数
第7引数 : 左側車輪に掛けられる最大トルク[Nm]
第8引数 : 右側の車輪のリンクオブジェクト名
第9引数 : 右側の車輪の消費電力[W]（2011/4/12時点未対応）
第10引数 : 右車輪の最大角速度[rad/s]
第11引数 : 速度の精度[rad/s]
第12引数 : シミュレーションステップごとに加えられる一様分布に従うノイズ
第13引数 : 1rad回転するごとに加えられるノイズの回数
第14引数 : 右側車輪に掛けられる最大トルク[Nm]
コンパイルします。
$ ./sigmake.sh DumbelController.cpp
***実行 [#hf6ef7e6]
それでは実行してみましょう。
$ ./sigserver.sh -w DumbelWorld.xml
viewerで見てみると、定期的にダンベルが前後に動くことが確認できます。
#highlight(end)
----
Up:[[Tutorial]] Previous：[[車輪移動ロボット]] Next: [[距離センサ]]
#counter
Site admin: inamura
PukiWiki 1.5.0 Copyright © 2001-2006 PukiWiki Developers Team. License is GPL.