[Algo] Follow new API of LieGroup algos

src/algorithm/joint-configuration.hpp

@@ -19,6 +19,7 @@
 #define __se3_joint_configuration_hpp__
 
 #include <Eigen/Core>
+#include "pinocchio/macros.hpp"
 #include "pinocchio/multibody/fwd.hpp"
 
 namespace se3
@@ -32,10 +33,11 @@ namespace se3
    * @param[in]  v       Velocity (size model.nv)
    * @return     The integrated configuration (size model.nq)
    */
-  template<typename LieGroup_t>
-  inline Eigen::VectorXd integrate(const Model & model,
-                                   const Eigen::VectorXd & q,
-                                   const Eigen::VectorXd & v);
+  template<typename LieGroup_t, typename JointCollection, typename ConfigVectorType, typename TangentVectorType>
+  inline typename EIGEN_PLAIN_TYPE(ConfigVectorType)
+  integrate(const ModelTpl<JointCollection> & model,
+            const Eigen::MatrixBase<ConfigVectorType> & q,
+            const Eigen::MatrixBase<TangentVectorType> & v);
   /**
    * @brief      Interpolate the model between two configurations
    *
@@ -45,11 +47,12 @@ namespace se3
    * @param[in]  u       u in [0;1] position along the interpolation.
    * @return     The interpolated configuration (q0 if u = 0, q1 if u = 1)
    */
-  template<typename LieGroup_t>
-  inline Eigen::VectorXd interpolate(const Model & model,
-                                     const Eigen::VectorXd & q0,
-                                     const Eigen::VectorXd & q1,
-                                     const double u);
+  template<typename JointCollection, typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar>
+  inline typename EIGEN_PLAIN_TYPE(ConfigVectorIn1)
+  interpolate(const ModelTpl<JointCollection> & model,
+              const Eigen::MatrixBase<ConfigVectorIn1> & q0,
+              const Eigen::MatrixBase<ConfigVectorIn2> & q1,
+              const Scalar & u);
 
   /**
    * @brief      Compute the tangent vector that must be integrated during one unit time to go from q0 to q1
@@ -59,10 +62,11 @@ namespace se3
    * @param[in]  q1      Wished configuration (size model.nq)
    * @return     The corresponding velocity (size model.nv)
    */
-  template<typename LieGroup_t>
-  inline Eigen::VectorXd difference(const Model & model,
-                                    const Eigen::VectorXd & q0,
-                                    const Eigen::VectorXd & q1);
+  template<typename LieGroup_t, typename JointCollection, typename ConfigVectorIn1, typename ConfigVectorIn2>
+  inline typename EIGEN_PLAIN_TYPE(ConfigVectorIn1)
+  difference(const ModelTpl<JointCollection> & model,
+             const Eigen::MatrixBase<ConfigVectorIn1> & q0,
+             const Eigen::MatrixBase<ConfigVectorIn2> & q1);
 
 
   /**
@@ -73,10 +77,11 @@ namespace se3
    * @param[in]  q1         Configuration 1 (size model.nq)
    * @return     The corresponding squared distances for each joint (size model.njoints-1 = number of joints)
    */
-  template<typename LieGroup_t>
-  inline Eigen::VectorXd squaredDistance(const Model & model,
-                                         const Eigen::VectorXd & q0,
-                                         const Eigen::VectorXd & q1);
+  template<typename LieGroup_t,typename JointCollection, typename ConfigVectorIn1, typename ConfigVectorIn2>
+  inline typename EIGEN_PLAIN_TYPE(ConfigVectorIn1)
+  squaredDistance(const ModelTpl<JointCollection> & model,
+                  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
+                  const Eigen::MatrixBase<ConfigVectorIn2> & q1);
   /**
    * @brief      Distance between two configuration vectors
    *
@@ -85,11 +90,11 @@ namespace se3
    * @param[in]  q1         Configuration 1 (size model.nq)
    * @return     The distance between the two configurations
    */
