Merge pull request #365 from jmirabel/devel

[C++] Move space operation to new class LieGroup and inherited classes.

CMakeLists.txt

@@ -189,6 +189,16 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
   multibody/joint/joint-composite.hpp
   ) 
 
+SET(${PROJECT_NAME}_MULTIBODY_LIEGROUP_HEADERS
+  multibody/liegroup/liegroup.hpp
+  multibody/liegroup/operation-base.hpp
+  multibody/liegroup/operation-base.hxx
+  multibody/liegroup/vector-space.hpp
+  multibody/liegroup/cartesian-product.hpp
+  multibody/liegroup/special-orthogonal.hpp
+  multibody/liegroup/special-euclidean.hpp
+  ) 
+
 SET(${PROJECT_NAME}_MULTIBODY_HEADERS
   multibody/fwd.hpp
   multibody/constraint.hpp
@@ -281,6 +291,7 @@ SET(HEADERS
   ${${PROJECT_NAME}_TOOLS_HEADERS}
   ${${PROJECT_NAME}_SPATIAL_HEADERS}
   ${${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS}
+  ${${PROJECT_NAME}_MULTIBODY_LIEGROUP_HEADERS}
   ${${PROJECT_NAME}_MULTIBODY_HEADERS}
   ${${PROJECT_NAME}_PARSERS_HEADERS}
   ${${PROJECT_NAME}_ALGORITHM_HEADERS}
@@ -295,6 +306,7 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/math")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/spatial")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/joint")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/liegroup")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers/lua")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers/urdf")

bindings/python/algorithm/expose-joints.cpp

@@ -78,10 +78,11 @@ namespace se3
               "return the configuration normalized ");
       
       bp::def("isSameConfiguration",
-              (bool (*)(const Model &, const VectorXd &, const VectorXd &))&isSameConfiguration,
+              (bool (*)(const Model &, const VectorXd &, const VectorXd &, const double&))&isSameConfiguration,
               bp::args("Model",
                        "Configuration q1 (size Model::nq)",
-                       "Configuration q2 (size Model::nq)"),
+                       "Configuration q2 (size Model::nq)",
+                       "Precision"),
               "Return true if two configurations are equivalent");
     }
     

src/math/quaternion.hpp

@@ -18,7 +18,7 @@
 #ifndef __math_quaternion_hpp__
 #define __math_quaternion_hpp__
 
-#include <boost/math/constants/constants.hpp>
+#include <pinocchio/math/fwd.hpp>
 
 namespace se3
 {
@@ -56,9 +56,10 @@ namespace se3
   ///
   template <typename Derived, typename otherDerived>
   bool defineSameRotation(const Eigen::QuaternionBase<Derived> & q1,
-                          const Eigen::QuaternionBase<otherDerived> & q2)
+                          const Eigen::QuaternionBase<otherDerived> & q2,
+                          const typename Derived::RealScalar & prec = Eigen::NumTraits<typename Derived::Scalar>::dummy_precision())
   {
-    return (q1.coeffs().isApprox(q2.coeffs()) || q1.coeffs().isApprox(-q2.coeffs()) );
+    return (q1.coeffs().isApprox(q2.coeffs(), prec) || q1.coeffs().isApprox(-q2.coeffs(), prec) );
   }
 
   /// Approximately normalize by applying the first order limited development

src/multibody/joint/fwd.hpp

