![[PukiWiki]](image/Concept_logo.jpg "[PukiWiki]"){kind=logo}
[[Tutorial]]
#contents
*Robot Dynamics [#la446dbf]
SIGVerse using OpenDynamicsEngine (ODE) to model the dynamics of the robot. The shapes that are used for such dynamics calculation are modeled using the OpenHRP format in SIGVerse. So, if your requirement is to use some shapes or robots only for the case when dynamics of the entity is OFF, then you may not need this tutorial. But, if your work requires usage of dynamics properties like force, torque etc, then this tutorial is important for you.
This tutorial shows how to model simple objects using the OpenHRP format so that dynamics calculations can be performed using ODE.
**Simple Model [#yc108ce2]
*** Creating a model [#o8f6b967]
To begin with, please create a directory and copy the x3d file given below into your editor and save it.
We shall then discuss its contents.
$ 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>
}}
*** Layout of the model [#e89cc0fd]
The Basic layout of the file is as FOLLOWS:
Header:
PROTO declaration part (Declare the PROTO Structure)
The rest:
Real-model definition part (Declare the instances that uses PROTO)
In OpenHRP format, a model is created by combining the instances of PROTO nodes.
There are three types of PROTO nodes:
--1.Joint node
--2.Segment node
--3.Humanoid node
The JOINT node specifies the link structure.
The Segment node defines the Link shape.
The Humanoid Node is the root node of the model.
For more information about OpenHRP format and modeling, please refer their [[website.>http://www.openrtp.jp/openhrp3/3.0.2/jp/create_model.html]]
The parent-child relationship of the model we have created, looks like the following:
Humanoid sample（the root node of the model）
+ Joint JOINT0 (the root joint has six degree of freedom)
+ Joint JOINT1
Segment LINK1 ( its fixed joint i.e. zero degrees of freedom )
+ Joint JOINT2
Segment LINK2(hinge joint, single degrees of freedom)
With the rotation and movement of a Joint, the segment and the child nodes associated with also undergo the rotation and movement.
In this example, LINK1 is the segment of JOINT1 while JOINT2 is its child node.
#ref(./bou_2.jpg)
*** Making of Configuration and World file [#afca224a]
Lets create the configuration file to read the model we created above.
$ emacs robot_test.xml
robot_test.xml
#highlight(xml){{
<?xml version="1.0" encoding="utf8"?>
<define-class name="robot_test" inherit="Agent.xml">
<x3d>
<!--Reading the model you just created-->
<filename>robot_test.x3d</filename>
</x3d>
</define-class>
}}
Lets create the world model to use the configuration file of the model.
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>
}}
The dynamics of the robot model is set as "TRUE"
*** Upload the World File to SIGViewer [#y3a4127e]
Let's run without any controller first. Just to see how it looks like:
$ sigserver.sh -w ./DynamicsWorld.xml
You can see the bar If you look in connection with the SIGViewer.
Then click the bar by clicking the "Entity Data" in the tray at the bottom right. You can check the location information of the parts and joints.
#ref(./bou_4.PNG,40%)
--Red dot indicates the position of the entity. Position of entity means, the position of root joint i.e. JOINT0.
--Blue is the position of the JOINT2.
--Green is the position of the part (Segment).
#ref(./bou_1.PNG,40%)
Bar stands straight in the beginning, but when we start the simulation, its joints collapse and bend under the influence of gravity after a while.
*** Controller [#yb37abf3]
Let's create a controller for applying a torque to the joint.
$ 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());
//add torque on JOINT2
my->addJointTorque("JOINT2",50000.0);
return 0.1;
}
extern "C" Controller * createController() {
return new MoveController;
}
}}
Here we apply torque to the hinge joint using the SIGVerse API function addJointTorque. Specify torque using name of the joint as first argument, the second argument in the first [N · cm]. This controller and make every one second to "JOINT2" a torque of 500 [Nm] (0.01 seconds).
Lets compile the controller.
$ ./sigmake.sh DynamicsController.cpp
*** World file modification [#hcb3d322]
Please add the executable in the xml world file.
<set-attr-value name="implementation" value="./DynamicsController.so"/>
Thus, the world file shall look like below:
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>
}}
*** Uploading the world file to SIGViewer [#h7ccf63e]
Its time to run.
$ sigserver.sh -w ./DynamicsWorld.xml
When you start the simulation, you will notice that the Joint bents slowly, under the influence of gravity. Butsince the addition of the torque, the joint will bend like a brick at the same time as the start of the simulation this time.
#ref(./bou_2.PNG,40%)
*** Setting of the angular velocity of the joint [#lc87ba4b]
We added a torque to the joint on the controller earlier. The following sets the angular velocity of the joint.
Please replace the function to torque to joints with a function to add velocity to the joints.
DynamicsController.cpp
#highlight(cpp:firstline[13]){{
my->addJointTorque("JOINT2",50000.0);
}}
↓
#highlight(cpp:firstline[13]){{
my->setJointVelocity("JOINT2", 1.57, 50000.0);
}}
We set the angular velocity of the joint using the API function setJointVelocity. The function takes joint names as the first argument, the second specifies the angular velocity [rad / s]and third specifies the maximum torque [N · cm] which is added to reach the angular velocity to its specified value.
You will reach quickly to the angular velocity that you specified when you specify a value greater to maximum torque.
It is now time to compile and run.
$ ./sigmake.sh DynamicsController.cpp
$ sigserver.sh -w ./DynamicsWorld.xml
#ref(./bou_3.PNG,40%)
Joint continues to rotate at a constant speed after it falls over the ground.
#highlight(end)
#counter
Site admin: inamura
PukiWiki 1.5.0 Copyright © 2001-2006 PukiWiki Developers Team. License is GPL.