Object Factory
A robot model is handled using the class dynamicRobot, and it is build using the following components:
A humanoid robot (humanoidDynamicRobot) is a particular robot which has in addition the following components:
All those components are created using an object factory. The class ObjectFactory provides such a factory.
Building a humanoid robot
Creating an empty humanoid robot is therefore very simple:
Hooks for to create objects.
Definition: dynamicsfactory.hh:62
CjrlHumanoidDynamicRobot * createHumanoidDynamicRobot()
\subsection subsecbj Building a revolute joint
The same way a joint can be created by specifying the pose of the joint when the robot is at state . Therefore if the first joint is at the origin of the reference frame, the following code snippet can be used:
j1 = anObjectFactory->createJointRotation(pose);
struct jrlMathTools::Matrix4x4< double > matrix4d
#define MAL_S4x4_MATRIX_SET_IDENTITY(name)
IMPORTANT: It is assumed that the revolute joint axis is aligned with the -axis of its own frame. This normalization is very similar to the one of Davit-Hartenberg, and is compatible with KineoWorks. The position of the joint and the kinematic tree specified below allow the library to derive all the needed parameters automatically.
Building a body
The same way a body can be build
l1= anObjectFactory->createBody();
The body mass is specified with:
l1->
mass (m_SetOfParameters.m[0]);
virtual double mass() const=0
The inertia matrix can be specified using:
#define MAL_S3x3_MATRIX_SET_IDENTITY(name)
jrlMathTools::Matrix3x3< double > matrix3d
virtual const matrix3d & inertiaMatrix() const=0
Finally the center of mass for the body l1 is written:
lc(0) = 0.0;
lc(1) = 0.0;
lc(2) = 0.25;
virtual const vector3d & localCenterOfMass() const=0
A body is related with the joint which is the parent of all the other joints on the body. It is specified with:
virtual void setLinkedBody(CjrlBody &inBody)=0
Specifying the kinematic tree
The library handles robots as kinematic trees. Therefore each joint has a father or is the root of the robot kinematic tree.
Specifying the root
To specify the root of the kinematic for the humanoid robot created above:
virtual void rootJoint(CjrlJoint &inJoint)=0
Adding a child in the kinematic chain
A joint is set as the son of another joint with:
j2 = anObjectFactory->createJointRotation(pose);
virtual bool addChildJoint(CjrlJoint &inJoint)=0
Initialization
In order to compute quantities, it is important to initialize the internal structure of the library by calling the following method:
virtual bool initialize()=0