@@ -21,7 +21,37 @@
 namespace se3
 {
   enum { MAX_JOINT_NV = 6 };
-  
+
+  template<int axis> struct JointModelRevolute;
+  template<int axis> struct JointDataRevolute;
+
+  struct JointModelRevoluteUnaligned;
+  struct JointDataRevoluteUnaligned;
+
+  template<int axis> struct JointModelRevoluteUnbounded;
+  template<int axis> struct JointDataRevoluteUnbounded;
+
+  struct JointModelSpherical;
+  struct JointDataSpherical;
+
+  struct JointModelSphericalZYX;
+  struct JointDataSphericalZYX;
+
+  template<int axis> struct JointModelPrismatic;
+  template<int axis> struct JointDataPrismatic;
+
+  struct JointModelPrismaticUnaligned;
+  struct JointDataPrismaticUnaligned;
+
+  struct JointModelFreeFlyer;
+  struct JointDataFreeFlyer;
+
+  struct JointModelPlanar;
+  struct JointDataPlanar;
+
+  struct JointModelTranslation;
+  struct JointDataTranslation;
+
   struct JointModelComposite;
   struct JointDataComposite;
   

src/multibody/joint/joint-base.hpp

@@ -20,6 +20,7 @@
 #define __se3_joint_base_hpp__
 
 #include "pinocchio/multibody/fwd.hpp"
+#include "pinocchio/multibody/joint/fwd.hpp"
 
 #include <Eigen/Core>
 #include <limits>

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

@@ -31,9 +31,6 @@
 namespace se3
 {
 
-  struct JointDataFreeFlyer;
-  struct JointModelFreeFlyer;
-
   struct ConstraintIdentity;
 
   template <>

src/multibody/joint/joint-planar.hpp

@@ -30,9 +30,6 @@
 namespace se3
 {
 
-  struct JointDataPlanar;
-  struct JointModelPlanar;
-
   struct MotionPlanar;
   template <>
   struct traits< MotionPlanar >
@@ -468,7 +465,7 @@ namespace se3
     { 
       ConfigVector_t result;
       result.head<2>().setRandom();
-      const Scalar angle = PI * ((Scalar)(-1 + 2 * rand()) / (Scalar) RAND_MAX);
+      const Scalar angle = PI * (-1 + 2 * ((Scalar)rand()) / RAND_MAX);
       SINCOS(angle, &result[3], &result[2]);
       return result;
     } 
@@ -488,7 +485,7 @@ namespace se3
         }
         result[i] = lower_pos_limit[i] + ( upper_pos_limit[i] - lower_pos_limit[i]) * rand()/RAND_MAX;
       }
-      const Scalar angle = PI * ((Scalar)(-1 + 2 * rand()) / (Scalar) RAND_MAX);
+      const Scalar angle = PI * (-1 + 2 * ((Scalar)rand()) / RAND_MAX);
       SINCOS(angle, &result[3], &result[2]);
       return result;
     } 

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

