Diff of ロボット・ダイナミクス（単純モデル）

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

----
#contents

*ロボット・ダイナミクス [#j7cc4426]
※このチュートリアルはSIGVerse v2.1.0以降で動作します。

ここでは関節を持ったオブジェクトの物理シミュレーションを行うサンプルを紹介します。
**単純モデル [#f84e822a]
***モデルの作成 [#q02438d0]
SIGVerseのロボットモデルはOpenHRP形式で記述されています。ここでは簡単なモデルを作成してみます。

まずSIGVerse作業ディレクトリに移動し、以下のファイルを作成します。
 $ cd ~/MyWorld
 $ emacs robot_test.x3d

robot_test.x3d

#highlight(xml){{
<?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='0.5' />
	    <Transform>
	      <Shape>
		<Box size="2 10 2"/>
		<Appearance>
		  <Material  diffuseColor='0.6 0.6 0.1' />
		</Appearance>
	      </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='0.5' />
	      <Transform>
		<Shape>
		  <Box size="2 10 2"/>
		  <Appearance>
		    <Material  diffuseColor='0.6 0.6 0.1' />
		  </Appearance>
		</Shape>
	      </Transform>
	    </ProtoInstance>
	  </ProtoInstance>
	</ProtoInstance>
      </ProtoInstance>
    </ProtoInstance>
  </Scene>
</X3D>
}}
OpenHRP形式のロボットはプロト宣言部と実モデル定義部からなります。実モデル定義部でロボットを組み立てます。OpenHRP形式のモデルの作成方法は詳しくは[[OpenHRP3 モデル作成法>http://www.openrtp.jp/openhrp3/3.0.2/jp/create_model.html]]を参照ください。

OpenHRP形式のロボットはプロト宣言部と実モデル定義部からなります。実モデル定義部でロボットを組み立てます。OpenHRP形式のモデルの作成方法は詳しくは[[OpenHRP3 モデル作成法>http://fkanehiro.github.io/openhrp3-doc/jp/create_model.html]]を参照ください。

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


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

    Humanoid sample（モデルのルート）
     + Joint JOINT0 (ルートジョイント、3軸における位置、回転の6つの自由度をもつ)
      + Joint JOINT1 :Segment LINK1 (固定ジョイント、自由度0)
        + Joint JOINT2 :Segment LINK2(ヒンジジョイント、自由度1）
Jointの回転や移動に伴い、そのJointに付随するSegmentや子ジョイントが回転および移動を行います。
このモデルではLINK1とLINK2がJOINT2によって接続されています。
#ref(ロボット・ダイナミクス（単純モデル） v120330 v1.4.8/bou_2.jpg)
***設定ファイル [#e3db8144]
まずエージェント設定ファイルを作成します。
 $ emacs robot_test.xml

robot_test.xml

#highlight(xml){{
<?xml version="1.0" encoding="utf8"?>
<define-class name="robot_test" inherit="Agent.xml">

  <x3d>
    <!--先ほど作成したモデルの読み込み-->
    <filename>robot_test.x3d</filename>
  </x3d>

</define-class>

}}
次に世界ファイルを作成します。
 $ emacs DynamicsWorld.xml

DynamicsWorld.xml
#highlight(xml){{
<?xml version="1.0" encoding="utf8"?>
<world name="myworld11">

  <gravity x="0.0" y="-980.7" z="0.0"/>

  <instanciate class="robot_test.xml">
    <set-attr-value name="name" value="robot_test"/>

    <!--dynamics true-->
    <set-attr-value name="dynamics" value="true"/>

    <set-attr-value name="x" value="5.0"/>
    <set-attr-value name="y" value="10.0"/>
    <set-attr-value name="z" value="5.0"/>

    <set-attr-value name="language" value="c++"/>
    <set-attr-value name="collision" value="true"/>
  </instanciate>
</world>
}}
ダイナミクスをtrueに設定しました。
***実行 [#l4c076b6]
まずはコントローラなしで実行してみましょう。
 $ sigserver.sh -w ./DynamicsWorld.xml

ビューワーで接続して見ると棒が見えます。

次に右下のトレイにあるEntity Dataをクリックして棒をクリックします。
関節やパーツの位置情報を確認することができます。

#ref(./bou_4.PNG,40%)

赤色のドットで示されているのがエンティティの位置になります。
エンティティの位置はルートジョイントの位置となります。
青色が関節の位置、緑色がパーツ(Segment）の位置となります。

シミュレーションを開始してみると初めは棒がまっすぐ立っていますが、しばらくすると重力の影響で関節が曲がって倒れます。
#ref(./bou_1.PNG,40%)

***コントローラ [#u4ff78dd]
次に関節にトルクを加えるコントローラを作成します。
 $ emacs DynamicsController.cpp

DynamicsController.cpp

#highlight(cpp){{
#include "Controller.h"

class MoveController : public Controller {
public:
  double onAction(ActionEvent&);
};

double MoveController::onAction(ActionEvent &evt) {
  SimObj *my = getObj(myname());

  //JOINT2にトルクを加えます。                                                                                                     
  my->addJointTorque("JOINT2",50000.0);
  return 0.1;
}

extern "C" Controller * createController() {
  return new MoveController;
}
}}
addJointTorqueでヒンジジョイントにトルクを加えます。最初の引数で関節名、2番目の引数で加えるトルク[N・cm]を指定します。
このコントローラでは500[Nm]のトルクを"JOINT2"に1秒おきに加えます(0.01秒間)。


コンパイルします。

 $ ./sigmake.sh DynamicsController.cpp


***世界ファイル修正 [#ua9e7301]
次に作成したコントローラを世界ファイルで指定します。
 $ emacs DynamicsWorld.xml

以下の一行を加えます。
         <set-attr-value name="implementation" value="./DynamicsController.so"/>

DynamicsWorld.xml
#highlight(xml){{
<?xml version="1.0" encoding="utf8"?>
<world name="myworld11">

  <gravity x="0.0" y="-980.7" z="0.0"/>

  <instanciate class="robot_test.xml">
    <set-attr-value name="name" value="robot_test"/>

    <!--dynamics true-->
    <set-attr-value name="dynamics" value="true"/>

    <set-attr-value name="x" value="5.0"/>
    <set-attr-value name="y" value="10.0"/>
    <set-attr-value name="z" value="5.0"/>

    <set-attr-value name="language" value="c++"/>
    <set-attr-value name="implementation" value="./DynamicsController.so"/>

    <set-attr-value name="collision" value="true"/>
  </instanciate>
</world>
}}
***実行 [#hf6ef7e6]
それでは実行してみましょう。

 $ sigserver.sh -w ./DynamicsWorld.xml

viewerで見てみると先ほどは重力の影響でゆっくり関節が曲がっていましたが、今回はトルクを加えたため、シミュレーション開始と同時に関節が勢いよく曲がります。地面でのたうちまわります。
#ref(./bou_2.PNG,40%)

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

 $ emacs DynamicsController.cpp

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

DynamicsController.cpp

#highlight(cpp:firstline[13]){{
   my->addJointTorque("JOINT2",50000.0);
}}
　　　　↓
#highlight(cpp:firstline[13]){{
  my->setJointVelocity("JOINT2", 1.57, 50000.0);
}}
setJointVelocityで関節の角速度を設定します。最初の引数は関節名、2番目の引数は角速度[rad/s]、3番目の引数は指定した角速度に到達するまでに加える最大トルク[N・cm]を指定します。

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

それではコンパイルして実行してみましょう。
 $ ./sigmake.sh DynamicsController.cpp
 $ sigserver.sh -w ./DynamicsWorld.xml

#ref(./bou_3.PNG,40%)
棒が地面に倒れても関節は一定の速度で回転し続けようとします。


#highlight(end)

*Old Version [#m1bdd74e]
-[[ロボット・ダイナミクス（単純モデル） v120330 v1.4.8]]

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

#counter