Create 3 types of translation joints

  - JointTranslation is now a template class with size_type as a template	    argument. The template argument represent the dimension of the joint
    configuration.

include/hpp/model/fwd.hh

@@ -40,12 +40,29 @@ namespace hpp {
     HPP_PREDEF_CLASS (JointAnchor);
     HPP_PREDEF_CLASS (JointRotation);
     HPP_PREDEF_CLASS (JointSO3);
-    HPP_PREDEF_CLASS (JointTranslation);
     HPP_PREDEF_CLASS (JointConfiguration);
     HPP_PREDEF_CLASS (ObjectFactory);
     HPP_PREDEF_CLASS (ObjectIterator);
     HPP_PREDEF_CLASS (Gripper);
     enum Request_t {COLLISION, DISTANCE};
+
+    typedef double value_type;
+    typedef Eigen::Matrix <double, Eigen::Dynamic, 1> vector_t;
+    typedef vector_t Configuration_t;
+    typedef Eigen::Ref <const Configuration_t> ConfigurationIn_t;
+    typedef Eigen::Ref <Configuration_t> ConfigurationOut_t;
+    typedef Eigen::Ref <const vector_t> vectorIn_t;
+    typedef Eigen::Ref <vector_t> vectorOut_t;
+    typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
+    typedef Eigen::Ref <matrix_t> matrixOut_t;
+    typedef matrix_t::Index size_type;
+    typedef fcl::Matrix3f matrix3_t;
+    typedef fcl::Vec3f vector3_t;
+    typedef Eigen::Matrix <double, 6, Eigen::Dynamic> JointJacobian_t;
+    typedef Eigen::Matrix <double, 3, Eigen::Dynamic> ComJacobian_t;
+    typedef Eigen::Block <JointJacobian_t, 3, Eigen::Dynamic>
+    HalfJointJacobian_t;
+
     typedef Body* BodyPtr_t;
     typedef std::vector<Body*> BodyVector_t;
     typedef boost::shared_ptr <CollisionObject> CollisionObjectPtr_t;
@@ -58,32 +75,21 @@ namespace hpp {
     typedef JointAnchor* JointAnchorPtr_t;
     typedef JointRotation* JointRotationPtr_t;
     typedef JointSO3* JointSO3Ptr_t;
-    typedef JointTranslation* JointTranslationPtr_t;
+    template <size_type dimension = 1> class  JointTranslation;
+    typedef JointTranslation <1>* JointTranslationPtr_t;
+    typedef JointTranslation <2>* JointTranslation2Ptr_t;
+    typedef JointTranslation <3>* JointTranslation3Ptr_t;
     typedef const Joint* JointConstPtr_t;
     typedef const JointAnchor* JointAnchorConstPtr_t;
     typedef const JointRotation* JointRotationConstPtr_t;
     typedef const JointSO3* JointSO3ConstPtr_t;
-    typedef const JointTranslation * JointTranslationConstPtr_t;
+    typedef const JointTranslation <1>* JointTranslationConstPtr_t;
+    typedef const JointTranslation <2>* JointTranslation2ConstPtr_t;
+    typedef const JointTranslation <3>* JointTranslation3ConstPtr_t;
     typedef std::map <std::string, JointPtr_t> JointByName_t;
-    typedef std::vector <JointPtr_t> JointVector_t;    
+    typedef std::vector <JointPtr_t> JointVector_t;
     typedef boost::shared_ptr <Gripper> GripperPtr_t;
     typedef std::vector <GripperPtr_t> Grippers_t;
-    typedef double value_type;
-    typedef Eigen::Matrix <double, Eigen::Dynamic, 1> vector_t;
-    typedef vector_t Configuration_t;
-    typedef Eigen::Ref <const Configuration_t> ConfigurationIn_t;
-    typedef Eigen::Ref <Configuration_t> ConfigurationOut_t;
-    typedef Eigen::Ref <const vector_t> vectorIn_t;
-    typedef Eigen::Ref <vector_t> vectorOut_t;
-    typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
-    typedef Eigen::Ref <matrix_t> matrixOut_t;
-    typedef matrix_t::Index size_type;
-    typedef fcl::Matrix3f matrix3_t;
-    typedef fcl::Vec3f vector3_t;
-    typedef Eigen::Matrix <double, 6, Eigen::Dynamic> JointJacobian_t;
-    typedef Eigen::Matrix <double, 3, Eigen::Dynamic> ComJacobian_t;
-    typedef Eigen::Block <JointJacobian_t, 3, Eigen::Dynamic>
-    HalfJointJacobian_t;
     typedef fcl::Transform3f Transform3f;
   } // namespace model
 } // namespace hpp

include/hpp/model/joint-configuration.hh

