車両の制御（動力学有り）

Up:Tutorial?　　　Previous：車輪移動ロボット　　　　　Next: 距離センサ　　　

車両のダイナミクス
- ダンベルモデル

車両のダイナミクス †

前節では，動力学計算をしない状態での車両の制御を説明しましたが，ここでは車両の物理シミュレーションを行うサンプルを紹介します。

↑

ダンベルモデル †

↑

モデルの作成 †

まずロボットの形状モデルなどが置かれたディレクトリに移動し、以下のファイルを作成します。

$ cd ~/sigverse-<version>/share/sigverse/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
$ emacs DumbelWorld.xml

ダイナミクスをtrueにしています。

↑

コントローラ †

次に車輪を回転させるコントローラを作成します。

$ emacs 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(
 			(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

↑

実行 †

それでは実行してみましょう。

$ ./sigserver.sh -w DumbelWorld.xml

viewerで見てみると、定期的にダンベルが前後に動くことが確認できます。