-  template<typename LieGroup_t>
-  inline double
-  distance(const Model & model,
-           const Eigen::VectorXd & q0,
-           const Eigen::VectorXd & q1);
+  template<typename LieGroup_t, typename JointCollection, typename ConfigVectorIn1, typename ConfigVectorIn2>
+  typename JointCollection::Scalar
+  distance(const ModelTpl<JointCollection> & model,
+           const Eigen::MatrixBase<ConfigVectorIn1> & q0,
+           const Eigen::MatrixBase<ConfigVectorIn2> & q1);
 
   /**
    * @brief      Generate a configuration vector uniformly sampled among provided limits.
@@ -102,10 +107,11 @@ namespace se3
    *
    * @return     The resulted configuration vector (size model.nq)
    */
-  template<typename LieGroup_t>
-  inline Eigen::VectorXd randomConfiguration(const Model & model,
-                                             const Eigen::VectorXd & lowerLimits,
-                                             const Eigen::VectorXd & upperLimits);
+  template<typename LieGroup_t,typename JointCollection, typename ConfigVectorIn1, typename ConfigVectorIn2>
+  typename EIGEN_PLAIN_TYPE(typename ModelTpl<JointCollection>::ConfigVectorType)
+  randomConfiguration(const ModelTpl<JointCollection> & model,
+                      const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
+                      const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits);
 
   /**
    * @brief      Generate a configuration vector uniformly sampled among the joint limits of the specified Model.
@@ -116,8 +122,9 @@ namespace se3
    * @param[in]  model   Model we want to generate a configuration vector of
    * @return     The resulted configuration vector (size model.nq)
    */
-  template<typename LieGroup_t>
-  inline Eigen::VectorXd randomConfiguration(const Model & model);
+  template<typename LieGroup_t,typename JointCollection>
+  typename EIGEN_PLAIN_TYPE(typename ModelTpl<JointCollection>::ConfigVectorType)
+  randomConfiguration(const ModelTpl<JointCollection> & model);
 
   /**
    * @brief         Normalize a configuration
@@ -125,9 +132,9 @@ namespace se3
    * @param[in]     model      Model
    * @param[in,out] q          Configuration to normalize
    */
-  template<typename LieGroup_t>
-  inline void normalize(const Model & model,
-                        Eigen::VectorXd & q);
+  template<typename LieGroup_t, typename JointCollection, typename ConfigVectorType>
+  inline void normalize(const ModelTpl<JointCollection> & model,
+                        const Eigen::MatrixBase<ConfigVectorType> & qout);
   
   /**
    * @brief         Return true if the given configurations are equivalents
@@ -140,11 +147,12 @@ namespace se3
    *
    * @return     Wheter the configurations are equivalent or not
    */
-  template<typename LieGroup_t>
-  inline bool isSameConfiguration(const Model & model,
-                                  const Eigen::VectorXd & q1,
-                                  const Eigen::VectorXd & q2,
-                                  const double & prec = Eigen::NumTraits<double>::dummy_precision());
+  template<typename LieGroup_t, typename JointCollection, typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar>
+  inline bool
+  isSameConfiguration(const ModelTpl<JointCollection> & model,
+                      const Eigen::MatrixBase<ConfigVectorIn1> & q1,
+                      const Eigen::MatrixBase<ConfigVectorIn2> & q2,
+                      const Scalar & prec);
   
   /**
    * @brief         Return the neutral configuration element related to the model configuration space.
@@ -153,11 +161,10 @@ namespace se3
    *
    * @return        The neutral configuration element.
    */
-  template<typename LieGroup_t>
-  inline Eigen::VectorXd neutral(const Model & model);
-  
-  
-  
+  template<typename LieGroup_t, typename JointCollection>
+  inline Eigen::Matrix<typename JointCollection::Scalar,Eigen::Dynamic,1,JointCollection::Options>
+  neutral(const ModelTpl<JointCollection> & model);
+
 } // namespace se3
 
 /* --- Details -------------------------------------------------------------------- */

src/algorithm/joint-configuration.hxx

