Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

Implement DynamicRobot::initialize method.

  * This method is called once the kinematic tree has been built.
  * The joint vector is first built by exploring the tree.
  * Joint vector is then used to build the rbdl model; the model must
    correspond exactly with the kinematic tree.
  * Fixed joints still need to be treated properly.
  • Loading branch information...
commit 9c319d15a9b2e86ead30347a325d61df42e1db3e 1 parent 357a2a3
Antonio El Khoury authored
Showing with 98 additions and 1 deletion.
  1. +17 −0 include/ard/rbdl/model/dynamic-robot.hh
  2. +81 −1 src/dynamic-robot.cc
View
17 include/ard/rbdl/model/dynamic-robot.hh
@@ -316,6 +316,23 @@ namespace ard
/// \}
protected:
+ /// \brief Build joint vector by exploring tree starting from
+ /// a specified joint (usually the root joint).
+ ///
+ /// Result can be retrieved with associated getter. It contains
+ /// all joints, including fixed ones.
+ virtual bool buildJointVector (const jointShPtr_t& joint);
+
+ /// \brief Build rbdl model attribute.
+ ///
+ /// This method builds the rbdl model from the joint vector.
+ ///
+ /// \warning Make sure joint vector is built before calling this
+ /// method.
+ ///
+ /// \retval true if success, false if failure
+ virtual bool buildRbdlModel ();
+
/// \brief Set the root joint of the robot.
virtual void rootJoint (joint_t& joint);
View
82 src/dynamic-robot.cc
@@ -87,7 +87,18 @@ namespace ard
bool DynamicRobot::initialize ()
{
- throw std::runtime_error ("Method not supported.");
+ // Build joint vector.
+ if (!buildJointVector (rootJoint_))
+ return false;
+
+ // Build rbdl model.
+ if (!buildRbdlModel ())
+ return false;
+
+ // Initialize rbdl model.
+ rbdlModel_.Init ();
+
+ return true;
}
void DynamicRobot::rootJoint (CjrlJoint& joint)
@@ -402,6 +413,75 @@ namespace ard
}
}
+ bool DynamicRobot::buildJointVector (const jointShPtr_t& joint)
+ {
+ jointVector_.push_back (jointWkPtr_t (joint));
+ BOOST_FOREACH (jointShPtr_t childJoint, joint->childJoints ())
+ buildJointVector (childJoint);
+
+ return true;
+ }
+
+ bool DynamicRobot::buildRbdlModel ()
+ {
+ jointShPtrs_t jointVector;
+ this->jointVector (jointVector);
+
+ BOOST_FOREACH (jointShPtr_t joint, jointVector)
+ {
+ // Retrieve rbdl joint.
+ assert (!!joint && "Null pointer to joint.");
+ rbdlJoint_t rbdlJoint = joint->rbdlJoint ();
+
+ // Retrieve rbdl parent body.
+ jointShPtr_t parentJoint;
+ joint->parentJoint (parentJoint);
+ assert (!!parentJoint && "Null pointer to parent joint.");
+ bodyShPtr_t parentBody;
+ parentJoint->linkedBody (parentBody);
+
+ // Root joint does not have a parent, skip.
+ if (parentBody)
+ {
+ // Retrieve parent body id.
+ std::string parentBodyName = parentBody->getName ();
+ unsigned int parentId
+ = rbdlModel_.GetBodyId (parentBodyName.c_str ());
+
+ // Compute parent joint transformation in joint frame.
+ matrix4d world_T_joint = joint->initialPosition ();
+ matrix4d world_T_pJoint = parentJoint->initialPosition ();
+ matrix4d joint_T_world = world_T_joint.inverse ();
+ matrix4d joint_T_pJoint = joint_T_world * world_T_pJoint;
+ matrix3d E = joint_T_pJoint.block<3,3> (0,0);
+ vector3d r = - joint_T_pJoint.block<3,1> (0,3);
+ rbdlSpatialTransform_t joint_X_pjoint (E, r);
+
+ // Retrieve rbdl body.
+ bodyShPtr_t body;
+ joint->linkedBody (body);
+ assert (!!body && "Null pointer to body.");
+ rbdlBody_t rbdlBody = body->rbdlBody ();
+
+ // Retrieve body name.
+ // FIXME: Add name getter and setter to abstract interface
+ // for body class later.
+ std::string bodyName = body->getName ();
+
+ // Attach body and joint to rbdl model attribute.
+ rbdlModel_.AddBody (parentId,
+ joint_X_pjoint,
+ rbdlJoint,
+ rbdlBody,
+ bodyName);
+ }
+ else
+ continue;
+ }
+
+ return true;
+ }
+
void DynamicRobot::rootJoint (joint_t& joint)
{
rootJoint_ = joint.shared_from_this ();
Please sign in to comment.
Something went wrong with that request. Please try again.