Store position and name of link in joint frame.

include/hpp/model/urdf/parser.hh

@@ -186,17 +186,20 @@ namespace hpp
 				DevicePtr_t robot);
 
 	/// \brief Create rotation joint and add it to joints map.
-	JointPtr_t createRotationJoint (const std::string& name,
-					  const MatrixHomogeneousType& mat,
-					  const UrdfJointLimitsPtrType& limits);
+	JointPtr_t createRotationJoint
+	(const std::string& name, const MatrixHomogeneousType& mat,
+	 const MatrixHomogeneousType& urdfLinkInJoint,
+	 const UrdfJointLimitsPtrType& limits);
 
 	/// \brief Create continuous joint and add it to joints map.
-	JointPtr_t createContinuousJoint (const std::string& name,
-					    const MatrixHomogeneousType& mat);
+	JointPtr_t createContinuousJoint
+	(const std::string& name, const MatrixHomogeneousType& urdfLinkInJoint,
+	 const MatrixHomogeneousType& mat);
 
 	/// \brief Create translation joint and add it to joints map.
 	JointPtr_t createTranslationJoint
 	(const std::string& name, const MatrixHomogeneousType& mat,
+	 const MatrixHomogeneousType& urdfLinkInJoint,
 	 const UrdfJointLimitsPtrType& limits);
 
 	/// \brief Create anchor joint and add it to joints map.

src/urdf/parser.cc

@@ -396,23 +396,28 @@ namespace hpp
 
 	  // Normalize orientation if this is an actuated joint.
 	  UrdfJointConstPtrType joint = model_.getJoint (it->first);
+	  Transform3f urdfLinkInJoint;
 	  if (joint->type == ::urdf::Joint::REVOLUTE
 	      || joint->type == ::urdf::Joint::CONTINUOUS
-	      || joint->type == ::urdf::Joint::PRISMATIC)
-	    position = position * normalizeFrameOrientation (joint);
+	      || joint->type == ::urdf::Joint::PRISMATIC) {
+	    Transform3f jointInUrdfLink = normalizeFrameOrientation (joint);
+	    urdfLinkInJoint = inverse (jointInUrdfLink);
+	    position = position * jointInUrdfLink;
+	  }
 
 	  switch(it->second->type) {
 	  case ::urdf::Joint::UNKNOWN:
 	    throw std::runtime_error ("Joint has UNKNOWN type");
 	    break;
 	  case ::urdf::Joint::REVOLUTE:
-	    createRotationJoint (it->first, position, it->second->limits);
+	    createRotationJoint (it->first, position, urdfLinkInJoint,
+				 it->second->limits);
 	    break;
 	  case ::urdf::Joint::CONTINUOUS:
-	    createContinuousJoint (it->first, position);
+	    createContinuousJoint (it->first, position, urdfLinkInJoint);
 	    break;
 	  case ::urdf::Joint::PRISMATIC:
-	    createTranslationJoint (it->first, position,
+	    createTranslationJoint (it->first, position, urdfLinkInJoint,
 				    it->second->limits);
 	    break;
 	  case ::urdf::Joint::FLOATING:
@@ -542,6 +547,7 @@ namespace hpp
 
 	  // Link dynamic body to dynamic joint.
 	  it->second->setLinkedBody (body);
+	  it->second->linkName (link->name);
 	  hppDout (info,  "Linking body " << body->name () << " to joint "
 		   << it->second->name ());
 
@@ -905,6 +911,7 @@ namespace hpp
       JointPtr_t
       Parser::createRotationJoint (const std::string& name,
 				   const MatrixHomogeneousType& mat,
+				   const MatrixHomogeneousType& urdfLinkInJoint,
 				   const Parser::UrdfJointLimitsPtrType& limits)
       {
 	JointPtr_t joint;
@@ -915,6 +922,7 @@ namespace hpp
 
 	joint = objectFactory_.createBoundedJointRotation (mat);
 	joint->name (name);
+	joint->linkInJointFrame (urdfLinkInJoint);
 	if (limits) {
 	  joint->isBounded (0, true);
 	  joint->lowerBound (0, limits->lower);
@@ -924,9 +932,9 @@ namespace hpp
 	return joint;
       }
 
-      JointPtr_t
-      Parser::createContinuousJoint (const std::string& name,
-				     const MatrixHomogeneousType& mat)
+      JointPtr_t Parser::createContinuousJoint
+      (const std::string& name, const MatrixHomogeneousType& mat,
+       const MatrixHomogeneousType& urdfLinkInJoint)
       {
 	JointPtr_t joint;
 	if (jointsMap_.find (name) != jointsMap_.end ()) {
@@ -936,15 +944,15 @@ namespace hpp
 
 	joint = objectFactory_.createUnBoundedJointRotation (mat);
 	joint->name (name);
+	joint->linkInJointFrame (urdfLinkInJoint);
 	jointsMap_[name] = joint;
 	return joint;
       }
 
-      JointPtr_t
-      Parser::createTranslationJoint (const std::string& name,
-				      const MatrixHomogeneousType& mat,
-				      const Parser::
-				      UrdfJointLimitsPtrType& limits)
+      JointPtr_t Parser::createTranslationJoint
+      (const std::string& name, const MatrixHomogeneousType& mat,
+       const MatrixHomogeneousType& urdfLinkInJoint,
+       const Parser::UrdfJointLimitsPtrType& limits)
       {
 	JointPtr_t joint;
 	if (jointsMap_.find (name) != jointsMap_.end ()) {
@@ -954,6 +962,7 @@ namespace hpp
 
 	joint = objectFactory_.createJointTranslation (mat);
 	joint->name (name);
+	joint->linkInJointFrame (urdfLinkInJoint);
 	if (limits) {
 	  joint->isBounded (0, true);
 	  joint->lowerBound (0, limits->lower);