@@ -30,209 +30,272 @@
 /* --- Details -------------------------------------------------------------------- */
 namespace se3
 {
-  inline Eigen::VectorXd
-  integrate(const Model & model,
-            const Eigen::VectorXd & q,
-            const Eigen::VectorXd & v)
+  template<typename JointCollection, typename ConfigVectorType, typename TangentVectorType>
+  inline typename EIGEN_PLAIN_TYPE(ConfigVectorType)
+  integrate(const ModelTpl<JointCollection> & model,
+            const Eigen::MatrixBase<ConfigVectorType> & q,
+            const Eigen::MatrixBase<TangentVectorType> & v)
   {
-    return integrate<LieGroupMap>(model, q, v);
+    return integrate<LieGroupMap,JointCollection,ConfigVectorType,TangentVectorType>(model, q.derived(), v.derived());
   }
 
-  template<typename LieGroup_t>
-  inline Eigen::VectorXd
-  integrate(const Model & model,
-            const Eigen::VectorXd & q,
-            const Eigen::VectorXd & v)
+  template<typename LieGroup_t, typename JointCollection, typename ConfigVectorType, typename TangentVectorType>
+  inline typename EIGEN_PLAIN_TYPE(ConfigVectorType)
+  integrate(const ModelTpl<JointCollection> & model,
+            const Eigen::MatrixBase<ConfigVectorType> & q,
+            const Eigen::MatrixBase<TangentVectorType> & v)
   {
-    Eigen::VectorXd integ(model.nq);
-    typename IntegrateStep<LieGroup_t>::ArgsType args(q, v, integ);
-    for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
+    typedef typename EIGEN_PLAIN_TYPE(ConfigVectorType) ReturnType;
+    typedef ModelTpl<JointCollection> Model;
+    typedef typename Model::JointIndex JointIndex;
+    ReturnType res(model.nq);
+    
+    typedef IntegrateStep<LieGroup_t,ConfigVectorType,TangentVectorType,ReturnType> Algo;
+    typename Algo::ArgsType args(q.derived(),v.derived(),res);
+    for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
     {
-      IntegrateStep<LieGroup_t>::run (model.joints[i], args);
+      Algo::run(model.joints[i], args);
     }
-    return integ;
+    return res;
   }
 
-  inline Eigen::VectorXd
-  interpolate(const Model & model,
-               const Eigen::VectorXd & q0,
-               const Eigen::VectorXd & q1,
-               const double u)
+  template<typename JointCollection, typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar>
+  inline typename EIGEN_PLAIN_TYPE(ConfigVectorIn1)
+  interpolate(const ModelTpl<JointCollection> & model,
+              const Eigen::MatrixBase<ConfigVectorIn1> & q0,
+              const Eigen::MatrixBase<ConfigVectorIn2> & q1,
+              const Scalar & u)
   {
-    return interpolate<LieGroupMap>(model, q0, q1, u);
+    return interpolate<LieGroupMap,JointCollection,ConfigVectorIn1,ConfigVectorIn2,Scalar>(model, q0, q1, u);
   }
 
-  template<typename LieGroup_t>
-  inline Eigen::VectorXd
-  interpolate(const Model & model,
-               const Eigen::VectorXd & q0,
-               const Eigen::VectorXd & q1,
-               const double u)
+  template<typename LieGroup_t, typename JointCollection, typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar>
+  inline typename EIGEN_PLAIN_TYPE(ConfigVectorIn1)
+  interpolate(const ModelTpl<JointCollection> & model,
+              const Eigen::MatrixBase<ConfigVectorIn1> & q0,
+              const Eigen::MatrixBase<ConfigVectorIn2> & q1,
+              const Scalar & u)
   {
-    Eigen::VectorXd interp(model.nq);
-    for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
+    typedef ModelTpl<JointCollection> Model;
+    typedef typename Model::JointIndex JointIndex;
+    typedef typename EIGEN_PLAIN_TYPE(ConfigVectorIn1) ReturnType;
+    typedef InterpolateStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,Scalar,ReturnType> Algo;
+    
+    ReturnType res(model.nq);
+    for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
     {
-      InterpolateStep<LieGroup_t>::run
-        (model.joints[i], typename InterpolateStep<LieGroup_t>::ArgsType (q0, q1, u, interp));
+      Algo::run(model.joints[i],
+                typename Algo::ArgsType(q0, q1, u, res));
     }
-    return interp;
+    
+    return res;
   }
 
-  template<typename LieGroup_t>
-  inline Eigen::VectorXd
-  difference(const Model & model,
-                     const Eigen::VectorXd & q0,
-                     const Eigen::VectorXd & q1)
+  template<typename LieGroup_t, typename JointCollection, typename ConfigVectorIn1, typename ConfigVectorIn2>
+  inline typename EIGEN_PLAIN_TYPE(ConfigVectorIn1)
+  difference(const ModelTpl<JointCollection> & model,
+             const Eigen::MatrixBase<ConfigVectorIn1> & q0,
+             const Eigen::MatrixBase<ConfigVectorIn2> & q1)
   {
-    Eigen::VectorXd diff(model.nv);
-    typename DifferenceStep<LieGroup_t>::ArgsType args(q0, q1, diff);
-    for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
+    typedef ModelTpl<JointCollection> Model;
+    typedef typename Model::JointIndex JointIndex;
+    typedef typename EIGEN_PLAIN_TYPE(ConfigVectorIn1) ReturnType;
+    
+    ReturnType res(model.nv);
+    
+    typedef DifferenceStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,ReturnType> Algo;
+    typename Algo::ArgsType args(q0.derived(),q1.derived(),res);
+    for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
     {
-      DifferenceStep<LieGroup_t>::run(model.joints[i], args);
+      Algo::run(model.joints[i], args);
     }
-    return diff;
+    
+    return res;
   }
 
-  inline Eigen::VectorXd
-  difference(const Model & model,
-                     const Eigen::VectorXd & q0,
-                     const Eigen::VectorXd & q1)
+  template<typename JointCollection, typename ConfigVectorIn1, typename ConfigVectorIn2>
+  inline typename EIGEN_PLAIN_TYPE(ConfigVectorIn1)
+  difference(const ModelTpl<JointCollection> & model,
+             const Eigen::MatrixBase<ConfigVectorIn1> & q0,
+             const Eigen::MatrixBase<ConfigVectorIn2> & q1)
   {
-    return difference<LieGroupMap>(model, q0, q1);
+    return difference<LieGroupMap,JointCollection,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived());
   }
 
