Added methods in joints and algorithm working on Model to determine if two...

Added methods in joints and algorithm working on Model to determine if two configurations are equivalent

src/algorithm/joint-configuration.hpp

@@ -112,6 +112,20 @@ namespace se3
    */
   inline void normalize(const Model & model,
                         Eigen::VectorXd & q);
+
+  /**
+   * @brief         Return true if the given configurations are equivalents
+   * \warning       Two configurations can be equivalent but not equally coefficient wise (e.g for quaternions)
+   * 
+   * @param[in]     model      Model
+   * @param[in]     q1        The first configuraiton to compare
+   * @param[in]     q2        The Second configuraiton to compare
+   * 
+   * @return     Wheter the configurations are equivalent or not
+   */
+  inline bool isSameConfiguration(const Model & model,
+                                  const Eigen::VectorXd & q1,
+                                  const Eigen::VectorXd & q2);
 } // namespace se3 
 
 /* --- Details -------------------------------------------------------------------- */
@@ -332,6 +346,40 @@ namespace se3
   }
 
 
+  struct IsSameConfigurationStep : public fusion::JointModelVisitor<IsSameConfigurationStep>
+  {
+    typedef boost::fusion::vector<bool &,
+                                  const Eigen::VectorXd &,
+                                  const Eigen::VectorXd &> ArgsType;
+
+    JOINT_MODEL_VISITOR_INIT(IsSameConfigurationStep);
+
+    template<typename JointModel>
+    static void algo(const se3::JointModelBase<JointModel> & jmodel,
+                     bool & isSame,
+                     const Eigen::VectorXd & q1,
+                     const Eigen::VectorXd & q2)
+    {
+      isSame = jmodel.isSameConfiguration(q1,q2);
+    }
+  };
+
+  inline bool
+  isSameConfiguration(const Model & model,
+                      const Eigen::VectorXd & q1,
+                      const Eigen::VectorXd & q2)
+  {
+    bool result = false;
+    for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoint; ++i )
+    {
+      IsSameConfigurationStep::run(model.joints[i], IsSameConfigurationStep::ArgsType (result,q1,q2)); 
+      if( !result )
+        return false;
+    }
+    return true;
+  }
+
+
 } // namespace se3
 
 #endif // ifndef __se3_joint_configuration_hpp__

src/multibody/joint/joint-base.hpp

@@ -68,6 +68,7 @@ namespace se3
 
 #define SE3_JOINT_TYPEDEF_ARG(prefix)              \
    typedef int Index;                \
+   typedef prefix traits<JointDerived>::Scalar Scalar;    \
    typedef prefix traits<JointDerived>::JointDataDerived JointDataDerived;        \
    typedef prefix traits<JointDerived>::JointModelDerived JointModelDerived;      \
    typedef prefix traits<JointDerived>::Constraint_t Constraint_t;      \
@@ -92,6 +93,7 @@ namespace se3
 
 #define SE3_JOINT_TYPEDEF_NOARG()       \
   typedef int Index;            \
+  typedef traits<JointDerived>::Scalar Scalar;    \
   typedef traits<JointDerived>::JointDataDerived JointDataDerived;     \
   typedef traits<JointDerived>::JointModelDerived JointModelDerived;     \
   typedef traits<JointDerived>::Constraint_t Constraint_t;   \
@@ -111,6 +113,7 @@ namespace se3
 
 #define SE3_JOINT_TYPEDEF_ARG(prefix)         \
   typedef int Index;              \
+  typedef prefix traits<JointDerived>::Scalar Scalar;
   typedef prefix traits<JointDerived>::JointDataDerived JointDataDerived;      \
   typedef prefix traits<JointDerived>::JointModelDerived JointModelDerived;      \
   typedef prefix traits<JointDerived>::Constraint_t Constraint_t;    \
@@ -135,6 +138,7 @@ namespace se3
 
 #define SE3_JOINT_TYPEDEF_ARG()              \
   typedef int Index;                 \
