Program Listing for File special-euclidean.hpp

↰ Return to documentation for file (include/pinocchio/multibody/liegroup/special-euclidean.hpp)

//
// Copyright (c) 2016-2020 CNRS INRIA
//

#ifndef __pinocchio_multibody_liegroup_special_euclidean_operation_hpp__
#define __pinocchio_multibody_liegroup_special_euclidean_operation_hpp__

#include <limits>

#include "pinocchio/macros.hpp"
#include "pinocchio/math/quaternion.hpp"
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/utils/static-if.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/liegroup/liegroup-base.hpp"

#include "pinocchio/multibody/liegroup/vector-space.hpp"
#include "pinocchio/multibody/liegroup/cartesian-product.hpp"
#include "pinocchio/multibody/liegroup/special-orthogonal.hpp"

namespace pinocchio
{
  template<int Dim, typename Scalar, int Options = 0>
  struct SpecialEuclideanOperationTpl
  {};

  template<int Dim, typename Scalar, int Options>
  struct traits< SpecialEuclideanOperationTpl<Dim,Scalar,Options> >
  {};

  template<typename _Scalar, int _Options>
  struct traits< SpecialEuclideanOperationTpl<2,_Scalar,_Options> >
  {
    typedef _Scalar Scalar;
    enum
    {
      Options = _Options,
      NQ = 4,
      NV = 3
    };
  };

  // SE(2)
  template<typename _Scalar, int _Options>
  struct SpecialEuclideanOperationTpl<2,_Scalar,_Options>
  : public LieGroupBase <SpecialEuclideanOperationTpl<2,_Scalar,_Options> >
  {
    PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(SpecialEuclideanOperationTpl);

    typedef VectorSpaceOperationTpl<2,Scalar,Options>       R2_t;
    typedef SpecialOrthogonalOperationTpl<2,Scalar,Options> SO2_t;
    typedef CartesianProductOperation<R2_t, SO2_t> R2crossSO2_t;

    typedef Eigen::Matrix<Scalar,2,2,Options> Matrix2;
    typedef Eigen::Matrix<Scalar,2,1,Options> Vector2;

    template<typename TangentVector, typename Matrix2Like, typename Vector2Like>
    static void exp(const Eigen::MatrixBase<TangentVector> & v,
                    const Eigen::MatrixBase<Matrix2Like> & R,
                    const Eigen::MatrixBase<Vector2Like> & t)
    {
      EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(TangentVector_t,TangentVector);
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);

      typedef typename Matrix2Like::Scalar Scalar;
      const Scalar omega = v(2);
      Scalar cv,sv; SINCOS(omega, &sv, &cv);
      PINOCCHIO_EIGEN_CONST_CAST(Matrix2Like,R) << cv, -sv, sv, cv;
      using internal::if_then_else;

      {
        typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) vcross(-v(1), v(0));
        vcross -= -v(1)*R.col(0) + v(0)*R.col(1);
        vcross /= omega;
        Scalar omega_abs = math::fabs(omega);
        PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t).coeffRef(0) = if_then_else(internal::GT, omega_abs , Scalar(1e-14),
                                                                             vcross.coeff(0),
                                                                             v.coeff(0));

        PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t).coeffRef(1) = if_then_else(internal::GT, omega_abs, Scalar(1e-14),
                                                                             vcross.coeff(1),
                                                                             v.coeff(1));
      }

    }

    template<typename Matrix2Like, typename Vector2Like, typename Matrix3Like>
    static void toInverseActionMatrix(const Eigen::MatrixBase<Matrix2Like> & R,
                                      const Eigen::MatrixBase<Vector2Like> & t,
                                      const Eigen::MatrixBase<Matrix3Like> & M,
                                      const AssignmentOperatorType op)
    {
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, M, 3, 3);
      Matrix3Like & Mout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,M);
      typedef typename Matrix3Like::Scalar Scalar;

      typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) tinv((R.transpose() * t).reverse());
      tinv[0] *= Scalar(-1.);
      switch(op)
        {
        case SETTO:
          Mout.template topLeftCorner<2,2>() = R.transpose();
          Mout.template topRightCorner<2,1>() = tinv;
          Mout.template bottomLeftCorner<1,2>().setZero();
          Mout(2,2) = (Scalar)1;
          break;
        case ADDTO:
          Mout.template topLeftCorner<2,2>() += R.transpose();
          Mout.template topRightCorner<2,1>() += tinv;
          Mout(2,2) += (Scalar)1;
          break;
        case RMTO:
          Mout.template topLeftCorner<2,2>() -= R.transpose();
          Mout.template topRightCorner<2,1>() -= tinv;
          Mout(2,2) -= (Scalar)1;
          break;
        default:
          assert(false && "Wrong Op requesed value");
          break;
        }



    }

    template<typename Matrix2Like, typename Vector2Like, typename TangentVector>
    static void log(const Eigen::MatrixBase<Matrix2Like> & R,
                    const Eigen::MatrixBase<Vector2Like> & p,
                    const Eigen::MatrixBase<TangentVector> & v)
    {
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
      EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(TangentVector_t,TangentVector);

      TangentVector & vout = PINOCCHIO_EIGEN_CONST_CAST(TangentVector,v);

      typedef typename Matrix2Like::Scalar Scalar1;

      Scalar1 t = SO2_t::log(R);
      const Scalar1 tabs = math::fabs(t);
      const Scalar1 t2 = t*t;
      Scalar1 st,ct; SINCOS(tabs, &st, &ct);
      Scalar1 alpha;
      alpha = internal::if_then_else(internal::LT, tabs, Scalar(1e-4),
                                     1 - t2/12 - t2*t2/720,
                                     tabs*st/(2*(1-ct)));

      vout.template head<2>().noalias() = alpha * p;
      vout(0) += t/2 * p(1);
      vout(1) += -t/2 * p(0);
      vout(2) = t;
    }

    template<typename Matrix2Like, typename Vector2Like, typename JacobianOutLike>
    static void Jlog(const Eigen::MatrixBase<Matrix2Like> & R,
                     const Eigen::MatrixBase<Vector2Like> & p,
                     const Eigen::MatrixBase<JacobianOutLike> & J)
    {
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
      EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(JacobianOutLike, JacobianMatrix_t);

      JacobianOutLike & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOutLike,J);

      typedef typename Matrix2Like::Scalar Scalar1;

      Scalar1 t = SO2_t::log(R);
      const Scalar1 tabs = math::fabs(t);
      Scalar1 alpha, alpha_dot;
      Scalar1 t2 = t*t;
      Scalar1 st,ct; SINCOS(t, &st, &ct);
      Scalar1 inv_2_1_ct = 0.5 / (1-ct);

      alpha = internal::if_then_else(internal::LT, tabs, Scalar(1e-4),
                                     1 - t2/12,
                                     t*st*inv_2_1_ct);
      alpha_dot = internal::if_then_else(internal::LT, tabs, Scalar(1e-4),
                                         - t / 6 - t2*t / 180,
                                         (st-t) * inv_2_1_ct);

      typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix2Like) V;
      V(0,0) = V(1,1) = alpha;
      V(1,0) = - t / 2;
      V(0,1) = - V(1,0);

      Jout.template topLeftCorner <2,2>().noalias() = V * R;
      Jout.template topRightCorner<2,1>() << alpha_dot*p[0] + p[1]/2, -p(0)/2 + alpha_dot*p(1);
      Jout.template bottomLeftCorner<1,2>().setZero();
      Jout(2,2) = 1;
    }

    static Index nq()
    {
      return NQ;
    }
    static Index nv()
    {
      return NV;
    }

    static ConfigVector_t neutral()
    {
      ConfigVector_t n; n << Scalar(0), Scalar(0), Scalar(1), Scalar(0);
      return n;
    }

    static std::string name()
    {
      return std::string("SE(2)");
    }

    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)
    {
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
      Matrix2 R0, R1; Vector2 t0, t1;
      forwardKinematics(R0, t0, q0);
      forwardKinematics(R1, t1, q1);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP
      Matrix2 R (R0.transpose() * R1);
      Vector2 t (R0.transpose() * (t1 - t0));

      log(R, t, d);
    }

    template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
    void dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                          const Eigen::MatrixBase<ConfigR_t> & q1,
                          const Eigen::MatrixBase<JacobianOut_t>& J) const
    {
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
      Matrix2 R0, R1; Vector2 t0, t1;
      forwardKinematics(R0, t0, q0);
      forwardKinematics(R1, t1, q1);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP
      Matrix2 R (R0.transpose() * R1);
      Vector2 t (R0.transpose() * (t1 - t0));

      if (arg == ARG0) {
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
        JacobianMatrix_t J1;
        Jlog (R, t, J1);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP

        // pcross = [ y1-y0, - (x1 - x0) ]
        Vector2 pcross (q1(1) - q0(1), q0(0) - q1(0));

        JacobianOut_t& J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
        J0.template topLeftCorner <2,2> ().noalias() = - R.transpose();
        J0.template topRightCorner<2,1> ().noalias() = R1.transpose() * pcross;
        J0.template bottomLeftCorner <1,2> ().setZero();
        J0 (2,2) = -1;
        J0.applyOnTheLeft(J1);
      } else if (arg == ARG1) {
        Jlog (R, t, J);
      }
    }

    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 = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);

PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
      Matrix2 R0, R;
      Vector2 t0, t;
      forwardKinematics(R0, t0, q);
      exp(v, R, t);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP

      out.template head<2>().noalias() = R0 * t + t0;
      out.template tail<2>().noalias() = R0 * R.col(0);
    }

    template <class Config_t, class Jacobian_t>
    static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
                                                const Eigen::MatrixBase<Jacobian_t> & J)
    {
      assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");

      Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
      Jout.setZero();

      const typename Config_t::Scalar & c_theta = q(2),
                                      & s_theta = q(3);

      Jout.template topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta;
      Jout.template bottomRightCorner<2,1>() << -s_theta, c_theta;
    }

    template <class Config_t, class Tangent_t, class JacobianOut_t>
    static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t >  & /*q*/,
                                   const Eigen::MatrixBase<Tangent_t>  & v,
                                   const Eigen::MatrixBase<JacobianOut_t>& J,
                                   const AssignmentOperatorType op=SETTO)
    {
      JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);

PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
      Matrix2 R;
      Vector2 t;
      exp(v, R, t);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP

      toInverseActionMatrix(R, t, Jout, op);
    }

    template <class Config_t, class Tangent_t, class JacobianOut_t>
    static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                   const Eigen::MatrixBase<Tangent_t> & v,
                                   const Eigen::MatrixBase<JacobianOut_t> & J,
                                   const AssignmentOperatorType op=SETTO)
    {
      JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
      // TODO sparse version
      MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2];
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
      Eigen::Matrix<Scalar,6,6> Jtmp6;
      Jexp6(nu, Jtmp6);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP

      switch(op)
        {
        case SETTO:
          Jout << Jtmp6.template topLeftCorner<2,2>(), Jtmp6.template topRightCorner<2,1>(),
            Jtmp6.template bottomLeftCorner<1,2>(), Jtmp6.template bottomRightCorner<1,1>();
          break;
        case ADDTO:
          Jout.template topLeftCorner<2,2>() += Jtmp6.template topLeftCorner<2,2>();
          Jout.template topRightCorner<2,1>() += Jtmp6.template topRightCorner<2,1>();
          Jout.template bottomLeftCorner<1,2>() += Jtmp6.template bottomLeftCorner<1,2>();
          Jout.template bottomRightCorner<1,1>() += Jtmp6.template bottomRightCorner<1,1>();
          break;
        case RMTO:
          Jout.template topLeftCorner<2,2>() -= Jtmp6.template topLeftCorner<2,2>();
          Jout.template topRightCorner<2,1>() -= Jtmp6.template topRightCorner<2,1>();
          Jout.template bottomLeftCorner<1,2>() -= Jtmp6.template bottomLeftCorner<1,2>();
          Jout.template bottomRightCorner<1,1>() -= Jtmp6.template bottomRightCorner<1,1>();
          break;
        default:
          assert(false && "Wrong Op requesed value");
          break;
        }
    }

    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                     const Eigen::MatrixBase<Tangent_t> & v,
                                     const Eigen::MatrixBase<JacobianIn_t> & Jin,
                                     const Eigen::MatrixBase<JacobianOut_t> & J_out)
    {
      JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
      Matrix2 R;
      Vector2 t;
      exp(v, R, t);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP

      Vector2 tinv = (R.transpose() * t).reverse();
      tinv[0] *= Scalar(-1.);

      Jout.template topRows<2>().noalias() = R.transpose() * Jin.template topRows<2>();
      Jout.template topRows<2>().noalias() += tinv * Jin.template bottomRows<1>();
      Jout.template bottomRows<1>() = Jin.template bottomRows<1>();
    }

    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                     const Eigen::MatrixBase<Tangent_t> & v,
                                     const Eigen::MatrixBase<JacobianIn_t> & Jin,
                                     const Eigen::MatrixBase<JacobianOut_t> & J_out)
    {
      JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
      MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2];

PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
      Eigen::Matrix<Scalar,6,6> Jtmp6;
      Jexp6(nu, Jtmp6);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP

      Jout.template topRows<2>().noalias()
      = Jtmp6.template topLeftCorner<2,2>() * Jin.template topRows<2>();
      Jout.template topRows<2>().noalias()
      += Jtmp6.template topRightCorner<2,1>() * Jin.template bottomRows<1>();
      Jout.template bottomRows<1>().noalias()
      = Jtmp6.template bottomLeftCorner<1,2>()* Jin.template topRows<2>();
      Jout.template bottomRows<1>().noalias()
      += Jtmp6.template bottomRightCorner<1,1>() * Jin.template bottomRows<1>();

    }

    template <class Config_t, class Tangent_t, class Jacobian_t>
    static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                     const Eigen::MatrixBase<Tangent_t> & v,
                                     const Eigen::MatrixBase<Jacobian_t> & J)
    {
      Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
      Matrix2 R;
      Vector2 t;
      exp(v, R, t);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP

      Vector2 tinv = (R.transpose() * t).reverse();
      tinv[0] *= Scalar(-1);
      //TODO: Aliasing
      Jout.template topRows<2>() = R.transpose() * Jout.template topRows<2>();
      //No Aliasing
      Jout.template topRows<2>().noalias() += tinv * Jout.template bottomRows<1>();
    }

    template <class Config_t, class Tangent_t, class Jacobian_t>
    static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                     const Eigen::MatrixBase<Tangent_t> & v,
                                     const Eigen::MatrixBase<Jacobian_t> & J)
    {
      Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
      MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2];

PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
      Eigen::Matrix<Scalar,6,6> Jtmp6;
      Jexp6(nu, Jtmp6);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP
      //TODO: Remove aliasing
      Jout.template topRows<2>()
      = Jtmp6.template topLeftCorner<2,2>() * Jout.template topRows<2>();
      Jout.template topRows<2>().noalias()
      += Jtmp6.template topRightCorner<2,1>() * Jout.template bottomRows<1>();
      Jout.template bottomRows<1>()
      = Jtmp6.template bottomRightCorner<1,1>() * Jout.template bottomRows<1>();
      Jout.template bottomRows<1>().noalias()
      += Jtmp6.template bottomLeftCorner<1,2>() * Jout.template topRows<2>();
    }

    template <class Config_t>
    static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout)
    {
      PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).template tail<2>().normalize();
    }

    template <class Config_t>
    static bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin,
                                  const Scalar& prec)
    {
      const Scalar norm = Scalar(qin.template tail<2>().norm());
      using std::abs;
      return abs(norm - Scalar(1.0)) < prec;
    }

    template <class Config_t>
    static void random_impl(const Eigen::MatrixBase<Config_t> & qout)
    {
      R2crossSO2_t().random(qout);
    }

    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)
    {
      R2crossSO2_t ().randomConfiguration(lower, upper, qout);
    }

    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 R2crossSO2_t().isSameConfiguration(q0, q1, prec);
    }

  protected:

    template<typename Matrix2Like, typename Vector2Like, typename Vector4Like>
    static void forwardKinematics(const Eigen::MatrixBase<Matrix2Like> & R,
                                  const Eigen::MatrixBase<Vector2Like> & t,
                                  const Eigen::MatrixBase<Vector4Like> & q)
    {
      EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix2Like, Matrix2);
      EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Vector2Like, Vector2);
      EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,Vector4Like);

      PINOCCHIO_EIGEN_CONST_CAST(Vector2Like,t) = q.template head<2>();
      const typename Vector4Like::Scalar & c_theta = q(2),
                                         & s_theta = q(3);
      PINOCCHIO_EIGEN_CONST_CAST(Matrix2Like,R) << c_theta, -s_theta, s_theta, c_theta;

    }
  }; // struct SpecialEuclideanOperationTpl<2>

  template<typename _Scalar, int _Options>
  struct traits< SpecialEuclideanOperationTpl<3,_Scalar,_Options> >
  {
    typedef _Scalar Scalar;
    enum
    {
      Options = _Options,
      NQ = 7,
      NV = 6
    };
  };

  template<typename _Scalar, int _Options>
  struct SpecialEuclideanOperationTpl<3,_Scalar,_Options>
  : public LieGroupBase <SpecialEuclideanOperationTpl<3,_Scalar,_Options> >
  {
    PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(SpecialEuclideanOperationTpl);

    typedef CartesianProductOperation <VectorSpaceOperationTpl<3,Scalar,Options>, SpecialOrthogonalOperationTpl<3,Scalar,Options> > R3crossSO3_t;

    typedef Eigen::Quaternion<Scalar,Options> Quaternion_t;
    typedef Eigen::Map<      Quaternion_t> QuaternionMap_t;
    typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
    typedef SE3Tpl<Scalar,Options> Transformation_t;
    typedef SE3Tpl<Scalar,Options> SE3;
    typedef typename Eigen::NumTraits<Scalar>::Real RealScalar;

    static Index nq()
    {
      return NQ;
    }
    static Index nv()
    {
      return NV;
    }

    static ConfigVector_t neutral()
    {
      ConfigVector_t n; n.template head<6>().setZero(); n[6] = 1;
      return n;
    }

    static std::string name()
    {
      return std::string("SE(3)");
    }

    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 quat0 (q0.derived().template tail<4>().data());
      assert(quaternion::isNormalized(quat0,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
      ConstQuaternionMap_t quat1 (q1.derived().template tail<4>().data());
      assert(quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));

      PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d)
        = log6(  SE3(quat0.matrix(), q0.derived().template head<3>()).inverse()
               * SE3(quat1.matrix(), q1.derived().template head<3>())).toVector();
    }

    template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
    static void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> & q0,
                           const Eigen::MatrixBase<ConfigR_t> & q1,
                           const Eigen::MatrixBase<JacobianOut_t> & J)
    {
      typedef typename SE3::Vector3 Vector3;
      typedef typename SE3::Matrix3 Matrix3;

      ConstQuaternionMap_t quat0 (q0.derived().template tail<4>().data());
      assert(quaternion::isNormalized(quat0,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
      ConstQuaternionMap_t quat1 (q1.derived().template tail<4>().data());
      assert(quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));

      Matrix3 R0(quat0.matrix()), R1 (quat1.matrix());
      assert(isUnitary(R0)); assert(isUnitary(R1));

      const SE3 M (  SE3(R0, q0.template head<3>()).inverse()
                   * SE3(R1, q1.template head<3>()));

      if (arg == ARG0) {
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
        JacobianMatrix_t J1;
        Jlog6 (M, J1);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP

        const Vector3 p1_p0 = R1.transpose()*(q1.template head<3>() - q0.template head<3>());

        JacobianOut_t & J0 = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
        J0.template bottomRightCorner<3,3> ().noalias() = J0.template topLeftCorner <3,3> ().noalias() = - M.rotation().transpose();
        J0.template topRightCorner<3,3> ().noalias() = skew(p1_p0) * M.rotation().transpose(); // = R1.T * skew(q1_t - q0_t) * R0;
        J0.template bottomLeftCorner<3,3> ().setZero();
        J0.applyOnTheLeft(J1);
      }
      else if (arg == ARG1) {
        Jlog6 (M, J);
      }
    }

    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 = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
      Quaternion_t const quat(q.derived().template tail<4>());
      assert(quaternion::isNormalized(quat,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
      QuaternionMap_t res_quat (out.template tail<4>().data());

      using internal::if_then_else;

      SE3 M0 (quat.matrix(), q.derived().template head<3>());
      MotionRef<const Velocity_t> mref_v(v.derived());
      SE3 M1 (M0 * exp6(mref_v));

      out.template head<3>() = M1.translation();
      quaternion::assignQuaternion(res_quat,M1.rotation()); // required by CasADi
      const Scalar dot_product = res_quat.dot(quat);
      for(Eigen::DenseIndex k = 0; k < 4; ++k)
      {
        res_quat.coeffs().coeffRef(k) = if_then_else(internal::LT, dot_product, Scalar(0),
                                                     -res_quat.coeffs().coeff(k),
                                                      res_quat.coeffs().coeff(k));
      }

      // Norm of qs might be epsilon-different to 1, so M1.rotation might be epsilon-different to a rotation matrix.
      // It is then safer to re-normalized after converting M1.rotation to quaternion.
      quaternion::firstOrderNormalize(res_quat);
      assert(quaternion::isNormalized(res_quat,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
    }

    template <class Config_t, class Jacobian_t>
    static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
                                                const Eigen::MatrixBase<Jacobian_t> & J)
    {
      assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");

      typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Config_t) ConfigPlainType;
      typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType;
      typedef typename ConfigPlainType::Scalar Scalar;

      Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
      Jout.setZero();

      ConstQuaternionMap_t quat_map(q.derived().template tail<4>().data());
      assert(quaternion::isNormalized(quat_map,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
      Jout.template topLeftCorner<3,3>() = quat_map.toRotationMatrix();
//      Jexp3(quat,Jout.template bottomRightCorner<4,3>());

      typedef Eigen::Matrix<Scalar,4,3,JacobianPlainType::Options|Eigen::RowMajor> Jacobian43;
      typedef SE3Tpl<Scalar,ConfigPlainType::Options> SE3;
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
      Jacobian43 Jexp3QuatCoeffWise;

      Scalar theta;
      typename SE3::Vector3 v = quaternion::log3(quat_map,theta);
      quaternion::Jexp3CoeffWise(v,Jexp3QuatCoeffWise);
      typename SE3::Matrix3 Jlog;
      Jlog3(theta,v,Jlog);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP

//      std::cout << "Jexp3QuatCoeffWise\n" << Jexp3QuatCoeffWise << std::endl;
//      std::cout << "Jlog\n" << Jlog << std::endl;

//      if(quat_map.w() >= 0.) // comes from the log3 for quaternions which may change the sign.
      if(quat_map.coeffs()[3] >= Scalar(0)) // comes from the log3 for quaternions which may change the sign.
        PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = Jexp3QuatCoeffWise * Jlog;
      else
        PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template bottomRightCorner<4,3>().noalias() = -Jexp3QuatCoeffWise * Jlog;
    }

    template <class Config_t, class Tangent_t, class JacobianOut_t>
    static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t >  & /*q*/,
                                   const Eigen::MatrixBase<Tangent_t>  & v,
                                   const Eigen::MatrixBase<JacobianOut_t>& J,
                                   const AssignmentOperatorType op=SETTO)
    {
      JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);

      switch(op)
      {
        case SETTO:
          Jout = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
          break;
        case ADDTO:
          Jout += exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
          break;
        case RMTO:
          Jout -= exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
          break;
        default:
          assert(false && "Wrong Op requesed value");
          break;
      }
    }

    template <class Config_t, class Tangent_t, class JacobianOut_t>
    static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t >  & /*q*/,
                                   const Eigen::MatrixBase<Tangent_t>  & v,
                                   const Eigen::MatrixBase<JacobianOut_t>& J,
                                   const AssignmentOperatorType op=SETTO)
    {
      switch(op)
      {
        case SETTO:
          Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
          break;
        case ADDTO:
          Jexp6<ADDTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
          break;
        case RMTO:
          Jexp6<RMTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
          break;
        default:
          assert(false && "Wrong Op requesed value");
          break;
      }
    }

    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                     const Eigen::MatrixBase<Tangent_t> & v,
                                     const Eigen::MatrixBase<JacobianIn_t> & Jin,
                                     const Eigen::MatrixBase<JacobianOut_t> & J_out)
    {
      JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
      Eigen::Matrix<Scalar,6,6> Jtmp6;
      Jtmp6 = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();

      Jout.template topRows<3>().noalias()
      = Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>();
      Jout.template topRows<3>().noalias()
      += Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>();
      Jout.template bottomRows<3>().noalias()
      = Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>();
    }

    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                     const Eigen::MatrixBase<Tangent_t> & v,
                                     const Eigen::MatrixBase<JacobianIn_t> & Jin,
                                     const Eigen::MatrixBase<JacobianOut_t> & J_out)
    {
      JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
      Eigen::Matrix<Scalar,6,6> Jtmp6;
      Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP

      Jout.template topRows<3>().noalias()
      = Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>();
      Jout.template topRows<3>().noalias()
      += Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>();
      Jout.template bottomRows<3>().noalias()
      = Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>();
    }


    template <class Config_t, class Tangent_t, class Jacobian_t>
    static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                     const Eigen::MatrixBase<Tangent_t> & v,
                                     const Eigen::MatrixBase<Jacobian_t> & J_out)
    {
      Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out);
      Eigen::Matrix<Scalar,6,6> Jtmp6;
      Jtmp6 = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();

      //Aliasing
      Jout.template topRows<3>()
      = Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>();
      Jout.template topRows<3>().noalias()
      += Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>();
      Jout.template bottomRows<3>()
      = Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>();
    }

    template <class Config_t, class Tangent_t, class Jacobian_t>
    static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                     const Eigen::MatrixBase<Tangent_t> & v,
                                     const Eigen::MatrixBase<Jacobian_t> & J_out)
    {
      Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out);
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
      Eigen::Matrix<Scalar,6,6> Jtmp6;
      Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP

      Jout.template topRows<3>()
      = Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>();
      Jout.template topRows<3>().noalias()
      += Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>();
      Jout.template bottomRows<3>()
      = Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>();
    }

    template <class ConfigL_t, class ConfigR_t>
    static Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                       const Eigen::MatrixBase<ConfigR_t> & q1)
    {
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
      TangentVector_t t;
      difference_impl(q0, q1, t);
PINOCCHIO_COMPILER_DIAGNOSTIC_POP
      return t.squaredNorm();
    }

    template <class Config_t>
    static void normalize_impl (const Eigen::MatrixBase<Config_t>& qout)
    {
      Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout);
      qout_.template tail<4>().normalize();
    }

    template <class Config_t>
    static bool isNormalized_impl(const Eigen::MatrixBase<Config_t>& qin,
                                  const Scalar& prec)
    {
      Scalar norm = Scalar(qin.template tail<4>().norm());
      using std::abs;
      return abs(norm - Scalar(1.0)) < prec;
    }

    template <class Config_t>
    static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
    {
      R3crossSO3_t().random(qout);
    }

    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)
    {
      R3crossSO3_t ().randomConfiguration(lower, upper, qout);
    }

    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 R3crossSO3_t().isSameConfiguration(q0, q1, prec);
    }
  }; // struct SpecialEuclideanOperationTpl<3>

} // namespace pinocchio

#endif // ifndef __pinocchio_multibody_liegroup_special_euclidean_operation_hpp__