-  template<typename LieGroup_t>
-  inline Eigen::VectorXd
-  squaredDistance(const Model & model,
-                  const Eigen::VectorXd & q0,
-                  const Eigen::VectorXd & q1)
+  template<typename LieGroup_t,typename JointCollection, typename ConfigVectorIn1, typename ConfigVectorIn2>
+  inline typename EIGEN_PLAIN_TYPE(ConfigVectorIn1)
+  squaredDistance(const ModelTpl<JointCollection> & model,
+                  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
+                  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
   {
-    Eigen::VectorXd distances(Eigen::VectorXd::Zero(model.njoints-1));
-    for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
+    typedef ModelTpl<JointCollection> Model;
+    typedef typename Model::JointIndex JointIndex;
+    typedef typename EIGEN_PLAIN_TYPE(ConfigVectorIn1) ReturnType;
+    
+    ReturnType distances(ReturnType::Zero(model.njoints-1));
+    typedef SquaredDistanceStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,ReturnType> Algo;
+    for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
     {
-      typename SquaredDistanceStep<LieGroup_t>::ArgsType args(i-1, q0, q1, distances);
-      SquaredDistanceStep<LieGroup_t>::run(model.joints[i], args);
+      typename Algo::ArgsType args(i-1,q0.derived(),q1.derived(), distances);
+      Algo::run(model.joints[i], args);
     }
+    
     return distances;
   }
 
-  inline Eigen::VectorXd
-  squaredDistance(const Model & model,
-                  const Eigen::VectorXd & q0,
-                  const Eigen::VectorXd & q1)
+  template<typename JointCollection, typename ConfigVectorIn1, typename ConfigVectorIn2>
+  inline typename EIGEN_PLAIN_TYPE(ConfigVectorIn1)
+  squaredDistance(const ModelTpl<JointCollection> & model,
+                  const Eigen::MatrixBase<ConfigVectorIn1> & q0,
+                  const Eigen::MatrixBase<ConfigVectorIn2> & q1)
   {
-    return squaredDistance<LieGroupMap>(model, q0, q1);
+    return squaredDistance<LieGroupMap,JointCollection,ConfigVectorIn1,ConfigVectorIn2>(model,q0.derived(),q1.derived());
   }
   
-  template<typename LieGroup_t>
-  inline double
-  distance(const Model & model,
-           const Eigen::VectorXd & q0,
-           const Eigen::VectorXd & q1)
+  template<typename LieGroup_t, typename JointCollection, typename ConfigVectorIn1, typename ConfigVectorIn2>
+  typename JointCollection::Scalar
+  distance(const ModelTpl<JointCollection> & model,
+           const Eigen::MatrixBase<ConfigVectorIn1> & q0,
+           const Eigen::MatrixBase<ConfigVectorIn2> & q1)
   {
-    return std::sqrt(squaredDistance<LieGroup_t>(model, q0, q1).sum());
+    return std::sqrt(squaredDistance<LieGroup_t,JointCollection,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived()).sum());
   }
   