+  typedef typename traits<JointDerived>::Scalar Scalar;    \
   typedef typename traits<JointDerived>::JointDataDerived JointDataDerived;         \
   typedef typename traits<JointDerived>::JointModelDerived JointModelDerived;       \
   typedef typename traits<JointDerived>::Constraint_t Constraint_t;       \
@@ -356,6 +360,15 @@ namespace se3
      */
     void normalize_impl(Eigen::VectorXd &) const { }
 
+    /**
+     * @brief      Check if two configurations are equivalent within the given precision 
+     *
+     * @param[in]  q1     Configuration 1 (size full model.nq)
+     * @param[in]  q2    Configuration 2 (size full model.nq)
+     */
+    bool isSameConfiguration(const Eigen::VectorXd & q1, const Eigen::VectorXd & q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+    { return derived().isSameConfiguration_impl(q1,q2, prec); }
+
     JointIndex i_id; // ID of the joint in the multibody list.
     int i_q;    // Index of the joint configuration in the joint configuration vector.
     int i_v;    // Index of the joint velocity in the joint velocity vector.

src/multibody/joint/joint-basic-visitors.hpp

@@ -173,6 +173,20 @@ namespace se3
   inline void normalize(const JointModelVariant & jmodel,
                         Eigen::VectorXd & q);
 
+  /**
+   * @brief      Visit a JointModelVariant through JointIsSameConfigurationVisitor to determine
+   *             if the two given configurations are equivalent or not.
+   *
+   * @param[in]  jmodel   The JointModelVariant
+   * @param[in]  q1       Configuration 1
+   * @param[in]  q2       Configuration 2
+   * 
+   * @return     Wheter the two configurations are equivalent
+   */
+  inline bool isSameConfiguration(const JointModelVariant & jmodel,
+                                  const Eigen::VectorXd & q1,
+                                  const Eigen::VectorXd & q2);
+
   /**
    * @brief      Visit a JointModelVariant through JointNvVisitor to get the dimension of 
    *             the joint tangent space

src/multibody/joint/joint-basic-visitors.hxx

@@ -265,6 +265,31 @@ namespace se3
     JointNormalizeVisitor::run(jmodel, q);
   }
 
+  /**
+   * @brief      JointIsSameConfigurationVisitor visitor
+   */
+  class JointIsSameConfigurationVisitor: public boost::static_visitor<bool>
+  {
+  public:
+    const Eigen::VectorXd & q1;
+    const Eigen::VectorXd & q2;
+
+    JointIsSameConfigurationVisitor(const Eigen::VectorXd & q1,const Eigen::VectorXd & q2) : q1(q1), q2(q2) {}
+
+    template<typename D>
+    bool operator()(const JointModelBase<D> & jmodel) const
+    { return jmodel.isSameConfiguration(q1, q2); }
+    
+    static bool run(const JointModelVariant & jmodel, const Eigen::VectorXd & q1, const Eigen::VectorXd & q2)
+    { return boost::apply_visitor( JointIsSameConfigurationVisitor(q1,q2), jmodel ); }
+  };
+
+  inline bool isSameConfiguration(const JointModelVariant & jmodel,
+                                  const Eigen::VectorXd & q1,
+                                  const Eigen::VectorXd & q2)
+  {
+    return JointIsSameConfigurationVisitor::run(jmodel, q1, q2);
+  }
 
   /**
    * @brief      JointDifferenceVisitor visitor

src/multibody/joint/joint-dense.hpp

@@ -30,6 +30,7 @@ namespace se3
       NQ = _NQ, // pb
       NV = _NV
     };
+    typedef double Scalar;
     typedef JointDataDense<_NQ, _NV> JointDataDerived;
     typedef JointModelDense<_NQ, _NV> JointModelDerived;
     typedef ConstraintXd Constraint_t;
@@ -202,6 +203,11 @@ namespace se3
       assert(false && "JointModelDense is read-only, should not perform any calc");
     }
 
+    bool isSameConfiguration_impl(const Eigen::VectorXd &, const Eigen::VectorXd &, const Scalar & = Eigen::NumTraits<Scalar>::dummy_precision()) const
+    {
+      assert(false && "JointModelDense is read-only, should not perform any calc");
+    }
+
     JointModelDense() {}
     JointModelDense(JointIndex idx, int idx_q, int idx_v)
     {

src/multibody/joint/joint-free-flyer.hpp

@@ -142,6 +142,7 @@ namespace se3
       NQ = 7,
       NV = 6
     };
+    typedef double Scalar;
     typedef JointDataFreeFlyer JointDataDerived;
     typedef JointModelFreeFlyer JointModelDerived;
     typedef ConstraintIdentity Constraint_t;
@@ -200,7 +201,6 @@ namespace se3
     using JointModelBase<JointModelFreeFlyer>::idx_v;
     using JointModelBase<JointModelFreeFlyer>::setIndexes;
     typedef Motion::Vector3 Vector3;
-    typedef double Scalar;
 
     JointDataDerived createData() const { return JointDataDerived(); }
     
@@ -254,10 +254,9 @@ namespace se3
         I.setZero();
     }
 
-    ConfigVector_t::Scalar finiteDifferenceIncrement() const
+    Scalar finiteDifferenceIncrement() const
     {
       using std::sqrt;
-      typedef ConfigVector_t::Scalar Scalar;
       return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
     }
     
@@ -362,6 +361,19 @@ namespace se3
       q.segment<4>(idx_q()+3).normalize();
     }
 
+    bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+    {
+      typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
+
+      Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
+      Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_2 = q2.segment<NQ> (idx_q ());
+
+      ConstQuaternionMap_t quat1(q_1.tail<4> ().data());
+      ConstQuaternionMap_t quat2(q_2.tail<4> ().data());
+
+      return ( q_1.head<3>().isApprox(q_2.head<3>(), prec) && defineSameRotation(quat1,quat2) );
+    }
+
     JointModelDense<NQ,NV> toDense_impl() const
     {
       return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());

src/multibody/joint/joint-planar.hpp

@@ -273,6 +273,7 @@ namespace se3
       NQ = 3,
       NV = 3
     };
+    typedef double Scalar;
     typedef JointDataPlanar JointDataDerived;
     typedef JointModelPlanar JointModelDerived;
     typedef ConstraintPlanar Constraint_t;
@@ -490,6 +491,14 @@ namespace se3
       return q;
     } 
 
+    bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+    {
+      Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
+      Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_2 = q2.segment<NQ> (idx_q ());
+
+      return q_1.isApprox(q_2, prec);
+    }
+
     JointModelDense<NQ, NV> toDense_impl() const
     {
       return JointModelDense<NQ, NV>(id(),idx_q(),idx_v());

src/multibody/joint/joint-prismatic-unaligned.hpp

@@ -253,6 +253,7 @@ namespace se3
         NQ = 1,
         NV = 1
       };
+      typedef double Scalar;
       typedef JointDataPrismaticUnaligned JointDataDerived;
       typedef JointModelPrismaticUnaligned JointModelDerived;
       typedef ConstraintPrismaticUnaligned Constraint_t;
@@ -320,7 +321,6 @@ namespace se3
     using JointModelBase<JointModelPrismaticUnaligned>::idx_v;
     using JointModelBase<JointModelPrismaticUnaligned>::setIndexes;
     typedef Motion::Vector3 Vector3;
-    typedef double Scalar;
     
     JointModelPrismaticUnaligned() : axis(Vector3::Constant(NAN))   {}
     JointModelPrismaticUnaligned(Scalar x, Scalar y, Scalar z)
@@ -371,10 +371,9 @@ namespace se3
         I -= data.UDinv * data.U.transpose();
     }
     
-    ConfigVector_t::Scalar finiteDifferenceIncrement() const
+    Scalar finiteDifferenceIncrement() const
     {
       using std::sqrt;
-      typedef ConfigVector_t::Scalar Scalar;
       return sqrt(Eigen::NumTraits<Scalar>::epsilon());
     }
 
@@ -445,6 +444,14 @@ namespace se3
       return q;
     } 
 
+    bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+    {
+      const Scalar & q_1 = q1[idx_q()];
+      const Scalar & q_2 = q2[idx_q()];
+
+      return (fabs(q_1 - q_2) < prec);
+    }
+
     JointModelDense<NQ,NV> toDense_impl() const
     {
       return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());

src/multibody/joint/joint-prismatic.hpp

@@ -337,6 +337,7 @@ namespace se3
       NQ = 1,
       NV = 1
     };
+    typedef double Scalar;
     typedef JointDataPrismatic<axis> JointDataDerived;
     typedef JointModelPrismatic<axis> JointModelDerived;
     typedef ConstraintPrismatic<axis> Constraint_t;
@@ -396,7 +397,6 @@ namespace se3
     using JointModelBase<JointModelPrismatic>::idx_v;
     using JointModelBase<JointModelPrismatic>::setIndexes;
     typedef Motion::Vector3 Vector3;
-    typedef double Scalar;
     
     JointDataDerived createData() const { return JointDataDerived(); }
     void calc( JointDataDerived& data, 
@@ -427,10 +427,9 @@ namespace se3
         I -= data.UDinv * data.U.transpose();
     }
     
-    typename ConfigVector_t::Scalar finiteDifferenceIncrement() const
+    Scalar finiteDifferenceIncrement() const
     {
       using std::sqrt;
-      typedef typename ConfigVector_t::Scalar Scalar;
       return sqrt(Eigen::NumTraits<Scalar>::epsilon());
     }
 
@@ -500,6 +499,14 @@ namespace se3
       return q;
     } 
 
+    bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+    {
+      const Scalar & q_1 = q1[idx_q()];
+      const Scalar & q_2 = q2[idx_q()];
+
+      return (fabs(q_1 - q_2) < prec);
+    }
+
     JointModelDense<NQ,NV> toDense_impl() const
     {
       return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());

src/multibody/joint/joint-revolute-unaligned.hpp

@@ -256,7 +256,7 @@ namespace se3
         NQ = 1,
         NV = 1
       };
-      
+      typedef double Scalar;
       typedef JointDataRevoluteUnaligned JointDataDerived;
       typedef JointModelRevoluteUnaligned JointModelDerived;
       typedef ConstraintRevoluteUnaligned Constraint_t;
@@ -324,7 +324,6 @@ namespace se3
     using JointModelBase<JointModelRevoluteUnaligned>::idx_v;
     using JointModelBase<JointModelRevoluteUnaligned>::setIndexes;
     typedef Motion::Vector3 Vector3;
-    typedef double Scalar;
     
     JointModelRevoluteUnaligned() : axis(Eigen::Vector3d::Constant(NAN))   {}
     JointModelRevoluteUnaligned(const double x, const double y, const double z)
@@ -376,10 +375,9 @@ namespace se3
         I -= data.UDinv * data.U.transpose();
     }
     
-    ConfigVector_t::Scalar finiteDifferenceIncrement() const
+    Scalar finiteDifferenceIncrement() const
     {
       using std::sqrt;
-      typedef ConfigVector_t::Scalar Scalar;
       return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
     }
 
@@ -449,6 +447,14 @@ namespace se3
       return q;
     } 
 
+    bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+    {
+      const Scalar & q_1 = q1[idx_q()];
+      const Scalar & q_2 = q2[idx_q()];
+
+      return (fabs(q_1 - q_2) < prec);
+    }
+
     JointModelDense<NQ,NV> toDense_impl() const
     {
       return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());

src/multibody/joint/joint-revolute-unbounded.hpp

@@ -42,7 +42,7 @@ namespace se3
       NQ = 2,
       NV = 1
     };
-    
+    typedef double Scalar;
     typedef JointDataRevoluteUnbounded<axis> JointDataDerived;
     typedef JointModelRevoluteUnbounded<axis> JointModelDerived;
     typedef ConstraintRevolute<axis> Constraint_t;
@@ -100,7 +100,6 @@ namespace se3
     using JointModelBase<JointModelRevoluteUnbounded>::idx_v;
     using JointModelBase<JointModelRevoluteUnbounded>::setIndexes;
     typedef Motion::Vector3 Vector3;
-    typedef double Scalar;
     
     JointDataDerived createData() const { return JointDataDerived(); }
     void calc( JointDataDerived& data, 
@@ -139,10 +138,9 @@ namespace se3
         I -= data.UDinv * data.U.transpose();
     }
     
-    typename ConfigVector_t::Scalar finiteDifferenceIncrement() const
+    Scalar finiteDifferenceIncrement() const
     {
       using std::sqrt;
-      typedef typename ConfigVector_t::Scalar Scalar;
       return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
     }
 
@@ -251,6 +249,14 @@ namespace se3
       q.segment<NQ>(idx_q()).normalize();
     }
 
+    bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+    {
+      typename Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
+      typename Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_2 = q2.segment<NQ> (idx_q ());
+
+      return q_1.isApprox(q_2, prec);
+    } 
+
     JointModelDense<NQ,NV> toDense_impl() const
     {
       return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());

src/multibody/joint/joint-revolute.hpp

@@ -369,7 +369,7 @@ namespace se3
       NQ = 1,
       NV = 1
     };
-    
+    typedef double Scalar;
     typedef JointDataRevolute<axis> JointDataDerived;
     typedef JointModelRevolute<axis> JointModelDerived;
     typedef ConstraintRevolute<axis> Constraint_t;
@@ -427,7 +427,6 @@ namespace se3
     using JointModelBase<JointModelRevolute>::idx_v;
     using JointModelBase<JointModelRevolute>::setIndexes;
     typedef Motion::Vector3 Vector3;
-    typedef double Scalar;
     
     JointDataDerived createData() const { return JointDataDerived(); }
     void calc( JointDataDerived& data, 
@@ -461,10 +460,9 @@ namespace se3
         I -= data.UDinv * data.U.transpose();
     }
     
-    typename ConfigVector_t::Scalar finiteDifferenceIncrement() const
+    Scalar finiteDifferenceIncrement() const
     {
       using std::sqrt;
-      typedef typename ConfigVector_t::Scalar Scalar;
       return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
     }
 
@@ -522,6 +520,14 @@ namespace se3
       return result; 
     } 
 
+    bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+    {
+      const Scalar & q_1 = q1[idx_q()];
+      const Scalar & q_2 = q2[idx_q()];
+
+      return (fabs(q_1 - q_2) < prec);
+    }
+
     double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
     { 
       return fabs(difference_impl(q0,q1)[0]);

src/multibody/joint/joint-spherical-ZYX.hpp

@@ -270,6 +270,7 @@ namespace se3
       NQ = 3,
       NV = 3
     };
+    typedef double Scalar;
     typedef JointDataSphericalZYX JointDataDerived;
     typedef JointModelSphericalZYX JointModelDerived;
     typedef JointSphericalZYX::ConstraintRotationalSubspace Constraint_t;
@@ -294,7 +295,6 @@ namespace se3
     typedef JointSphericalZYX JointDerived;
     SE3_JOINT_TYPEDEF;
 
-    typedef Motion::Scalar Scalar;
 
     typedef Eigen::Matrix<Scalar,6,6> Matrix6;
     typedef Eigen::Matrix<Scalar,3,3> Matrix3;
@@ -330,7 +330,6 @@ namespace se3
     using JointModelBase<JointModelSphericalZYX>::idx_v;
     using JointModelBase<JointModelSphericalZYX>::setIndexes;
     typedef Motion::Vector3 Vector3;
-    typedef double Scalar;
 
     JointDataDerived createData() const { return JointDataDerived(); }
 
@@ -398,10 +397,9 @@ namespace se3
         I -= data.UDinv * data.U.transpose();
     }
     
-    ConfigVector_t::Scalar finiteDifferenceIncrement() const
+    Scalar finiteDifferenceIncrement() const
     {
       using std::sqrt;
-      typedef ConfigVector_t::Scalar Scalar;
       return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
     }
 
@@ -465,6 +463,14 @@ namespace se3
       ConfigVector_t q;
       q << 0, 0, 0;
       return q;
+    }
+
+    bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+    {
+      Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
+      Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_2 = q2.segment<NQ> (idx_q ());
+
+      return q_1.isApprox(q_2, prec);
     } 
 
     JointModelDense<NQ,NV> toDense_impl() const

src/multibody/joint/joint-spherical.hpp

@@ -205,6 +205,7 @@ namespace se3
       NQ = 4,
       NV = 3
     };
+    typedef double Scalar;
     typedef JointDataSpherical JointDataDerived;
     typedef JointModelSpherical JointModelDerived;
     typedef ConstraintRotationalSubspace Constraint_t;
@@ -263,7 +264,6 @@ namespace se3
     using JointModelBase<JointModelSpherical>::idx_v;
     using JointModelBase<JointModelSpherical>::setIndexes;
     typedef Motion::Vector3 Vector3;
-    typedef double Scalar;
     typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
 
     JointDataDerived createData() const { return JointDataDerived(); }
@@ -320,10 +320,9 @@ namespace se3
       }
     }
     
-    ConfigVector_t::Scalar finiteDifferenceIncrement() const
+    Scalar finiteDifferenceIncrement() const
     {
       using std::sqrt;
-      typedef ConfigVector_t::Scalar Scalar;
       return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
     }
 
@@ -410,6 +409,19 @@ namespace se3
       q.segment<NQ>(idx_q()).normalize();
     }
 
+    bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & = Eigen::NumTraits<Scalar>::dummy_precision()) const
+    {
+      typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
+
+      Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
+      Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_2 = q2.segment<NQ> (idx_q ());
+
+      ConstQuaternionMap_t quat1(q_1.data());
+      ConstQuaternionMap_t quat2(q_2.data());
+
+      return defineSameRotation(quat1,quat2);
+    }
+
     JointModelDense<NQ,NV> toDense_impl() const
     {
       return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());

src/multibody/joint/joint-translation.hpp

@@ -219,6 +219,7 @@ namespace se3
       NQ = 3,
       NV = 3
     };
+    typedef double Scalar;
     typedef JointDataTranslation JointDataDerived;
     typedef JointModelTranslation JointModelDerived;
     typedef ConstraintTranslationSubspace Constraint_t;
@@ -278,7 +279,6 @@ namespace se3
     using JointModelBase<JointModelTranslation>::idx_v;
     using JointModelBase<JointModelTranslation>::setIndexes;
     typedef Motion::Vector3 Vector3;
-    typedef double Scalar;
 
     JointDataDerived createData() const { return JointDataDerived(); }
 
@@ -310,10 +310,9 @@ namespace se3
       }
     }
     
-    ConfigVector_t::Scalar finiteDifferenceIncrement() const
+    Scalar finiteDifferenceIncrement() const
     {
       using std::sqrt;
-      typedef ConfigVector_t::Scalar Scalar;
       return sqrt(Eigen::NumTraits<Scalar>::epsilon());
     }
 
@@ -378,6 +377,14 @@ namespace se3
       return q;
     } 
 
+    bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
+    {
+      Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
+      Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_2 = q2.s