@@ -290,6 +290,7 @@ namespace hpp {
     }; // class RotationJointConfig
 
     /// Configuration of a JointTranslation
+    template <size_type dimension>
     class HPP_MODEL_DLLAPI TranslationJointConfig : public JointConfiguration
     {
     public:

include/hpp/model/joint.hh

@@ -31,7 +31,7 @@ namespace hpp {
     ///
     /// A joint maps an input vector to a transformation of SE(3) from the
     /// parent frame to the joint frame.
-    /// 
+    ///
     /// The input vector is provided through the configuration vector of the
     /// robot the joint belongs to. The joint input vector is composed of the
     /// components of the robot configuration starting at
@@ -348,13 +348,13 @@ namespace hpp {
 
     /// Translation Joint
     ///
-    /// Map a length as one-dimensional input vector to a translation along a
-    /// fixed axis in parent frame.
+    /// Map a 1,2 or 3-dimensional input vector to a translation
+    template <size_type dimension>
     class HPP_MODEL_DLLAPI JointTranslation : public Joint
     {
     public:
       JointTranslation (const Transform3f& initialPosition);
-      JointTranslation (const JointTranslation& joint);
+      JointTranslation (const JointTranslation <dimension>& joint);
       /// Return pointer to copy of this
       /// Clone body and therefore inner and outer objects (see Body::clone).
       virtual JointPtr_t clone () const;
@@ -371,7 +371,7 @@ namespace hpp {
       virtual void writeComSubjacobian (ComJacobian_t& jacobian,
 					const double& totalMass);
       mutable fcl::Vec3f t_;
-      mutable fcl::Vec3f axis_;
+      mutable fcl::Vec3f axis_ [dimension];
       mutable fcl::Vec3f com_;
     }; // class JointTranslation
 
@@ -382,5 +382,4 @@ namespace fcl {
   std::ostream& operator<< (std::ostream& os , const fcl::Transform3f& trans);
 }
 std::ostream& operator<< (std::ostream& os, const hpp::model::Joint& joint);
-
 #endif // HPP_MODEL_JOINT_HH

include/hpp/model/object-factory.hh

@@ -36,7 +36,7 @@ namespace hpp {
       ObjectFactory ()
       {}
       virtual ~ObjectFactory () {}
- 
+
       virtual DevicePtr_t createRobot (const std::string& name)
       {
 	return Device::create (name);
@@ -61,7 +61,17 @@ namespace hpp {
       virtual JointPtr_t createJointTranslation
       (const Transform3f& initialPosition)
       {
-	return new JointTranslation (initialPosition);
+	return new JointTranslation <1> (initialPosition);
+      }
+      virtual JointPtr_t createJointTranslation2
+      (const Transform3f& initialPosition)
+      {
+	return new JointTranslation <2> (initialPosition);
+      }
+      virtual JointPtr_t createJointTranslation3
+      (const Transform3f& initialPosition)
+      {
+	return new JointTranslation <3> (initialPosition);
       }
       virtual BodyPtr_t createBody ()
       {

src/joint-configuration.cc

@@ -99,12 +99,14 @@ namespace hpp {
     {
     }
 
-    TranslationJointConfig::TranslationJointConfig () :
-      JointConfiguration (1)
+    template <size_type dimension>
+    TranslationJointConfig <dimension>::TranslationJointConfig () :
+      JointConfiguration (dimension)
     {
     }
 
-    TranslationJointConfig::~TranslationJointConfig ()
+    template <size_type dimension>
+    TranslationJointConfig <dimension>::~TranslationJointConfig ()
     {
     }
 
@@ -251,61 +253,74 @@ namespace hpp {
       result [index+3] = sqrt (u1) * cos(2*M_PI*u3);
     }
 
-    void TranslationJointConfig::interpolate (ConfigurationIn_t q1,
-					      ConfigurationIn_t q2,
-					      const double& u,
-					      const size_type& index,
-					      ConfigurationOut_t result)
+    template <size_type dimension>
+    void TranslationJointConfig <dimension>::interpolate
+    (ConfigurationIn_t q1, ConfigurationIn_t q2, const double& u,
+     const size_type& index, ConfigurationOut_t result)
     {
-      result [index] = (1-u) * q1 [index] + u * q2 [index];
+      result.segment <dimension> (index) =
+	(1-u) * q1.segment <dimension> (index) +
+	u * q2.segment <dimension> (index);
     }
 
-    double TranslationJointConfig::distance (ConfigurationIn_t q1,
-					     ConfigurationIn_t q2,
-					     const size_type& index) const
+    template <size_type dimension>
+    double TranslationJointConfig <dimension>::distance
+    (ConfigurationIn_t q1, ConfigurationIn_t q2, const size_type& index) const
     {
-      return fabs (q2 [index] - q1 [index]);
+      if (dimension == 1) {
+	return fabs (q2 [index] - q1 [index]);
+      } else {
+	return (q2.segment <dimension> (index) -
+		q1.segment <dimension> (index)).norm ();
+      }
     }
 
-    void TranslationJointConfig::integrate (ConfigurationIn_t q,
-					    vectorIn_t v,
-					    const size_type& indexConfig,
-					    const size_type& indexVelocity,
-					    ConfigurationOut_t result) const
+    template <size_type dimension>
+    void TranslationJointConfig <dimension>::integrate
+    (ConfigurationIn_t q, vectorIn_t v, const size_type& indexConfig,
+     const size_type& indexVelocity, ConfigurationOut_t result) const
     {
       assert (indexConfig < result.size ());
-      result [indexConfig] = q [indexConfig] + v [indexVelocity];
-      if (isBounded (0)) {
-	if (result [indexConfig] < lowerBound (0)) {
-	  result [indexConfig] = lowerBound (0);
-	} else if (result [indexConfig] > upperBound (0)) {
-	  result [indexConfig] = upperBound (0);
+      result.segment <dimension> (indexConfig) =
+	q.segment <dimension> (indexConfig) +
+	v.segment <dimension> (indexVelocity);
+      for (unsigned int i=0; i<dimension; ++i) {
+	if (isBounded (i)) {
+	  if (result [indexConfig + i] < lowerBound (i)) {
+	    result [indexConfig + i] = lowerBound (i);
+	  } else if (result [indexConfig + i] > upperBound (i)) {
+	    result [indexConfig + i] = upperBound (i);
+	  }
 	}
       }
     }
 
-    void TranslationJointConfig::difference (ConfigurationIn_t q1,
-					     ConfigurationIn_t q2,
-					     const size_type& indexConfig,
-					     const size_type& indexVelocity,
-					     vectorOut_t result) const
+    template <size_type dimension>
+    void TranslationJointConfig <dimension>::difference
+    (ConfigurationIn_t q1, ConfigurationIn_t q2, const size_type& indexConfig,
+     const size_type& indexVelocity, vectorOut_t result) const
     {
-      result [indexVelocity] = q1 [indexConfig] - q2 [indexConfig];
+      result.segment <dimension> (indexVelocity) =
+	q1.segment <dimension> (indexConfig) -
+	q2.segment <dimension> (indexConfig);
     }
 
-    void TranslationJointConfig::uniformlySample (const size_type& index,
-						  ConfigurationOut_t result) const
+    template <size_type dimension>
+    void TranslationJointConfig <dimension>::uniformlySample
+    (const size_type& index, ConfigurationOut_t result) const
     {
-      if (!isBounded (0)) {
-	std::ostringstream iss
-	  ("Cannot uniformly sample non bounded translation degrees of "
-	   "freedom at rank ");
-	iss  << index;
-	throw std::runtime_error (iss.str ());
-      }
-      else {
-	result [index] = lowerBound (0) +
-	  (upperBound (0) - lowerBound (0)) * rand ()/RAND_MAX;
+      for (unsigned int i=0; i<dimension; ++i) {
+	if (!isBounded (i)) {
+	  std::ostringstream iss
+	    ("Cannot uniformly sample non bounded translation degrees of "
+	     "freedom at rank ");
+	  iss  << index + i;
+	  throw std::runtime_error (iss.str ());
+	}
+	else {
+	  result [index + i] = lowerBound (i) +
+	    (upperBound (i) - lowerBound (i)) * rand ()/RAND_MAX;
+	}
       }
     }
 
@@ -385,6 +400,9 @@ namespace hpp {
 	  (upperBound (0) - lowerBound (0)) * rand ()/RAND_MAX;
       }
     }
+    template class TranslationJointConfig <1>;
+    template class TranslationJointConfig <2>;
+    template class TranslationJointConfig <3>;
 
   } // namespace model
 } // namespace hpp

src/joint.cc

@@ -362,60 +362,6 @@ namespace hpp {
       }
     }
 
-    JointTranslation::JointTranslation (const Transform3f& initialPosition) :
-      Joint (initialPosition, 1, 1), t_ ()
-    {
-      configuration_ = new TranslationJointConfig;
-      t_.setValue (0);
-    }
-
-    JointTranslation::JointTranslation (const JointTranslation& joint) :
-      Joint (joint), t_ ()
-    {
-      t_.setValue (0);
-    }
-
-    JointPtr_t JointTranslation::clone () const
-    {
-      return new JointTranslation (*this);
-    }
-
-    JointTranslation::~JointTranslation ()
-    {
-      delete configuration_;
-    }
-
-    void JointTranslation::computePosition
-    (ConfigurationIn_t configuration, const Transform3f& parentPosition,
-     Transform3f& position) const
-    {
-      t_ [0] = configuration [rankInConfiguration ()];
-      T3f_.setTranslation (t_);
-      position = parentPosition * positionInParentFrame_ * T3f_;
-    }
-
-    void JointTranslation::writeSubJacobian (const JointPtr_t& child)
-    {
-      size_type col = rankInVelocity ();
-      // Get translation axis
-      axis_ = currentTransformation_.getRotation ().getColumn (0);
-      child->jacobian () (0, col) = axis_ [0];
-      child->jacobian () (1, col) = axis_ [1];
-      child->jacobian () (2, col) = axis_ [2];
-    }
-
-    void JointTranslation::writeComSubjacobian (ComJacobian_t& jacobian,
-						const double& totalMass)
-    {
-      if (mass_ > 0) {
-	size_type col = rankInVelocity ();
-	// Get translation axis
-	axis_ = currentTransformation_.getRotation ().getColumn (0);
-	jacobian (0, col) = (mass_/totalMass) * axis_ [0];
-	jacobian (1, col) = (mass_/totalMass) * axis_ [1];
-	jacobian (2, col) = (mass_/totalMass) * axis_ [2];
-      }
-    }
 
     std::ostream& displayTransform3f (std::ostream& os,
 				      const fcl::Transform3f trans)
@@ -479,7 +425,86 @@ namespace hpp {
       }
       return os;
     }
+    template <size_type dimension>
+    JointTranslation <dimension>::JointTranslation
+    (const Transform3f& initialPosition) : Joint (initialPosition, dimension,
+						  dimension), t_ ()
+    {
+      if (dimension > 3 || dimension ==0) {
+	throw std::runtime_error
+	  ("Dimension of translation should be between 1 and 3.");
+      }
+      configuration_ = new TranslationJointConfig <dimension>;
+      t_.setValue (0);
+    }
+
+    template <size_type dimension>
+    JointTranslation <dimension>::JointTranslation
+    (const JointTranslation <dimension>& joint) : Joint (joint), t_ ()
+    {
+      t_.setValue (0);
+    }
+
+    template <size_type dimension>
+    JointPtr_t JointTranslation <dimension>::clone () const
+    {
+      return new JointTranslation <dimension> (*this);
+    }
+
+    template <size_type dimension>
+    JointTranslation <dimension>::~JointTranslation ()
+    {
+      delete configuration_;
+    }
+
+    template <size_type dimension>
+    void JointTranslation <dimension>::computePosition
+    (ConfigurationIn_t configuration, const Transform3f& parentPosition,
+     Transform3f& position) const
+    {
+      t_ [0] = configuration [rankInConfiguration ()];
+      if (dimension >= 2) {
+	t_ [1] = configuration [rankInConfiguration () + 1];
+      }
+      if (dimension >= 3) {
+	t_ [2] = configuration [rankInConfiguration () + 2];
+      }
+      T3f_.setTranslation (t_);
+      position = parentPosition * positionInParentFrame_ * T3f_;
+    }
 
+    template <size_type dimension>
+    void JointTranslation <dimension>::writeSubJacobian
+    (const JointPtr_t& child)
+    {
+      size_type col = rankInVelocity ();
+      // Get translation axis
+      for (unsigned int i=0; i<dimension; ++i) {
+	axis_[i] = currentTransformation_.getRotation ().getColumn (i);
+	child->jacobian () (0, col+i) = axis_ [i][0];
+	child->jacobian () (1, col+i) = axis_ [i][1];
+	child->jacobian () (2, col+i) = axis_ [i][2];
+      }
+    }
+
+    template <size_type dimension>
+    void JointTranslation <dimension>::writeComSubjacobian
+    (ComJacobian_t& jacobian, const double& totalMass)
+    {
+      if (mass_ > 0) {
+	size_type col = rankInVelocity ();
+	// Get translation axis
+	for (unsigned int i=0; i<dimension; ++i) {
+	  axis_ [i] = currentTransformation_.getRotation ().getColumn (i);
+	  jacobian (0, col+i) = (mass_/totalMass) * axis_ [i][0];
+	  jacobian (1, col+i) = (mass_/totalMass) * axis_ [i][1];
+	  jacobian (2, col+i) = (mass_/totalMass) * axis_ [i][2];
+	}
+      }
+    }
+    template class JointTranslation <1>;
+    template class JointTranslation <2>;
+    template class JointTranslation <3>;
   } // namespace model
 } // namespace hpp
 
@@ -502,3 +527,4 @@ std::ostream& operator<< (std::ostream& os, const hpp::model::Joint& joint)
 {
   return joint.display (os);
 }
+