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

* The spaces taken into account are SO1, SO3, SE1, SE3, vector spaces
* and Cartesian product of two of them.

CMakeLists.txt

@@ -189,6 +189,15 @@ 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/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 +290,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 +305,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")

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/joint.hpp"
+
+namespace se3 {
+  template<typename Joint>
+  struct LieGroup {
+    typedef VectorSpaceOperation<Joint::NQ> type;
+  };
+
+  template<> struct LieGroup <JointModelSpherical> {
+    typedef SpecialOrthogonalOperation type;
+  };
+  template<> struct LieGroup <JointModelFreeFlyer> {
+    typedef SpecialEuclideanOperation type;
+  };
+  template<> struct LieGroup <JointModelPlanar> {
+    typedef SpecialEuclidean1Operation type;
+  };
+  template<int Axis> struct LieGroup <JointModelRevoluteUnbounded<Axis> > {
+    typedef SpecialOrthogonal1Operation type;
+  };
+
+  // TODO REMOVE: For testing purposes only
+  // template<>
+  // struct LieGroup <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;
+
+    ///
+    /// \brief Return the resolution of the finite differerence increment according to the Scalar type
+    /// \remark Ideally, this function must depend on the value of q
+    ///
+    /// \returns The finite difference increment.
+    ///
+    // typename ConfigVector_t::Scalar finiteDifferenceIncrement() const
+    // { return derived().finiteDifferenceIncrement(); }
+
+    /**
+     * @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 Config_t, class Tangent_t>
+    static ConfigVector_t integrate(const Eigen::MatrixBase<Config_t>  & q,
+                                   const Eigen::MatrixBase<Tangent_t> & v)
+    {
+      ConfigVector_t qout;
+      integrate(q, v, qout);
+      return qout;
+    }
+
+    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)
+    {
+      Derived::integrate_impl(q, v, 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)
+     */
+    // ConfigVector_t interpolate(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1, double u) const
+    // { return derived().interpolate_impl(q0, q1, u); }
+    template <class ConfigL_t, class ConfigR_t>
+    static ConfigVector_t interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
+                                      const Eigen::MatrixBase<ConfigR_t> & q1,
+                                      const Scalar& u)
+    {
+      ConfigVector_t qout;
+      interpolate(q0, q1, u, qout);
+      return qout;
+    }
+
+    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)
+    {
+      // assert(is_config_vector_size(q0) && is_config_vector_size(q1));
+      Derived::interpolate_impl(q0, q1, u, qout);
+    }
+
+    template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
+    static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
+                                 const Eigen::MatrixBase<ConfigR_t> & q1,
+                                 const Scalar& u,
+                                 const Eigen::MatrixBase<ConfigOut_t>& qout)
+    {
+      if     (u == 0) const_cast<Eigen::MatrixBase<ConfigOut_t>&>(qout) = q0;
+      else if(u == 1) const_cast<Eigen::MatrixBase<ConfigOut_t>&>(qout) = q1;
+      else integrate(q0, u * difference(q0, q1), 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
+     */
+    static ConfigVector_t random()
+    {
+      ConfigVector_t qout;
+      random(qout);
+      return qout;
+    }
+
+    template <class Config_t>
+    static void random (const Eigen::MatrixBase<Config_t>& qout)
+    { return Derived::random_impl (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>
+    static ConfigVector_t randomConfiguration(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
+                                              const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit)
+    {
+      ConfigVector_t qout;
+      randomConfiguration(lower_pos_limit, upper_pos_limit, qout);
+      return qout;
+    }
+
+    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)
+    { Derived::randomConfiguration_impl(lower_pos_limit, upper_pos_limit, 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>
+    static TangentVector_t difference(const Eigen::MatrixBase<ConfigL_t> & q0,
+                                      const Eigen::MatrixBase<ConfigR_t> & q1)
+    {
+      TangentVector_t diff;
+      difference(q0, q1, diff);
+      return diff;
+    }
+
+    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)
+    {
+      // assert(is_config_vector_size(q0) && is_config_vector_size(q1));
+      Derived::difference_impl(q0, q1, 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)
+    { return Derived::squaredDistance_impl(q0, q1); }
+
+    template <class ConfigL_t, class ConfigR_t>
+    static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
+                                       const Eigen::MatrixBase<ConfigR_t> & q1)
+    {
+      TangentVector_t t;
+      difference(q0, q1, t);
+      return t.squaredNorm();
+    }
+
+    /**
+     * @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)
+    { return sqrt(squaredDistance(q0, q1)); }
+
+    /**
+     * @brief      Get neutral configuration of joint
+     *
+     * @return     The joint's neutral configuration
+     */
+    // ConfigVector_t neutralConfiguration() const
+    // { return derived().neutralConfiguration_impl(); }
+
+    /**
+     * @brief      Normalize a configuration
+     *
+     * @param[in,out]  q     Configuration to normalize (size full model.nq)
+     */
+    // void normalize(Eigen::VectorXd & q) const
+    // { return derived().normalize_impl(q); }
+
+    /**
+     * @brief      Default implementation of normalize
+     */
+    // void normalize_impl(Eigen::VectorXd &) const { }
+
+    /**
+     * @brief      Check if two configurations are equivalent within the given precision
+     *
+     * @param[in]  q0    Configuration 0
+     * @param[in]  q1    Configuration 1
+     */
+    template <class ConfigL_t, class ConfigR_t>
+    static bool isSameConfiguration(const Eigen::MatrixBase<ConfigL_t> & q0,
+                                    const Eigen::MatrixBase<ConfigR_t> & q1,
+                                    const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
+    { return Derived::isSameConfiguration_impl(q0,q1, prec); }
+
+    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 q0.isApprox(q1, prec); }
+
+  protected:
+    /// Default constructor.
+    /// 
+    /// Prevent the construction of derived class.
+    LieGroupOperationBase() {}
+    
+    /// Copy constructor
+    ///
+    /// Prevent the copy of derived class.
+    LieGroupOperationBase( const LieGroupOperationBase& clone) {}
+  }; // struct LieGroupOperationBase
+
+} // namespace se3
+
+#endif // ifndef __se3_lie_group_operation_base_hpp__

src/multibody/liegroup/special-euclidean.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_special_euclidean_operation_hpp__
+#define __se3_special_euclidean_operation_hpp__
+
+#include <limits>
+
+#include <pinocchio/spatial/fwd.hpp>
+#include <pinocchio/spatial/se3.hpp>
+#include <pinocchio/multibody/liegroup/operation-base.hpp>
+
+#include <pinocchio/multibody/liegroup/vector-space.hpp>
+#include <pinocchio/multibody/liegroup/cartesian-product.hpp>
+#include <pinocchio/multibody/liegroup/special-orthogonal.hpp>
+
+namespace se3
+{
+  struct SpecialEuclideanOperation;
+  template <> struct traits<SpecialEuclideanOperation> {
+    typedef double Scalar;
+    enum {
+      NQ = 7,
+      NV = 6
+    };
+    typedef Eigen::Matrix<Scalar,NQ,1> ConfigVector_t;
+    typedef Eigen::Matrix<Scalar,NV,1> TangentVector_t;
+  };
+
+  struct SpecialEuclideanOperation : public LieGroupOperationBase <SpecialEuclideanOperation>
+  {
+    typedef CartesianProductOperation <VectorSpaceOperation<3>, SpecialOrthogonalOperation> R3crossSO3_t;
+    typedef SpecialEuclideanOperation LieGroupDerived;
+    SE3_LIE_GROUP_TYPEDEF;
+
+    typedef Eigen::Quaternion<Scalar> Quaternion_t;
+    typedef Eigen::Map<      Quaternion_t> QuaternionMap_t;
+    typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
+    typedef SE3 Transformation_t;
+
+    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)
+    {
+      ConstQuaternionMap_t p0 (q0.derived().template tail<4>().data());
+      ConstQuaternionMap_t p1 (q1.derived().template tail<4>().data());
+      const_cast < Eigen::MatrixBase<Tangent_t>& > (d)
+        = log6(  SE3(p0.matrix(), q0.derived().template head<3>()).inverse()
+               * SE3(p1.matrix(), q1.derived().template head<3>())).toVector();
+    }
+
+    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();
+      ConstQuaternionMap_t quat(q.derived().template tail<4>().data());
+      QuaternionMap_t res_quat (out.template tail<4>().data());
+
+      SE3 M0 (quat.matrix(), q.derived().template head<3>());
+      SE3 M1 (M0 * exp6(Motion(v)));
+
+      out.template head<3>() = M1.translation();
+      res_quat = M1.rotation();