@@ -28,9 +28,6 @@
 namespace se3
 {
 
-  struct JointDataPrismaticUnaligned;
-  struct JointModelPrismaticUnaligned;
-
   struct MotionPrismaticUnaligned;
   
   template <>

src/multibody/joint/joint-prismatic.hpp

@@ -27,9 +27,6 @@
 
 namespace se3
 {
-
-  template<int axis> struct JointDataPrismatic;
-  template<int axis> struct JointModelPrismatic;
   
   namespace prismatic
   {

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

@@ -28,9 +28,6 @@
 namespace se3
 {
 
-  struct JointDataRevoluteUnaligned;
-  struct JointModelRevoluteUnaligned;
-
   struct MotionRevoluteUnaligned;
   template <>
   struct traits < MotionRevoluteUnaligned >

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

@@ -31,10 +31,6 @@ namespace se3
 
   template<int axis> struct JointRevoluteUnbounded;
 
-  template<int axis> struct JointDataRevoluteUnbounded;
-  template<int axis> struct JointModelRevoluteUnbounded;
-
-
   template<int axis>
   struct traits< JointRevoluteUnbounded<axis> >
   {

src/multibody/joint/joint-revolute.hpp

@@ -29,9 +29,6 @@
 namespace se3
 {
 
-  template<int axis> struct JointDataRevolute;
-  template<int axis> struct JointModelRevolute;
-  
   template<int axis> struct SE3Revolute;
   template<int axis> struct MotionRevolute;
 

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

@@ -29,9 +29,6 @@
 
 namespace se3
 {
-
-  struct JointDataSphericalZYX;
-  struct JointModelSphericalZYX;
   
   template <typename _Scalar, int _Options>
   struct JointSphericalZYXTpl

src/multibody/joint/joint-spherical.hpp

@@ -31,9 +31,6 @@
 namespace se3
 {
 
-  struct JointDataSpherical;
-  struct JointModelSpherical;
-
   struct MotionSpherical;
   template <>
   struct traits< MotionSpherical >

src/multibody/joint/joint-translation.hpp

@@ -29,9 +29,6 @@
 namespace se3
 {
 
-  struct JointDataTranslation;
-  struct JointModelTranslation;
-
   struct MotionTranslation;
   template <>
   struct traits < MotionTranslation >

src/multibody/liegroup/cartesian-product.hpp

+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_cartesian_product_operation_hpp__
+#define __se3_cartesian_product_operation_hpp__
+
+#include <pinocchio/multibody/liegroup/operation-base.hpp>
+
+namespace se3
+{
+  template<typename LieGroup1, typename LieGroup2>
+  struct CartesianProductOperation;
+  template<typename LieGroup1, typename LieGroup2>
+  struct traits<CartesianProductOperation<LieGroup1, LieGroup2> > {
+    typedef double Scalar;
+    enum {
+      NQ = LieGroup1::NQ + LieGroup2::NQ,
+      NV = LieGroup1::NV + LieGroup2::NV
+    };
+    typedef Eigen::Matrix<Scalar,NQ,1> ConfigVector_t;
+    typedef Eigen::Matrix<Scalar,NV,1> TangentVector_t;
+  };
+
+  template<typename LieGroup1, typename LieGroup2>
+  struct CartesianProductOperation : public LieGroupOperationBase <CartesianProductOperation<LieGroup1, LieGroup2> >
+  {
+    typedef CartesianProductOperation<LieGroup1, LieGroup2>  LieGroupDerived;
+    SE3_LIE_GROUP_TYPEDEF;
+
+    template <class ConfigL_t, class ConfigR_t, class Tangent_t>
+    static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
+                                const Eigen::MatrixBase<ConfigR_t> & q1,
+                                const Eigen::MatrixBase<Tangent_t> & d)
+    {
+      Tangent_t& out = const_cast< Eigen::MatrixBase<Tangent_t>& > (d).derived();
+      LieGroup1::difference(q0.template head<LieGroup1::NQ>(), q1.template head<LieGroup1::NQ>(), out.template head<LieGroup1::NV>());
+      LieGroup2::difference(q0.template tail<LieGroup2::NQ>(), q1.template tail<LieGroup2::NQ>(), out.template tail<LieGroup2::NV>());
+    }
+
+    template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
+    static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
+                               const Eigen::MatrixBase<Velocity_t> & v,
+                               const Eigen::MatrixBase<ConfigOut_t> & qout)
+    {
+      ConfigOut_t& out = const_cast< Eigen::MatrixBase<ConfigOut_t>& > (qout).derived();
+      LieGroup1::integrate(q.template head<LieGroup1::NQ>(), v.template head<LieGroup1::NV>(), out.template head<LieGroup1::NQ>());
+      LieGroup2::integrate(q.template tail<LieGroup2::NQ>(), v.template tail<LieGroup2::NV>(), out.template tail<LieGroup2::NQ>());
+    }
+
+    template <class ConfigL_t, class ConfigR_t>
+    static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
+                                       const Eigen::MatrixBase<ConfigR_t> & q1)
+    {
+      return LieGroup1::squaredDistance(q0.template head<LieGroup1::NQ>(), q1.template head<LieGroup1::NQ>())
+        +    LieGroup2::squaredDistance(q0.template tail<LieGroup2::NQ>(), q1.template tail<LieGroup2::NQ>());
+    }
+
+    template <class Config_t>
+    static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
+    {
+      Config_t& out = const_cast< Eigen::MatrixBase<Config_t>& > (qout).derived();
+      LieGroup1::random(out.template head<LieGroup1::NQ>());
+      LieGroup2::random(out.template tail<LieGroup2::NQ>());
+    }
+
+    template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
+    static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
+                                         const Eigen::MatrixBase<ConfigR_t> & upper,
+                                         const Eigen::MatrixBase<ConfigOut_t> & qout)
+    {
+      ConfigOut_t& out = const_cast< Eigen::MatrixBase<ConfigOut_t>& > (qout).derived();
+      LieGroup1::randomConfiguration(lower.template head<LieGroup1::NQ>(), upper.template head<LieGroup1::NQ>(), out.template head<LieGroup1::NQ>());
+      LieGroup2::randomConfiguration(lower.template tail<LieGroup2::NQ>(), upper.template tail<LieGroup2::NQ>(), out.template tail<LieGroup2::NQ>());
+    }
+
+    template <class ConfigL_t, class ConfigR_t>
+    static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
+                                         const Eigen::MatrixBase<ConfigR_t> & q1,
+                                         const Scalar & prec)
+    {
+      return LieGroup1::isSameConfiguration(q0.template head<LieGroup1::NQ>(), q1.template head<LieGroup1::NQ>(), prec)
+        +    LieGroup2::isSameConfiguration(q0.template tail<LieGroup2::NQ>(), q1.template tail<LieGroup2::NQ>(), prec);
+    }
+  }; // struct CartesianProductOperation
+
+} // namespace se3
+
+#endif // ifndef __se3_cartesian_product_operation_hpp__

src/multibody/liegroup/liegroup.hpp

+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_lie_group_hpp__
+#define __se3_lie_group_hpp__
+
+#include "pinocchio/assert.hpp"
+#include "pinocchio/multibody/liegroup/vector-space.hpp"
+#include "pinocchio/multibody/liegroup/cartesian-product.hpp"
+#include "pinocchio/multibody/liegroup/special-orthogonal.hpp"
+#include "pinocchio/multibody/liegroup/special-euclidean.hpp"
+
+#include "pinocchio/multibody/joint/fwd.hpp"
+
+namespace se3 {
+  struct LieGroupTpl {
+    template<typename JointModel> struct operation {
+      typedef VectorSpaceOperation<JointModel::NQ> type;
+    };
+  };
+  template<typename JointModel>
+  struct LieGroup {
+    typedef typename LieGroupTpl::operation<JointModel>::type type;
+  };
+
+  template<> struct LieGroupTpl::operation <JointModelComposite> {};
+  template<> struct LieGroupTpl::operation <JointModelSpherical> {
+    typedef SpecialOrthogonalOperation<3> type;
+  };
+  template<> struct LieGroupTpl::operation <JointModelFreeFlyer> {
+    typedef SpecialEuclideanOperation<3> type;
+  };
+  template<> struct LieGroupTpl::operation <JointModelPlanar> {
+    typedef SpecialEuclideanOperation<2> type;
+  };
+  template<int Axis> struct LieGroupTpl::operation <JointModelRevoluteUnbounded<Axis> > {
+    typedef SpecialOrthogonalOperation<2> type;
+  };
+
+  // TODO REMOVE: For testing purposes only
+  // template<>
+  // struct LieGroupTpl::operation <JointModelTranslation> {
+    // typedef CartesianProductOperation<
+      // CartesianProductOperation<typename LieGroup<JointModelPX>::type, typename LieGroup<JointModelPY>::type>,
+      // typename LieGroup<JointModelPZ>::type
+      // > type;
+  // };
+}
+
+#endif // ifndef __se3_lie_group_hpp__

src/multibody/liegroup/operation-base.hpp

+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_lie_group_operation_base_hpp__
+#define __se3_lie_group_operation_base_hpp__
+
+#include "pinocchio/spatial/fwd.hpp" // struct traits
+
+#include <Eigen/Core>
+#include <limits>
+
+namespace se3
+{
+#ifdef __clang__
+
+#define SE3_LIE_GROUP_TYPEDEF_ARG(prefix)              \
+   typedef prefix traits<LieGroupDerived>::Scalar Scalar;    \
+   enum {                  \
+    NQ = traits<LieGroupDerived>::NQ,              \
+    NV = traits<LieGroupDerived>::NV               \
+  };                        \
+  typedef prefix traits<LieGroupDerived>::ConfigVector_t ConfigVector_t;        \
+  typedef prefix traits<LieGroupDerived>::TangentVector_t TangentVector_t
+
+#define SE3_LIE_GROUP_TYPEDEF SE3_LIE_GROUP_TYPEDEF_ARG()
+#define SE3_LIE_GROUP_TYPEDEF_TEMPLATE SE3_LIE_GROUP_TYPEDEF_ARG(typename)
+
+#elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2)
+
+#define SE3_LIE_GROUP_TYPEDEF_NOARG()       \
+  typedef int Index;            \
+  typedef traits<LieGroupDerived>::Scalar Scalar;    \
+  enum {              \
+    NQ = traits<LieGroupDerived>::NQ,         \
+    NV = traits<LieGroupDerived>::NV          \
+  };                        \
+  typedef traits<LieGroupDerived>::ConfigVector_t ConfigVector_t;        \
+  typedef traits<LieGroupDerived>::TangentVector_t TangentVector_t
+
+#define SE3_LIE_GROUP_TYPEDEF_ARG(prefix)         \
+  typedef int Index;              \
+  typedef prefix traits<LieGroupDerived>::Scalar Scalar;           \
+  enum {                \
+    NQ = traits<LieGroupDerived>::NQ,           \
+    NV = traits<LieGroupDerived>::NV            \
+  };                        \
+  typedef prefix traits<LieGroupDerived>::ConfigVector_t ConfigVector_t;        \
+  typedef prefix traits<LieGroupDerived>::TangentVector_t TangentVector_t
+
+#define SE3_LIE_GROUP_TYPEDEF SE3_LIE_GROUP_TYPEDEF_NOARG()
+#define SE3_LIE_GROUP_TYPEDEF_TEMPLATE SE3_LIE_GROUP_TYPEDEF_ARG(typename)
+
+#else
+
+#define SE3_LIE_GROUP_TYPEDEF_ARG()              \
+  typedef int Index;                 \
+  typedef typename traits<LieGroupDerived>::Scalar Scalar;    \
+  enum {                   \
+    NQ = traits<LieGroupDerived>::NQ,              \
+    NV = traits<LieGroupDerived>::NV               \
+  };                        \
+  typedef typename traits<LieGroupDerived>::ConfigVector_t ConfigVector_t;        \
+  typedef typename traits<LieGroupDerived>::TangentVector_t TangentVector_t
+
+#define SE3_LIE_GROUP_TYPEDEF SE3_LIE_GROUP_TYPEDEF_ARG()
+#define SE3_LIE_GROUP_TYPEDEF_TEMPLATE SE3_LIE_GROUP_TYPEDEF_ARG()
+
+#endif
+
+  template<typename Derived>
+  struct LieGroupOperationBase
+  {
+    typedef Derived LieGroupDerived;
+    SE3_LIE_GROUP_TYPEDEF_TEMPLATE;
+
+    /// \name API with return value as argument
+    /// \{
+
+    /**
+     * @brief      Integrate joint's configuration for a tangent vector during one unit time
+     *
+     * @param[in]  q     initatial configuration  (size full model.nq)
+     * @param[in]  v     joint velocity (size full model.nv)
+     *
+     * @return     The configuration integrated
+     */
+    template <class ConfigIn_t, class Tangent_t, class ConfigOut_t>
+    static void integrate(const Eigen::MatrixBase<ConfigIn_t> & q,
+                          const Eigen::MatrixBase<Tangent_t>  & v,
+                          const Eigen::MatrixBase<ConfigOut_t>& qout);
+
+    /**
+     * @brief      Interpolation between two joint's configurations
+     *
+     * @param[in]  q0    Initial configuration to interpolate
+     * @param[in]  q1    Final configuration to interpolate
+     * @param[in]  u     u in [0;1] position along the interpolation.
+     *
+     * @return     The interpolated configuration (q0 if u = 0, q1 if u = 1)
+     */
+    template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
+    static void interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
+                            const Eigen::MatrixBase<ConfigR_t> & q1,
+                            const Scalar& u,
+                            const Eigen::MatrixBase<ConfigOut_t>& qout);
+
+    /**
+     * @brief      Generate a random joint configuration, normalizing quaternions when necessary.
+     *
+     * \warning    Do not take into account the joint limits. To shoot a configuration uniformingly
+     *             depending on joint limits, see uniformySample
+     *
+     * @return     The joint configuration
+     */
+    template <class Config_t>
+    static void random (const Eigen::MatrixBase<Config_t>& qout);
+
+    /**
+     * @brief      Generate a configuration vector uniformly sampled among
+     *             provided limits.
+     *
+     * @param[in]  lower_pos_limit  lower joint limit
+     * @param[in]  upper_pos_limit  upper joint limit
+     *
+     * @return     The joint configuration
+     */
+    template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
+    static void randomConfiguration(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
+                                    const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
+                                    const Eigen::MatrixBase<ConfigOut_t> & qout);
+
+    /**
+     * @brief      the tangent vector that must be integrated during one unit time to go from q0 to q1
+     *
+     * @param[in]  q0    Initial configuration
+     * @param[in]  q1    Wished configuration
+     *
+     * @return     The corresponding velocity
+     */
+    template <class ConfigL_t, class ConfigR_t, class Tangent_t>
+    static void difference(const Eigen::MatrixBase<ConfigL_t> & q0,
+                           const Eigen::MatrixBase<ConfigR_t> & q1,
+                           const Eigen::MatrixBase<Tangent_t>& d);
+
+    /**
+     * @brief      Squared distance between two configurations of the joint
+     *
+     * @param[in]  q0    Configuration 1
+     * @param[in]  q1    Configuration 2
+     *
+     * @return     The corresponding distance
+     */
+    template <class ConfigL_t, class ConfigR_t>
+    static double squaredDistance(const Eigen::MatrixBase<ConfigL_t> & q0,
+                                  const Eigen::MatrixBase<ConfigR_t> & q1);
+
+    /**
+     * @brief      Distance between two configurations of the joint
+     *
+     * @param[in]  q0    Configuration 1
+     * @param[in]  q1    Configuration 2
+     *
+     * @return     The corresponding distance
+     */
+    template <class ConfigL_t, class ConfigR_t>
+    static double distance(const Eigen::MatrixBase<ConfigL_t> & q0,
+                           const Eigen::MatrixBase<ConfigR_t> & q1);
+
+    /**
+     * @brief      Check if two configurations are equivalent within the given precision