-  inline double
-  distance(const Model & model,
-           const Eigen::VectorXd & q0,
-           const Eigen::VectorXd & q1)
+  template<typename JointCollection, typename ConfigVectorIn1, typename ConfigVectorIn2>
+  inline typename JointCollection::Scalar
+  distance(const ModelTpl<JointCollection> & model,
+           const Eigen::MatrixBase<ConfigVectorIn1> & q0,
+           const Eigen::MatrixBase<ConfigVectorIn2> & q1)
   {
-    return std::sqrt(squaredDistance<LieGroupMap>(model, q0, q1).sum());
+    return std::sqrt(squaredDistance<LieGroupMap,JointCollection,ConfigVectorIn1,ConfigVectorIn2>(model, q0.derived(), q1.derived()).sum());
   }
 
-  template<typename LieGroup_t>
-  inline Eigen::VectorXd
-  randomConfiguration(const Model & model, const Eigen::VectorXd & lowerLimits, const Eigen::VectorXd & upperLimits)
+  template<typename LieGroup_t,typename JointCollection, typename ConfigVectorIn1, typename ConfigVectorIn2>
+  typename EIGEN_PLAIN_TYPE(typename ModelTpl<JointCollection>::ConfigVectorType)
+  randomConfiguration(const ModelTpl<JointCollection> & model,
+                      const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
+                      const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits)
   {
-    Eigen::VectorXd q(model.nq);
-    typename RandomConfigurationStep<LieGroup_t>::ArgsType args(q, lowerLimits, upperLimits);
-    for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
+    typedef ModelTpl<JointCollection> Model;
+    typedef typename Model::JointIndex JointIndex;
+    typedef typename EIGEN_PLAIN_TYPE(typename ModelTpl<JointCollection>::ConfigVectorType) ReturnType;
+    
+    ReturnType q(model.nq);
+    typedef RandomConfigurationStep<LieGroup_t,ReturnType,ConfigVectorIn1,ConfigVectorIn2> Algo;
+    typename Algo::ArgsType args(EIGEN_CONST_CAST(ReturnType,q), lowerLimits.derived(), upperLimits.derived());
+    for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
     {
-      RandomConfigurationStep<LieGroup_t>::run(model.joints[i], args);
+      Algo::run(model.joints[i], args);
     }
+    
     return q;
   }
 
