![[PukiWiki]](image/Concept_logo.jpg "[PukiWiki]"){kind=logo}
Up:[[Tutorial]] Previous：[[BVHファイルの読み込み]] Next: [[ロボット・ダイナミクス（ヒューマノイド）]] #contents *ロボット・ダイナミクス [#j7cc4426] ※このサンプルはv121029, v2.0.1には対応しておりません。 ※このサンプルはv121029, v2.0.1以降には対応しておりません。 ここでは関節を持ったオブジェクトの物理シミュレーションを行うサンプルを紹介します。(バージョン110418以降で正常に動作） **単純モデル [#f84e822a] ***モデルの作成 [#q02438d0] SIGVerseのロボットモデルはOpenHRP形式で記述されています。ここでは簡単なモデルを作成してみます。 まずロボットの形状モデルなどが置かれたディレクトリに移動し、以下のファイルを作成します。 $ cd ~/sigverse-<version>/share/sigverse/data/shape/ $ 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]]を参照ください。 このサンプルは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] まずエージェント設定ファイルを作成します。 $ cd ../xml $ 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] まずはコントローラなしで実行してみましょう。 $ cd ~/sigverse-<version>/bin $ ./sigserver.sh -p 9001 -w DynamicsWorld.xml viewerで接続し、シミュレーションを開始してみると初めは棒がまっすぐ立っていますが、しばらくすると重力の影響で関節が曲がって倒れます。 #ref(./bou_1.PNG,40%) ***コントローラ [#u4ff78dd] 次に関節にトルクを加えるコントローラを作成します。 $ cd NewWorld $ 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秒間)。 Makefileを修正してコンパイルします。 $ emacs Makefile オブジェクトファイルDynamicsController.soを指定 #オブジェクトファイルの指定 OBJS = DynamicsController.so $ make ***世界ファイル修正 [#ua9e7301] 次に作成したコントローラを世界ファイルで指定します。 $ cd .. $ emacs xml/DynamicsWorld.xml 以下の一行を加えます。 <set-attr-value name="implementation" value="./NewWorld/DynamicsController.so"/> DynamicsWorld.xml #highlight(xml){{ <?xml version="1.0" encoding="utf8"?> <world name="myworld"> <gravity x="0.0" y="-9.8" 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="5.0"/> <set-attr-value name="z" value="5.0"/> <set-attr-value name="language" value="c++"/> <set-attr-value name="implementation" value="./NewWorld/DynamicsController.so"/> </instanciate> </world> }} ***実行 [#hf6ef7e6] それでは実行してみましょう。 $ ./sigserver.sh -p 9001 -w DynamicsWorld.xml viewerで見てみると先ほどは重力の影響でゆっくり関節が曲がっていましたが、今回はトルクを加えたため、シミュレーション開始と同時に関節が勢いよく曲がります。地面でのたうちまわります。 #ref(./bou_2.PNG,40%) ***関節の角速度の設定 [#oa8d06b1] 先ほどはコントローラで関節にトルクを加えました。次は関節の角速度を設定します。 $ cd NewWorld $ 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]を指定します。 最大トルクに大きい値を指定すると指定した角速度に早く到達します。 それではコンパイルして実行してみましょう。 $ make $ cd .. $ ./sigserver.sh -p 9001 -w DynamicsWorld.xml #ref(./bou_3.PNG,40%) 棒が地面に倒れても関節は一定の速度で回転し続けようとします。 #highlight(end) *Old Version [#m1bdd74e] -[[ロボット・ダイナミクス（単純モデル） v120330 v1.4.8]] Up:[[Tutorial]] Previous：[[BVHファイルの読み込み]] Next: [[ロボット・ダイナミクス（ヒューマノイド）]]
Site admin: inamura
PukiWiki 1.5.0 Copyright © 2001-2006 PukiWiki Developers Team. License is GPL.