ロボット・ダイナミクス（単純モデル） v120330 v1.4.8

Up:Tutorial?　　　Previous：BVHファイルの読み込み?　　　　Next: ロボット・ダイナミクス（ヒューマノイド）　　

ロボット・ダイナミクス
- 単純モデル

ロボット・ダイナミクス †

ここでは関節を持ったオブジェクトの物理シミュレーションを行うサンプルを紹介します。(バージョン110418以降で正常に動作）

↑

単純モデル †

↑

モデルの作成 †

SIGVerseのロボットモデルはOpenHRP形式で記述されています。ここでは簡単なモデルを作成してみます。

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

$ cd ~/sigverse-<version>/share/sigverse/data/shape/
$ emacs robot_test.x3d

robot_test.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="JOINT1"  containerField='children'>
          <fieldValue name='name' value='JOINT1' />
          <fieldValue name='jointId' value='2.0' />
          
          <fieldValue name='jointType' value='fixed' />
 
          
          <ProtoInstance name='Segment' DEF="LINK1"     containerField='children'>
            <fieldValue name='name' value='LINK1' />
            <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="JOINT2"  containerField='children'>
            <fieldValue name='name' value='JOINT2' />
            
            <fieldValue name='translation' value='0.0 5.0 0.0' />
            
            <fieldValue name='jointType' value='rotate' />
            
            <fieldValue name='jointAxis' value='1.0 0.0 0.0' />
            <fieldValue name='jointId' value='3.0' />
            
            <ProtoInstance name='Segment' DEF="LINK2"   containerField='children'>
              <fieldValue name='name' value='LINK2' />
              
              <fieldValue name='centerOfMass' value='0.0 5.0 0.0' />
              <fieldValue name='mass' value='1.0' />
                <Transform>
                <Shape>
                  <Appearance>
                    <ImageTexture url="tumiki_moku.jpg"/>
                  </Appearance>
                  <IndexedFaceSet USE="link1"/>
                </Shape>
              </Transform>
            </ProtoInstance>
          </ProtoInstance>
        </ProtoInstance>
      </ProtoInstance>
    </ProtoInstance>
  </Scene>
 </X3D>

OpenHRP形式のロボットはプロト宣言部と実モデル定義部からなります。実モデル定義部でロボットを組み立てます。OpenHRP形式のモデルの作成方法は詳しくはOpenHRP3 モデル作成法を参照ください。

このサンプルは2つの棒を関節で接続したモデルです。

同じ形状ファイルをクライアント側のSIGViewerのインストール先に置きます。

※形状ファイルにコメント部分があるとクライアント側で正常に読み込まれない場合があるので、クライアント側ではコメントを削除するようにしてください。

↑

Jointの親子関係 †

作成したモデルのJointの親子関係は以下のようになっています。

   Humanoid sample（モデルのルート）
    + Joint JOINT0 (ルートジョイント、3軸における位置、回転の6つの自由度をもつ)
     + Joint JOINT1 :Segment LINK1 (固定ジョイント、自由度0)
       + Joint JOINT2 :Segment LINK2(ヒンジジョイント、自由度1）

Jointの回転や移動に伴い、そのJointに付随するSegmentや子ジョイントが回転および移動を行います。このモデルではLINK1とLINK2がJOINT2によって接続されています。

↑

設定ファイル †

まずエージェント設定ファイルを作成します。

$ cd ../xml
$ emacs robot_test.xml

robot_test.xml

次に世界ファイルを作成します。

$ emacs DynamicsWorld.xml

DynamicsWorld.xml

ダイナミクスをtrueに設定しました。

↑

実行 †

まずはコントローラなしで実行してみましょう。

$ cd ~/sigverse-<version>/bin
$ ./sigserver.sh -p 9001 -w DynamicsWorld.xml

viewerで接続し、シミュレーションを開始してみると初めは棒がまっすぐ立っていますが、しばらくすると重力の影響で関節が曲がって倒れます。

↑

コントローラ †

次に関節にトルクを加えるコントローラを作成します。

$ cd NewWorld
$ emacs DynamicsController.cpp

DynamicsController.cpp

addJointTorqueでヒンジジョイントにトルクを加えます。最初の引数で関節名、2番目の引数で加えるトルク[Nm]を指定します。このコントローラでは500[Nm]のトルクを"JOINT2"に1秒おきに加えます。

Makefileを修正してコンパイルします。

$ emacs Makefile

オブジェクトファイルDynamicsController.soを指定

#オブジェクトファイルの指定
OBJS     = DynamicsController.so

$ make

↑

世界ファイル修正 †

次に作成したコントローラを世界ファイルで指定します。

$ cd ..
$ emacs xml/DynamicsWorld.xml

以下の一行を加えます。

        <set-attr-value name="implementation" value="./NewWorld/DynamicsController.so"/>

DynamicsWorld.xml

↑

実行 †

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

$ ./sigserver.sh -p 9001 -w DynamicsWorld.xml

viewerで見てみると先ほどは重力の影響でゆっくり関節が曲がっていましたが、今回はトルクを加えたため、シミュレーション開始と同時に関節が勢いよく曲がります。トルクは定期的にくわえられていくため、しばらくすると棒の動きの激しさが増します。

↑

関節の角速度の設定 †

先ほどはコントローラで関節にトルクを加えました。次は関節の角速度を設定します。

$ cd NewWorld
$ emacs DynamicsController.cpp

先ほどトルクを加えた部分を以下のように修正します。

DynamicsController.cpp

　　　　↓

setJointVelocityで関節の角速度を設定します。最初の引数は関節名、2番目の引数は角速度[rad/s]、3番目の引数は指定した角速度に到達するまでに加える最大トルク[Nm]を指定します。

最大トルクに大きい値を指定すると指定した角速度に早く到達します。

それではコンパイルして実行してみましょう。

$ make
$ cd ..
$ ./sigserver.sh -p 9001 -w DynamicsWorld.xml

棒が地面に倒れても関節は一定の速度で回転し続けようとします。

Up:Tutorial?　　　Previous：BVHファイルの読み込み?　　　　Next: ロボット・ダイナミクス（ヒューマノイド）　　

ロボット・ダイナミクス（単純モデル） v120330 v1.4.8

recent(20)