-  inline Eigen::VectorXd
-  randomConfiguration(const Model & model, const Eigen::VectorXd & lowerLimits, const Eigen::VectorXd & upperLimits)
+  template<typename JointCollection, typename ConfigVectorIn1, typename ConfigVectorIn2>
+  typename EIGEN_PLAIN_TYPE(typename ModelTpl<JointCollection>::ConfigVectorType)
+  randomConfiguration(const ModelTpl<JointCollection> & model,
+                      const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
+                      const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits)
   {
-    return randomConfiguration<LieGroupMap>(model, lowerLimits, upperLimits);
+    return randomConfiguration<LieGroupMap,JointCollection,ConfigVectorIn1,ConfigVectorIn2>(model, lowerLimits.derived(), upperLimits.derived());
   }
 
-  template<typename LieGroup_t>
-  inline Eigen::VectorXd
-  randomConfiguration(const Model & model)
+  template<typename LieGroup_t,typename JointCollection>
+  typename EIGEN_PLAIN_TYPE(typename ModelTpl<JointCollection>::ConfigVectorType)
+  randomConfiguration(const ModelTpl<JointCollection> & model)
   {
-    return randomConfiguration<LieGroup_t>(model, model.lowerPositionLimit, model.upperPositionLimit);
+    typedef ModelTpl<JointCollection> Model;
+    typedef typename Model::ConfigVectorType ConfigVectorType;
+    return randomConfiguration<LieGroup_t,JointCollection,ConfigVectorType,ConfigVectorType>(model, model.lowerPositionLimit, model.upperPositionLimit);
   }
 
-  inline Eigen::VectorXd
-  randomConfiguration(const Model & model)
+  template<typename JointCollection>
+  typename EIGEN_PLAIN_TYPE(typename ModelTpl<JointCollection>::ConfigVectorType)
+  randomConfiguration(const ModelTpl<JointCollection> & model)
   {
-    return randomConfiguration<LieGroupMap>(model);
+    return randomConfiguration<LieGroupMap,JointCollection>(model);
   }
 
-  template<typename LieGroup_t>
-  inline void normalize(const Model & model, Eigen::VectorXd & qout)
+  template<typename LieGroup_t, typename JointCollection, typename ConfigVectorType>
+  inline void normalize(const ModelTpl<JointCollection> & model,
+                        const Eigen::MatrixBase<ConfigVectorType> & qout)
   {
-    for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i)
+    typedef ModelTpl<JointCollection> Model;
+    typedef typename Model::JointIndex JointIndex;
+    
+    typedef NormalizeStep<LieGroup_t,ConfigVectorType> Algo;
+    for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
     {
-      NormalizeStep<LieGroup_t>::run(model.joints[i],
-                                     typename NormalizeStep<LieGroup_t>::ArgsType(qout));
+      Algo::run(model.joints[i],
+                typename Algo::ArgsType(EIGEN_CONST_CAST(ConfigVectorType,qout)));
     }
   }
   
-  inline void normalize(const Model & model, Eigen::VectorXd & qout)
+  template<typename JointCollection, typename ConfigVectorType>
+  inline void normalize(const ModelTpl<JointCollection> & model,
+                        const Eigen::MatrixBase<ConfigVectorType> & qout)
   {
-    return normalize<LieGroupMap>(model,qout);
+    return normalize<LieGroupMap,JointCollection,ConfigVectorType>(model,qout.derived());
   }
 
-  template<typename LieGroup_t>
+  template<typename LieGroup_t, typename JointCollection, typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar>
   inline bool
