Up:Tutorial? Previous:ロボット・ダイナミクス(ヒューマノイド) Next: 距離センサ? 車両のダイナミクス †ここでは車輪を持ったオブジェクトの物理シミュレーションを行うサンプルを紹介します。 ダンベルモデル †モデルの作成 †まずロボットの形状モデルなどが置かれたディレクトリに移動し、以下のファイルを作成します。 $ cd ~/sigverse-<version>/share/data/shape/ $ emacs Dumbel.x3d <?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の親子関係 †作成したモデルの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つのシリンダが回転することによって車輪運動が実現されます。 設定ファイル †エージェント設定ファイルと世界ファイルを作成します。 $ cd ../xml $ emacs DumbelTest.xml 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にしています。 コントローラ †次に車輪を回転させるコントローラを作成します。 $ cd NewWorld $ emacs DumbelController.cpp DumbelController.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( "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, "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] Makefileを修正してコンパイルします。 $ emacs Makefile オブジェクトファイルDumbelController.soを指定 #オブジェクトファイルの指定 OBJS = DumbelController.so $ make 実行 †それでは実行してみましょう。 $ ./sigserver.sh -p 9001 -w DumbelWorld.xml viewerで見てみると、定期的にダンベルが前後に動くことが確認できます。 Up:Tutorial? Previous:ロボット・ダイナミクス(ヒューマノイド) Next: 距離センサ? |