-  isSameConfiguration(const Model & model,
-                      const Eigen::VectorXd & q1,
-                      const Eigen::VectorXd & q2,
-                      const double& prec)
+  isSameConfiguration(const ModelTpl<JointCollection> & model,
+                      const Eigen::MatrixBase<ConfigVectorIn1> & q1,
+                      const Eigen::MatrixBase<ConfigVectorIn2> & q2,
+                      const Scalar & prec)
   {
+    typedef ModelTpl<JointCollection> Model;
+    typedef typename Model::JointIndex JointIndex;
+    
     bool result = true;
-    typename IsSameConfigurationStep<LieGroup_t>::ArgsType args (result, q1, q2, prec);
-    for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
+    typedef IsSameConfigurationStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,Scalar> Algo;
+    typename Algo::ArgsType args(result,q1.derived(),q2.derived(),prec);
+    for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
     {
-      IsSameConfigurationStep<LieGroup_t>::run(model.joints[i], args);
-      if( !result )
+      Algo::run(model.joints[i], args);
+      if(!result)
         return false;
     }
+    
     return true;
   }
 
+  template<typename JointCollection, typename ConfigVectorIn1, typename ConfigVectorIn2>
   inline bool
-  isSameConfiguration(const Model & model,
-                      const Eigen::VectorXd & q1,
-                      const Eigen::VectorXd & q2,
-                      const double& prec = Eigen::NumTraits<double>::dummy_precision())
+  isSameConfiguration(const ModelTpl<JointCollection> & model,
+                      const Eigen::MatrixBase<ConfigVectorIn1> & q1,
+                      const Eigen::MatrixBase<ConfigVectorIn2> & q2,
+                      const typename JointCollection::Scalar & prec = Eigen::NumTraits<typename JointCollection::Scalar>::dummy_precision())
   {
-    return isSameConfiguration<LieGroupMap>(model, q1, q2, prec);
+    typedef typename JointCollection::Scalar Scalar;
+    return isSameConfiguration<LieGroupMap,JointCollection,ConfigVectorIn1,ConfigVectorIn2,Scalar>(model, q1.derived(), q2.derived(), prec);
   }
   
-
-  
-  template<typename LieGroup_t>
-  inline Eigen::VectorXd neutral(const Model & model)
+  template<typename LieGroup_t, typename JointCollection>
+  inline Eigen::Matrix<typename JointCollection::Scalar,Eigen::Dynamic,1,JointCollection::Options>
+  neutral(const ModelTpl<JointCollection> & model)
   {
-    Eigen::VectorXd neutral_elt(model.nq);
-    typename NeutralStep<LieGroup_t>::ArgsType args(neutral_elt);
-    for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
+    typedef Eigen::Matrix<typename JointCollection::Scalar,Eigen::Dynamic,1,JointCollection::Options> ReturnType;
+    typedef ModelTpl<JointCollection> Model;
+    typedef typename Model::JointIndex JointIndex;
+    
+    ReturnType neutral_elt(model.nq);
+    
+    typename NeutralStep<LieGroup_t,ReturnType>::ArgsType args(neutral_elt.derived());
+    for(JointIndex i=1; i<(JointIndex)model.njoints; ++i )
     {
-      NeutralStep<LieGroup_t>::run(model.joints[i], args);
-      
+      NeutralStep<LieGroup_t,ReturnType>::run(model.joints[i],args);
     }
+    
     return neutral_elt;
   }
   
-  inline Eigen::VectorXd neutral(const Model & model)
+  template<typename JointCollection>
+  inline Eigen::Matrix<typename JointCollection::Scalar,Eigen::Dynamic,1,JointCollection::Options>
+  neutral(const ModelTpl<JointCollection> & model)
   {
-    return neutral<LieGroupMap>(model);
+    return neutral<LieGroupMap,JointCollection>(model);
   }
 
 

src/multibody/model.hxx

@@ -106,7 +106,8 @@ namespace se3
     jmodel.jointConfigSelector(upperPositionLimit) = max_config;
     
     neutralConfiguration.conservativeResize(nq);
-    NeutralStepAlgo<LieGroupMap,JointModelDerived>::run(jmodel,neutralConfiguration);
+    typedef NeutralStep<LieGroupMap,ConfigVectorType> NeutralVisitor;
+    NeutralStepAlgo<NeutralVisitor,JointModelDerived>::run(jmodel,neutralConfiguration);
 
     rotorInertia.conservativeResize(nv);
     jmodel.jointVelocitySelector(rotorInertia).setZero();