Re: [eigen] Eigen and rigid body simulation

[Benoit Jacob <jacob.benoit.1@xxxxxxxxx>]

2009/11/26 Benoit Jacob <jacob.benoit.1@xxxxxxxxx>:
> Here are some options:
>
> 1) Just implement these 2 new methods as global functions taking a quaternion?
>
> This seems especially reasonable as these functions are log and exp.
> So you could simply have overloads of ei_log() and ei_exp()!
>
> This is actually what you would have to do anyway if you wanted to
> ever use Quaternion as the Scalar type for Eigen matrices/vectors and
> use such functions on them.

Nevermind 1), i hadn't yet noticed that log() and exp() convert
between 2 different types.

So i would go for 2) below:

>
> 2) Move all the angularVelocity part to the Geometry module? It's
> quite reasonable actually, only the naming is a bit too "mechanics"
> oriented. If you could rename "AngularVelocity" to "Angle3D", rename
> log() to angle() and replace exp() by some conversion
> constructor/operator, we could talk about that!
>
> In both cases there's something I don't understand: why do you focus
> 100% on Quaternion? All that Angular stuff could work for any kind of
> rotation. So that could be handled at the level of RotationBase and
> you'd let the user choose any kind of rotation. That would be a
> requirement to put that in the Geometry module. If you want that to
> stay only in your own module, well, then it's your call!
>
>
> Benoit
>
>
>>
>> --
>> Mathieu
>>
>> #ifndef LGSM_DISPLACEMENT_H
>> #define LGSM_DISPLACEMENT_H
>>
>> /**************************************
>> Displacement Base
>> **************************************/
>>
>> namespace Eigen{
>>
>>  template<class Derived>
>>  class DisplacementBase {
>>  public:
>>    Derived& derived() { return *static_cast<Derived*>(this); }
>>    const Derived& derived() const { return *static_cast<const
>> Derived*>(this); }
>>
>>    typedef typename ei_traits<Derived>::Scalar Scalar;
>>    typedef Matrix<Scalar, 3, 1> Vector3;
>>
>>    inline Map<Rotation3D<Scalar> > getRotation3D(){
>>      return Map<Rotation3D<Scalar> >(this->derived().data());
>>    }
>>
>>    inline const Map<Rotation3D<Scalar> > getRotation3D() const{
>>      return Map<Rotation3D<Scalar> >(this->derived().data());
>>    }
>>
>>    inline Map<Vector3> getTranslation(){
>>      return Map<Vector3>(this->derived().data() + 4);
>>    }
>>
>>    inline const Map<Vector3> getTranslation() const {
>>      return Map<Vector3>(this->derived().data() + 4);
>>    }
>>
>>    inline Displacement<Scalar> inverse() const;
>>    template<class OtherDerived> EIGEN_STRONG_INLINE Displacement<Scalar>
>> operator* (const DisplacementBase<OtherDerived>& d) const;
>>    template<class OtherDerived> EIGEN_STRONG_INLINE Vector3 operator* (const
>> MatrixBase<OtherDerived>& d) const;
>>
>>    inline Twist<Scalar> log(const Scalar precision =
>> Rotation3D<Scalar>::precision()) const;
>>
>>    template<class OtherDerived> Twist<Scalar> adjoint(const
>> TwistBase<OtherDerived>&) const;
>>    template<class OtherDerived> Wrench<Scalar> adjointTr(const
>> WrenchBase<OtherDerived>&) const;
>>
>>    Matrix<Scalar, 6, 6> adjoint() const;
>>    Matrix<Scalar, 6, 6> adjointInv() const;
>>
>>
>>    template<class OtherDerived> Twist<Scalar> changeBase(const
>> TwistBase<OtherDerived>&) const;
>>    template<class OtherDerived> Wrench<Scalar> changeBase(const
>> WrenchBase<OtherDerived>&) const;
>>
>>
>>    template<class OtherDerived> Twist<Scalar> changeReductionPoint (const
>> TwistBase<OtherDerived>&) const;
>>    template<class OtherDerived> Wrench<Scalar> changeReductionPoint (const
>> WrenchBase<OtherDerived>&) const;
>>  };
>>
>>  /**************************************
>>  Displacement
>>  **************************************/
>>
>>  template<typename Scalar> class Displacement;
>>
>>  template<typename _Scalar>
>>  struct ei_traits<Displacement<_Scalar> >
>>  {
>>    typedef _Scalar Scalar;
>>    typedef Scalar Coefficients[7];
>>    enum{
>>      PacketAccess = Unaligned // check alignement
>>    };
>>  };
>>
>>  template<typename Scalar>
>>  class Displacement : public DisplacementBase<Displacement<Scalar> >{
>>  public:
>>    //  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,
>> 7)
>>
>>    typedef typename ei_traits<Displacement<Scalar> >::Coefficients
>> Coefficients;
>>
>>    inline Displacement(const Scalar vx, const Scalar vy, const Scalar vz,
>> const Scalar qx, const Scalar qy, const Scalar qz, const Scalar qw)
>>    {
>>      Matrix<Scalar, 7, 1>::Map(m_data) << qx, qy, qz, qw, vx, vy, vz;
>>    }
>>
>>    template<class RotationDerived, class VectorDerived> inline
>> Displacement(const Rotation3DBase<RotationDerived>& rot, const
>> MatrixBase<VectorDerived>& trans){
>>      Matrix<Scalar, 4, 1>::Map(m_data) = rot.coeffs();
>>      Matrix<Scalar, 3, 1>::Map(m_data+4) = trans;
>>    }
>>
>>    template<class Derived> inline Displacement(const
>> DisplacementBase<Derived>& other) : Displacement(other.coeffs()){}
>>
>>    inline Displacement(const Scalar* data)
>>    {
>>      Matrix<Scalar, 7, 1>::Map(m_data) << Matrix<Scalar, 7, 1>::Map(data);
>>    }
>>
>>    EIGEN_STRONG_INLINE Displacement(const Displacement& other)
>>      : Displacement(other.coeffs())
>>    {}
>>
>>    Coefficients& data(){
>>      return m_data;
>>    }
>>
>>    const Coefficients& data() const {
>>      return m_data;
>>    }
>>
>>  protected:
>>    Coefficients m_data;
>>  };
>>
>>  typedef Displacement<double> Displacementd;
>>  typedef Displacement<float> Displacementf;
>>
>>  /**************************************
>>  Map<Displacement>
>>  **************************************/
>>
>>  template<typename _Scalar, int _PacketAccess>
>>  struct ei_traits<Map<Displacement<_Scalar>, _PacketAccess> >
>>  {
>>    typedef _Scalar Scalar;
>>    typedef Scalar* Coefficients;
>>    enum{
>>      PacketAccess = Unaligned  // check alignement
>>    };
>>  };
>>
>>  template<typename _Scalar, int PacketAccess> // only unaligned is supported
>>  class Map<Displacement<_Scalar>, PacketAccess > : public
>> DisplacementBase<Map<Displacement<_Scalar>, PacketAccess> >{
>>  private:
>>    Map(){}
>>    typedef DisplacementBase<Map<Displacement<_Scalar>, PacketAccess> > Base;
>>
>>  public:
>>    typedef _Scalar Scalar;
>>    using Base::operator*;
>>
>>    typedef _Scalar Scalar;
>>    typedef typename ei_traits<Map<Displacement<Scalar>, PacketAccess>
>>>::Coefficients Coefficients;
>>
>>    inline Map<Displacement<_Scalar>, PacketAccess >(const Scalar* data) :
>> m_data(data) {}
>>
>>    inline Coefficients& data() { return m_data;}
>>    inline const Coefficients& data() const { return m_data;}
>>
>>  protected:
>>    Coefficients m_data;
>>  };
>>
>>  typedef Map<Displacement<double> > DisplacementMapd;
>>  typedef Map<Displacement<float> > DisplacementMapf;
>>  typedef Map<Displacement<double>, Aligned> DisplacementMapAlignedd;
>>  typedef Map<Displacement<float>, Aligned> DisplacementMapAlignedf;
>>
>>
>>  template<class Derived> inline Displacement<typename
>> ei_traits<Derived>::Scalar > DisplacementBase<Derived>::inverse() const{
>>    return Displacement<Scalar>(this->getRotation3D().inverse(),      //
>> rot^-1
>>      -(this->getRotation3D()*this->getTranslation()));               //
>> -rot*trans
>>  }
>>
>>  //
>>  // [ R1   p1 ] * [ R2   p2 ] = [ R1*R2   R1*p2 + p1 ]
>>  // [ 0    1  ]   [ 0    1  ]   [   0          1     ]
>>  //
>>  template <class Derived>
>>  template <class OtherDerived>
>>  EIGEN_STRONG_INLINE Displacement<typename ei_traits<Derived>::Scalar>
>> DisplacementBase<Derived>::operator* (const DisplacementBase<OtherDerived>&
>> other) const
>>  {
>>    EIGEN_STATIC_ASSERT((ei_is_same_type<typename Derived::Scalar, typename
>> OtherDerived::Scalar>::ret),
>>      YOU_MADE_A_PROGRAMMING_MISTAKE)
>>
>>      return Displacement<Scalar>(this->getRotation3D() *
>> other.getRotation3D(),                              // rot1 * rot2
>>      this->getRotation3D() * other.getTranslation() +
>> this->getTranslation()); // rot1 * trans2 + trans1
>>  }
>>
>>  //
>>  // [ R1   p1 ] * [ p2 ] = [ R1*p2 + p1 ]
>>  //
>>  template <class Derived>
>>  template<class OtherDerived>
>>  EIGEN_STRONG_INLINE typename DisplacementBase<Derived>::Vector3
>> DisplacementBase<Derived>::operator*(const MatrixBase<OtherDerived>& v)
>> const
>>  {
>>    return Vector3(this->getRotation()*v + this->getTranslation());
>>  }
>>
>>  //
>>  // Ad_H * T =  [ R          0 ] * [ w ] = [       R*w   ]
>>  //             [ ptilde*R   R ]   [ v ]   [ (p^R*w)+R*v ]
>>  //
>>  template<class Derived>
>>  template<class OtherDerived> inline Twist<typename
>> ei_traits<Derived>::Scalar> DisplacementBase<Derived>::adjoint(const
>> TwistBase<OtherDerived>& t) const{
>>
>>    Vector3 Rw = this->getRotation3D() * t.getAngularVelocity();
>>           // R*w
>>
>>    return Twist<Scalar>(AngularVelocity<Scalar>(Rw),
>>      this->getTranslation().cross(Rw) +
>> this->getRotation3D()*t.getLinearVelocity()); // (p^R*w)+R*v
>>  }
>>
>>  //
>>  // (Ad_H)' * W =  [ R'   -R'*ptilde ] * [ t ] = [  R'*(f^p+t) ]
>>  //                [ 0           R'  ]   [ f ]   [      R'*f   ]
>>  //
>>  template<class Derived>
>>  template<class OtherDerived> inline Wrench<typename
>> ei_traits<Derived>::Scalar> DisplacementBase<Derived>::adjointTr(const
>> WrenchBase<OtherDerived>& w) const{
>>
>>    return Wrench<Scalar>(this->getRotation3D().inverse() *
>> (w.getForce.cross(this->getTranslation) + w.getTorque()),  // R'*(f^p+t)
>>      this->getRotation3D().inverse() * w.getForce());     // R'*f
>>  }
>>
>>  //
>>  // Ad_H =  [ R          0 ]
>>  //         [ ptilde*R   R ]
>>  //
>>  template<class Derived>
>>  inline Matrix<typename ei_traits<Derived>::Scalar, 6, 6>
>> DisplacementBase<Derived>::adjoint() const{
>>    Matrix<Scalar, 6, 6> res;
>>
>>    res.block<3,3>(0,0) = this->getRotation3D().toRotationMatrix();
>>    res.block<3,3>(3,3) = res.block<3,3>(0,0);
>>
>>
>>                res.block<3,3>(3,0) = (Matrix<Scalar,3,3>() << 0,
>> -this->getTranslation()[2], this->getTranslation()[1],  // 0 -z  y
>>
>> this->getTranslation()[2], 0, -this->getTranslation()[0],    // z  0 -x
>>
>> -this->getTranslation()[1], this->getTranslation()[0], 0)              //-y
>>  x  0
>>                          .finished()*res.block<3,3>(0,0);
>>
>>    res.block<3,3>(0,3).setZero();
>>
>>    return res;
>>  }
>>
>>  //
>>  // ((Ad_H)')^-1 = [ R      ptilde*R ]
>>  //                [ 0           R   ]
>>  //
>>  template<class Derived>
>>  Matrix<typename ei_traits<Derived>::Scalar, 6, 6>
>> DisplacementBase<Derived>::adjointInv() const{
>>    Matrix<Scalar, 6, 6> res;
>>
>>    res.block<3,3>(0,0) =
>> this->getRotation3D().toRotationMatrix().transpose();
>>    res.block<3,3>(3,3) = res.block<3,3>(0,0);
>>
>>
>>                res.block<3,3>(0,3) = -(Matrix<Scalar,3,3>() << 0,
>> -this->getTranslation()[2], this->getTranslation()[1],  // 0 -z  y
>>
>> this->getTranslation()[2], 0, -this->getTranslation()[0],    // z  0 -x
>>
>> -this->getTranslation()[1], this->getTranslation()[0], 0)              //-y
>>  x  0
>>                          .finished()*res.block<3,3>(0,0);
>>
>>    res.block<3,3>(3,0).setZero();
>>
>>    return res;
>>  }
>>
>>  template<class Derived>
>>  template<class OtherDerived> inline Twist<typename
>> ei_traits<Derived>::Scalar> DisplacementBase<Derived>::changeBase(const
>> TwistBase<OtherDerived>& t) const{
>>
>>    return Twist<Scalar>(this->getRotation3D()* t.getAngularVelocity(),
>>      this->getRotation3D()* t.getLinearVelocity());
>>  }
>>
>>  template<class Derived>
>>  template<class OtherDerived> inline Wrench<typename
>> ei_traits<Derived>::Scalar> DisplacementBase<Derived>::changeBase(const
>> WrenchBase<OtherDerived>& w) const{
>>
>>    return Wrench<Scalar>(this->getRotation3D()* w.getTorque(),
>>      this->getRotation3D()* w.getForce());
>>  }
>>
>>  template<class Derived>
>>  template<class OtherDerived> inline Twist<typename
>> ei_traits<Derived>::Scalar>
>> DisplacementBase<Derived>::changeReductionPoint(const
>> TwistBase<OtherDerived>& t) const{
>>
>>    return Twist<Scalar>(t.getAngularVelocity(),
>>      t.getLinearVelocity() +
>> t.getAngularVelocity().cross(this->getTranslation()));
>>  }
>>
>>  template<class Derived>
>>  template<class OtherDerived> inline Wrench<typename
>> ei_traits<Derived>::Scalar>
>> DisplacementBase<Derived>::changeReductionPoint(const
>> WrenchBase<OtherDerived>& w) const{
>>
>>    return Wrench<Scalar>(w.getTorque() +
>> w.getForce().cross(this->getTranslation()),
>>      w.getForce());
>>  }
>>
>>  // log SE(3) -> se(3)
>>  //
>>  // log [ R   p ] = [ wtilde   B*p ]
>>  //     [ 0   1 ]   [   0       0  ]
>>  //
>>  // wtilde = log R
>>  // B = I - 0.5*wtilde + (2*sin||w||-||w||(1+cos||w||))/(2*||w||^2*sin||w||)
>> * wtilde^2
>>  //
>>  template<class Derived> inline Twist<typename ei_traits<Derived>::Scalar>
>> DisplacementBase<Derived>::log(const Scalar precision) const{
>>    AngularVelocity ang = this->getRotation3D().log(precision);
>>
>>    const Scalar n2 = ang.squaredNorm();  // ||wtidle||^2
>>
>>    if(n2 < precision)
>>      return Twist<Scalar>(ang, this->getTranslation());
>>    else{
>>      const Scalar n = ei_sqrt(n2);
>>      const Scalar sn = ei_sin(n);
>>
>>      Scalar val = (2.0 * sn - n * (1.0 + ei_cos(n))) / (2.0 *n2 * sn);
>>
>>      Vector3d lin = -0.5*ang.cross(this->getTranslation());
>>      lin += (1. - val * n2 ) * this->getTranslation();
>>      lin += val * ang.dot(this->getTranslation()) * ang;
>>
>>      return Twist<Scalar>(ang, lin);
>>    }
>>  }
>> }
>>
>> #endif
>>
>>
>> #ifndef LGSM_ROTATION3D_H
>> #define LGSM_ROTATION3D_H
>>
>> /**************************************
>> Rotation3D Base
>> **************************************/
>>
>> namespace Eigen{
>>
>>  template<typename Derived>
>>  struct ei_traits<Rotation3DBase<Derived> >
>>  {
>>    typedef typename ei_traits<Derived>::Scalar Scalar;
>>    enum {
>>      PacketAccess = ei_traits<Derived>::PacketAccess
>>    };
>>  };
>>
>>  template<class Derived>
>>  class Rotation3DBase : public QuaternionBase<Derived>{
>>  protected:
>>    typedef QuaternionBase<Derived> Base;
>>  public:
>>    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Rotation3DBase<Derived>)
>>    using Base::operator*=;
>>    using Base::operator*;
>>    using Base::derived;
>>
>>    typedef typename ei_traits<Derived>::Scalar Scalar;
>>    typedef typename Base::Matrix3 Matrix3;
>>    typedef typename Base::Vector3 Vector3;
>>
>>    EIGEN_STRONG_INLINE Matrix3 adjoint(void) const {return
>> this->toRotationMatrix();}
>>    EIGEN_STRONG_INLINE Matrix3 transposedAdjoint(void) const {return
>> this->toRotationMatrix().transpose();} // check optimisation
>>
>>    template<class OtherDerived> EIGEN_STRONG_INLINE const Rotation3D<Scalar>
>> operator*(const Rotation3DBase<OtherDerived>& other) const{
>>      return Rotation3D<Scalar>(this->Base::operator*(other));
>>    }
>>
>>    AngularVelocity<Scalar> log(const Scalar precision = precision()) const;
>>    const Rotation3D<Scalar> inverse(void) const;
>>
>>    // return v = ad(R)*u -> call v = rot3d*u
>>    template<class VectorDerived, class VectorOtherDerived>
>> EIGEN_STRONG_INLINE void applyAdjoint(MatrixBase<VectorDerived>& v, const
>> MatrixBase<VectorOtherDerived>& u) const {
>>      v = *this * u;
>>    }
>>
>>    static Scalar precision();  // [XXX] used
>> precision<Rotation3DBase<Derived> instead ?
>>
>>    //Vector3 as(Scalar precision = 1.0e-5/*precision<Rotation3DBase<Derived>
>>>()*/) const;
>>  };
>>
>>  /**************************************
>>  Rotation3D
>>  **************************************/
>>
>>  template<typename Scalar> class Rotation3D;
>>
>>  template<typename _Scalar>
>>  struct ei_traits<Rotation3D<_Scalar> > : ei_traits<Quaternion<_Scalar> >{};
>>
>>  template<typename _Scalar>
>>  class Rotation3D : public Rotation3DBase<Rotation3D<_Scalar> >{
>>    typedef Rotation3DBase<Rotation3D<_Scalar> > Base;
>>  public:
>>    typedef _Scalar Scalar;
>>
>>    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Rotation3D<Scalar>)
>>    using Base::operator*=;
>>
>>    typedef typename ei_traits<Rotation3D<Scalar> >::Coefficients
>> Coefficients;
>>    typedef typename Base::AngleAxisType AngleAxisType;
>>
>>    /** Default constructor leaving the quaternion uninitialized. */
>>    inline Rotation3D() {}
>>
>>    /** Constructs and initializes the quaternion \f$ w+xi+yj+zk \f$ from
>>    * its four coefficients \a w, \a x, \a y and \a z.
>>    *
>>    * \warning Note the order of the arguments: the real \a w coefficient
>> first,
>>    * while internally the coefficients are stored in the following order:
>>    * [\c x, \c y, \c z, \c w]
>>    */
>>    inline Rotation3D(const Scalar w, const Scalar x, const Scalar y, const
>> Scalar z) : m_coeffs(x, y, z, w){}
>>
>>    template<class Derived> inline Rotation3D(const Scalar w, const
>> MatrixBase<Derived>& other) : m_coeffs(other[0], other[1], other[2], w){} //
>> check other size
>>
>>    /** Constructs and initialize a quaternion from the array data
>>    * This constructor is also used to map an array */
>>    inline Rotation3D(const Scalar* data) : m_coeffs(data) {}
>>
>>    /** Copy constructor */
>>    template<class Derived> EIGEN_STRONG_INLINE Rotation3D(const
>> Rotation3DBase<Derived>& other) {
>>      this->Base::operator=(other);
>>    }
>>
>>        /** Copy constructor */
>>    template<class Derived> explicit EIGEN_STRONG_INLINE Rotation3D(const
>> QuaternionBase<Derived>& other) {
>>      this->Base::operator=(other);
>>    }
>>
>>
>>    /** Constructs and initializes a quaternion from the angle-axis \a aa */
>>    explicit inline Rotation3D(const AngleAxisType& aa) { *this = aa; }
>>
>>    /** Constructs and initializes a quaternion from either:
>>    *  - a rotation matrix expression,
>>    *  - a 4D vector expression representing quaternion coefficients.
>>    */
>>    template<typename Derived>
>>    explicit inline Rotation3D(const MatrixBase<Derived>& other) { *this =
>> other; }
>>
>>    inline Coefficients& coeffs() { return m_coeffs;}
>>    inline const Coefficients& coeffs() const { return m_coeffs;}
>>
>>    template<class VectorDerived>
>>    static Rotation3D<Scalar> Exp(const MatrixBase<VectorDerived>& expVector,
>> Scalar precision = Base::precision());
>>
>>  protected:
>>    Coefficients m_coeffs;
>>  };
>>
>>  typedef Rotation3D<double> Rotation3Dd;
>>  typedef Rotation3D<float> Rotation3Df;
>>
>>  /**************************************
>>  Map<Rotation3D>
>>  **************************************/
>>
>>  template<typename _Scalar, int _PacketAccess>
>>  struct ei_traits<Map<Rotation3D<_Scalar>, _PacketAccess> >:
>>    ei_traits<Map<Quaternion<_Scalar>, _PacketAccess> >{};
>>
>>  template<typename _Scalar, int PacketAccess>
>>  class Map<Rotation3D<_Scalar>, PacketAccess >
>>    : public Rotation3DBase<Map<Rotation3D<_Scalar>, PacketAccess> >
>>  {
>>    typedef Rotation3DBase<Map<Rotation3D<_Scalar>, PacketAccess> > Base;
>>        public:
>>    typedef _Scalar Scalar;
>>                typedef Map<Rotation3D<Scalar>, PacketAccess > MapRot;
>>
>>        private:
>>                Map<Rotation3D<Scalar>, PacketAccess >();
>>                //Map<Rotation3D<Scalar>, PacketAccess >(const
>> Map<Rotation3D<Scalar>, PacketAccess>&);
>>
>>    typedef Rotation3DBase<Map<Rotation3D<_Scalar>, PacketAccess> > Base;
>>  public:
>>        EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(MapRot)
>>    using Base::operator*=;
>>
>>    typedef typename ei_traits<Map<Rotation3D<Scalar>, PacketAccess>
>>>::Coefficients Coefficients;
>>
>>    inline Map(const Scalar* coeffs) : m_coeffs(coeffs) {}
>>
>>    inline Coefficients& coeffs() { return m_coeffs;}
>>    inline const Coefficients& coeffs() const { return m_coeffs;}
>>
>>  protected:
>>    Coefficients m_coeffs;
>>  };
>>
>>  typedef Map<Rotation3D<double> > Rotation3DMapd;
>>  typedef Map<Rotation3D<float> > Rotation3DMapf;
>>  typedef Map<Rotation3D<double>, Aligned> Rotation3DMapAlignedd;
>>  typedef Map<Rotation3D<float>, Aligned> Rotation3DMapAlignedf;
>>
>>  template <class Derived>
>>  inline const Rotation3D<typename ei_traits<Derived>::Scalar>
>> Rotation3DBase<Derived>::inverse() const
>>  {
>>    return Rotation3D<Scalar>(this->w(),-this->x(),-this->y(),-this->z());
>>  }
>>
>>  template<class Derived>
>>  inline typename ei_traits<Derived>::Scalar
>> Rotation3DBase<Derived>::precision(){
>>    return ei_sqrt(Eigen::precision<ei_traits<Derived>::Scalar>());
>>  }
>>
>>  template<typename Scalar>
>>  template<class VectorDerived>
>>  inline Rotation3D<Scalar> Rotation3D<Scalar>::Exp(const
>> MatrixBase<VectorDerived>& vec, Scalar precision){
>>    const double n = vec.norm();
>>    const double w = ei_cos(n * 0.5);
>>
>>    if (n < precision)
>>      return Rotation3D<Scalar>(w, vec);
>>    else
>>      return Rotation3D<Scalar>(w, ei_sin(n * 0.5) / n * vec);
>>  }
>>
>>  template<class Derived>
>>  inline AngularVelocity<typename ei_traits<Derived>::Scalar>
>> Rotation3DBase<Derived>::log(Scalar precision) const{
>>    const Scalar n2 = this->vec().squaredNorm();
>>    const Scalar n = ei_sqrt(n2);
>>
>>    if (n < precision)
>>      return AngularVelocity<Scalar>((2 / this->w()) * this->vec());
>>    else
>>      return AngularVelocity<Scalar>(ei_atan2(2 * n * this->w(), this->w() *
>> this->w() - n2) / n * this->vec());
>>  }
>> }
>>
>> #endif
>>
>>
>> #ifndef LGSM_TORQUE_H
>> #define LGSM_TORQUE_H
>>
>> /**************************************
>> Torque Base
>> **************************************/
>>
>> namespace Eigen{
>>  template<typename Derived>
>>  struct ei_traits<TorqueBase<Derived> >
>>  {
>>    typedef typename ei_traits<Derived>::Scalar Scalar;
>>    enum {
>>      PacketAccess = ei_traits<Derived>::PacketAccess
>>    };
>>  };
>>
>>
>>  template<class Derived>
>>  class TorqueBase : public Eigen::MatrixBase<Derived>{
>>    typedef Eigen::MatrixBase<Derived> Base;
>>  public:
>>    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(TorqueBase)
>>
>>    typedef typename Eigen::ei_traits<Derived>::Scalar Scalar;
>>
>>    inline int rows() const { return 3; }
>>
>>    inline int cols() const { return 1; }
>>
>>    inline Scalar& coeffRef(int row, int col)
>>    {
>>      return this->derived().data().coeffRef(row, col);
>>    }
>>
>>    inline const Scalar coeff(int row, int col) const
>>    {
>>      return this->derived().data().coeff(row, col);
>>    }
>>
>>    inline Scalar& coeffRef(int index)
>>    {
>>      return this->derived().data().coeffRef(index);
>>    }
>>
>>    inline const Scalar coeff(int index) const
>>    {
>>      return this->derived().data().coeff(index);
>>    }
>>  };
>>
>>  /**************************************
>>  Torque
>>  **************************************/
>>
>>  template<typename _Scalar>
>>  struct ei_traits<Torque<_Scalar> > : ei_traits<Matrix<_Scalar,3,1> >
>>  {
>>    typedef Matrix<_Scalar,3,1> TorqueVectorType;
>>    enum{
>>      PacketAccess = Aligned
>>    };
>>  };
>>
>>  template<typename _Scalar>
>>  class Torque : public TorqueBase<Torque<_Scalar> >{
>>  public:
>>    _EIGEN_GENERIC_PUBLIC_INTERFACE(Torque, TorqueBase<Torque>)
>>    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Torque)
>>
>>    typedef typename Eigen::ei_traits<Torque<Scalar> >::TorqueVectorType
>> TorqueVectorType;
>>
>>    inline Torque() : m_data(Eigen::MatrixBase<Torque<Scalar> >::Zero()){}
>>    inline Torque(const Scalar vx, const Scalar vy, const Scalar vz) :
>> m_data(vx, vy, vz) {}
>>
>>    template<typename OtherDerived>
>>    EIGEN_STRONG_INLINE Torque(const Eigen::MatrixBase<OtherDerived>& other)
>>      : m_data(other)
>>    {
>>      //  _check_template_params(); [XXX] what what ?
>>      //  _set_noalias(other);
>>    }
>>
>>    EIGEN_STRONG_INLINE Torque(const Torque& other)
>>    {
>>      Base::operator=(other);
>>    }
>>
>>    TorqueVectorType& data(){
>>      return m_data;
>>    }
>>
>>    const TorqueVectorType& data() const {
>>      return m_data;
>>    }
>>
>>  protected:
>>    TorqueVectorType m_data;
>>  };
>>
>>  typedef Torque<double> Torqued;
>>  typedef Torque<float> Torquef;
>>
>>  /**************************************
>>  Map<Torque>
>>  **************************************/
>>
>>  template<typename _Scalar, int _PacketAccess>
>>  struct ei_traits<Map<Torque<_Scalar>, _PacketAccess> > :
>> ei_traits<Map<Matrix<_Scalar,3,1>, _PacketAccess> >
>>  {
>>    typedef Map<Matrix<_Scalar,3,1>, _PacketAccess > TorqueVectorType;
>>  };
>>
>>  template<typename _Scalar, int PacketAccess>
>>  class Map<Torque<_Scalar>, PacketAccess > : public
>> TorqueBase<Map<Torque<_Scalar>, PacketAccess> > {
>>  private:
>>    typedef Map<Torque<_Scalar>, PacketAccess > MapTorque;
>>  public:
>>    _EIGEN_GENERIC_PUBLIC_INTERFACE(MapTorque, TorqueBase<MapTorque>)
>>    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(MapTorque)
>>
>>  private:
>>    Map(){};
>>
>>  public:
>>
>>    typedef typename ei_traits<Map<Torque<Scalar>, PacketAccess>
>>>::TorqueVectorType TorqueVectorType;
>>
>>    inline Map(const Map<Torque<Scalar>, PacketAccess>& mt) :
>> m_data(mt.data().data()){};
>>    inline Map(const _Scalar* data) : m_data(data) {}
>>
>>    inline TorqueVectorType& data() { return m_data;}
>>    inline const TorqueVectorType& data() const { return m_data;}
>>
>>  protected:
>>    TorqueVectorType m_data;
>>  };
>>
>>  typedef Map<Torque<double> > TorqueMapd;
>>  typedef Map<Torque<float> > TorqueMapf;
>>  typedef Map<Torque<double>, Aligned> TorqueMapAlignedd;
>>  typedef Map<Torque<float>, Aligned> TorqueMapAlignedf;
>>
>> }
>>
>> #endif
>>
>>
>> #ifndef LGSM_TWIST_H
>> #define LGSM_TWIST_H
>>
>> /**************************************
>>        Twist Base
>> **************************************/
>>
>> namespace Eigen{
>>  template<typename Derived>
>>  struct ei_traits<TwistBase<Derived> >
>>  {
>>    typedef typename ei_traits<Derived>::Scalar Scalar;
>>    enum {
>>      PacketAccess = ei_traits<Derived>::PacketAccess
>>    };
>>  };
>>
>>        template<class Derived>
>>        class TwistBase : public MatrixBase<Derived>{
>>    typedef Eigen::MatrixBase<Derived> Base;
>>        public:
>>    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(TwistBase)
>>
>>                typedef typename ei_traits<Derived>::Scalar Scalar;
>>                typedef typename ei_traits<Derived>::TwistVectorType
>> TwistVectorType;
>>
>>                inline Map<AngularVelocity<Scalar> > getAngularVelocity(){
>>                        return Map<AngularVelocity<Scalar>
>>>(this->derived().data().data());
>>                }
>>
>>                inline Map<AngularVelocity<Scalar> > getAngularVelocity()
>> const {
>>                        return Map<AngularVelocity<Scalar>
>>>(this->derived().data().data());
>>                }
>>
>>          EIGEN_STRONG_INLINE VectorBlock<TwistVectorType, 3>
>> getLinearVelocity(){
>>                        return this->derived().data().template start<3>();
>>                }
>>
>>          EIGEN_STRONG_INLINE VectorBlock<TwistVectorType, 3>
>> getLinearVelocity() const{
>>                        return this->derived().data().template start<3>();
>>                }
>>
>>                inline int rows() const { return 6; }
>>
>>                inline int cols() const { return 1; }
>>
>>                inline Scalar& coeffRef(int row, int col)
>>                {
>>                        return this->derived().data().coeffRef(row, col);
>>                }
>>
>>                inline const Scalar coeff(int row, int col) const
>>                {
>>                        return this->derived().data().coeff(row, col);
>>                }
>>
>>                inline Scalar& coeffRef(int index)
>>                {
>>                        return this->derived().data().coeffRef(index);
>>                }
>>
>>                inline const Scalar coeff(int index) const
>>                {
>>                        return this->derived().data().coeff(index);
>>                }
>>        };
>>
>>        /**************************************
>>                                                                        Twist
>>        **************************************/
>>
>>  template<typename _Scalar>
>>  struct ei_traits<Twist<_Scalar> > : ei_traits<Matrix<_Scalar,6,1> >
>>  {
>>    typedef Matrix<_Scalar,6,1> TwistVectorType;
>>    enum{
>>      PacketAccess = Aligned
>>    };
>>  };
>>
>>        template<typename _Scalar>
>>        class Twist : public TwistBase<Twist<_Scalar> >{
>>        public:
>>    _EIGEN_GENERIC_PUBLIC_INTERFACE(Twist, TwistBase<Twist>)
>>
>>    #if !(defined(_MSC_VER) && !defined(__INTEL_COMPILER))
>>    EIGEN_STRONG_INLINE Twist& operator=(const Twist& other)
>>    {
>>      // the 'false' below means to enforce lazy evaluation. We don't use
>> lazyAssign() because
>>      // it wouldn't allow to copy a row-vector into a column-vector. (Matrix
>> l.664)
>>      return ei_assign_selector<Twist,Twist,false>::run(*this,
>> other.derived());
>>    }
>>    #endif
>>
>>    using Base::operator=;
>>    using Base::operator*=;
>>    using Base::operator/=;
>>    using Base::operator+=;
>>    using Base::operator-=;
>>
>>                typedef typename ei_traits<Twist<Scalar> >::TwistVectorType
>> TwistVectorType;
>>
>>    inline Twist() : m_data(Eigen::MatrixBase<Twist<Scalar> >::Zero()){}
>>                inline Twist(const Scalar vx, const Scalar vy, const Scalar
>> vz, const Scalar wx, const Scalar wy, const Scalar wz)
>>                        { m_data << vx, vy, vz, wx, wy, wz;}
>>
>>                template<class LinDerived, class AngDerived> inline
>> Twist(const AngularVelocityBase<AngDerived>& a, const
>> MatrixBase<LinDerived>& l)
>>                {
>>                        this->getLinearVelocity() = l;
>>                        this->getAngularVelocity() = a;
>>                }
>>
>>    template<typename OtherDerived>
>>    EIGEN_STRONG_INLINE Twist(const MatrixBase<OtherDerived>& other)
>>      : m_data(other)
>>    {
>>      //  _check_template_params(); [XXX] what what ?
>>      //  _set_noalias(other);
>>    }
>>
>>    EIGEN_STRONG_INLINE Twist(const Twist& other)
>>      : m_data(other)
>>    {}
>>
>>    TwistVectorType& data(){
>>      return m_data;
>>    }
>>
>>                const TwistVectorType& data() const {
>>                        return m_data;
>>                }
>>
>>        protected:
>>                TwistVectorType m_data;
>>        };
>>
>>        typedef Twist<double> Twistd;
>>        typedef Twist<float> Twistf;
>>
>>
>>        /**************************************
>>
>>  Map<Twist>
>>        **************************************/
>>
>>  template<typename _Scalar, int _PacketAccess>
>>  struct ei_traits<Map<Twist<_Scalar>, _PacketAccess> > :
>> ei_traits<Map<Matrix<_Scalar,6,1>, _PacketAccess> >
>>  {
>>    typedef Map<Matrix<_Scalar,6,1>, _PacketAccess > TwistVectorType;
>>  };
>>
>>  template<typename _Scalar, int PacketAccess>
>>  class Map<Twist<_Scalar>, PacketAccess > : public
>> TwistBase<Map<Twist<_Scalar>, PacketAccess> > {
>>  private:
>>    typedef Map<Twist<_Scalar>, PacketAccess > MapTwist;
>>  public:
>>    _EIGEN_GENERIC_PUBLIC_INTERFACE(MapTwist, TwistBase<MapTwist>)
>>    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(MapTwist)
>>
>>  private:
>>    Map(){};
>>  public:
>>    typedef typename ei_traits<Map<Twist<Scalar>, PacketAccess>
>>>::TwistVectorType TwistVectorType;
>>
>>    inline Map<Twist<_Scalar>, PacketAccess >(const _Scalar* data) :
>> m_data(data) {}
>>
>>    inline TwistVectorType& data() { return m_data;}
>>    inline const TwistVectorType& data() const { return m_data;}
>>
>>  protected:
>>    TwistVectorType m_data;
>>  };
>>
>>        typedef Map<Twist<double> > TwistMapd;
>>        typedef Map<Twist<float> > TwistMapf;
>>        typedef Map<Twist<double>, Aligned> TwistMapAlignedd;
>>        typedef Map<Twist<float>, Aligned> TwistMapAlignedf;
>> }
>>
>> #endif
>>
>>
>> #ifndef LGSM_WRENCH_H
>> #define LGSM_WRENCH_H
>>
>> /**************************************
>> Wrench Base
>> **************************************/
>>
>> namespace Eigen{
>>  template<typename Derived>
>>  struct ei_traits<WrenchBase<Derived> >
>>  {
>>    typedef typename ei_traits<Derived>::Scalar Scalar;
>>    enum {
>>      PacketAccess = ei_traits<Derived>::PacketAccess
>>    };
>>  };
>>
>>  template<class Derived>
>>  class WrenchBase : public MatrixBase<Derived>{
>>    typedef Eigen::MatrixBase<Derived> Base;
>>  public:
>>    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(WrenchBase)
>>
>>    typedef typename ei_traits<Derived>::Scalar Scalar;
>>    typedef typename ei_traits<Derived>::WrenchVectorType WrenchVectorType;
>>
>>    EIGEN_STRONG_INLINE Map<Torque<Scalar> > getTorque(){
>>      return Map<Torque<Scalar> >(this->derived().data().data());
>>    }
>>    EIGEN_STRONG_INLINE Map<Torque<Scalar> > getTorque() const {
>>      return Map<Torque<Scalar> >(this->derived().data().data());
>>    }
>>
>>          EIGEN_STRONG_INLINE VectorBlock<WrenchVectorType, 3> getForce(){
>>                        return this->derived().data().template start<3>();
>>                }
>>
>>          EIGEN_STRONG_INLINE VectorBlock<WrenchVectorType, 3> getForce()
>> const{
>>                        return this->derived().data().template start<3>();
>>                }
>>
>>    inline int rows() const { return 6; }
>>
>>    inline int cols() const { return 1; }
>>
>>    inline Scalar& coeffRef(int row, int col)
>>    {
>>      return this->derived().data().coeffRef(row, col);
>>    }
>>
>>    inline const Scalar coeff(int row, int col) const
>>    {
>>      return this->derived().data().coeff(row, col);
>>    }
>>
>>    inline Scalar& coeffRef(int index)
>>    {
>>      return this->derived().data().coeffRef(index);
>>    }
>>
>>    inline const Scalar coeff(int index) const
>>    {
>>      return this->derived().data().coeff(index);
>>    }
>>  };
>>
>>  /**************************************
>>  Wrench
>>  **************************************/
>>
>>  template<typename _Scalar>
>>  struct ei_traits<Wrench<_Scalar> > : ei_traits<Matrix<_Scalar,6,1> >
>>  {
>>    typedef Matrix<_Scalar,6,1> WrenchVectorType;
>>    enum{
>>      PacketAccess = Aligned
>>    };
>>  };
>>
>>  template<typename _Scalar>
>>  class Wrench : public WrenchBase<Wrench<_Scalar> >{
>>  public:
>>    _EIGEN_GENERIC_PUBLIC_INTERFACE(Wrench, WrenchBase<Wrench>)
>>
>>    #if !(defined(_MSC_VER) && !defined(__INTEL_COMPILER))
>>    EIGEN_STRONG_INLINE Wrench& operator=(const Wrench& other)
>>    {
>>      // the 'false' below means to enforce lazy evaluation. We don't use
>> lazyAssign() because
>>      // it wouldn't allow to copy a row-vector into a column-vector. (Matrix
>> l.664)
>>      return ei_assign_selector<Wrench,Wrench,false>::run(*this,
>> other.derived());
>>    }
>>    #endif
>>
>>    using Base::operator=;
>>    using Base::operator*=;
>>    using Base::operator/=;
>>    using Base::operator+=;
>>    using Base::operator-=;
>>
>>    // END sinon utiliser EIGEN_INHERIT_ASSIGNMENT_OPERATORS
>>
>>
>>    //EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Wrench)
>>
>>    typedef typename ei_traits<Wrench<Scalar> >::WrenchVectorType
>> WrenchVectorType;
>>
>>    inline Wrench() : m_data(Eigen::MatrixBase<Wrench<Scalar> >::Zero()){}
>>    inline Wrench(const Scalar fx, const Scalar fy, const Scalar fz, const
>> Scalar tx, const Scalar ty, const Scalar tz)
>>    { m_data << tx, ty, tz, fx, fy, fz;}
>>
>>    template<class ForDerived, class TorDerived> inline Wrench(const
>> MatrixBase<ForDerived>& f, const TorqueBase<TorDerived>& t)
>>    {
>>      this->getTorque() = t;
>>      this->getForce() = f;
>>    }
>>
>>    template<typename OtherDerived>
>>    EIGEN_STRONG_INLINE Wrench(const MatrixBase<OtherDerived>& other)
>>      : m_data(other)
>>    {
>>      // check parameter
>>    }
>>
>>    EIGEN_STRONG_INLINE Wrench(const Wrench& other)
>>      : m_data(other)
>>    {
>>    }
>>
>>    WrenchVectorType& data(){
>>      return m_data;
>>    }
>>
>>    const WrenchVectorType& data() const {
>>      return m_data;
>>    }
>>
>>  protected:
>>    WrenchVectorType m_data;
>>  };
>>
>>  typedef Wrench<double> Wrenchd;
>>  typedef Wrench<float> Wrenchf;
>>
>>
>>  /**************************************
>>  Map<Wrench>
>>  **************************************/
>>
>>  template<typename _Scalar, int _PacketAccess>
>>  struct ei_traits<Map<Wrench<_Scalar>, _PacketAccess> > :
>> ei_traits<Map<Matrix<_Scalar,6,1>, _PacketAccess> >
>>  {
>>    typedef Map<Matrix<_Scalar,6,1>, _PacketAccess > WrenchVectorType;
>>  };
>>
>>  template<typename _Scalar, int PacketAccess>
>>  class Map<Wrench<_Scalar>, PacketAccess > : public
>> WrenchBase<Map<Wrench<_Scalar>, PacketAccess> > {
>>  private:
>>    typedef Map<Wrench<_Scalar>, PacketAccess > MapWrench;
>>  public:
>>    _EIGEN_GENERIC_PUBLIC_INTERFACE(MapWrench, WrenchBase<MapWrench>)
>>    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(MapWrench)
>>
>>  private:
>>    Map(){};
>>
>>  public:
>>    typedef typename ei_traits<Map<Wrench<Scalar>, PacketAccess>
>>>::WrenchVectorType WrenchVectorType;
>>
>>    inline Map(const Map<Wrench<Scalar>, PacketAccess>& mw) :
>> m_data(mw.data().data()){};
>>    inline Map(const _Scalar* data) : m_data(data) {}
>>
>>    inline WrenchVectorType& data() { return m_data;}
>>    inline const WrenchVectorType& data() const { return m_data;}
>>
>>  protected:
>>    WrenchVectorType m_data;
>>  };
>>
>>  typedef Map<Wrench<double> > WrenchMapd;
>>  typedef Map<Wrench<float> > WrenchMapf;
>>  typedef Map<Wrench<double>, Aligned> WrenchMapAlignedd;
>>  typedef Map<Wrench<float>, Aligned> WrenchMapAlignedf;
>> }
>>
>> #endif
>>
>>
>> #ifndef LGSM_ANGULAR_VELOCITY_H
>> #define LGSM_ANGULAR_VELOCITY_H
>>
>> /**************************************
>>        AngularVelocity Base
>> **************************************/
>>
>> namespace Eigen{
>>  template<typename Derived>
>>  struct ei_traits<AngularVelocityBase<Derived> >
>>  {
>>    typedef typename ei_traits<Derived>::Scalar Scalar;
>>    enum {
>>      PacketAccess = ei_traits<Derived>::PacketAccess
>>    };
>>  };
>>
>>
>>        template<class Derived>
>>        class AngularVelocityBase : public MatrixBase<Derived>{
>>                typedef MatrixBase<Derived> Base;
>>        public:
>>    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(AngularVelocityBase)
>>
>>                typedef typename ei_traits<Derived>::Scalar Scalar;
>>
>>                inline int rows() const { return 3; }
>>
>>                inline int cols() const { return 1; }
>>
>>                inline Scalar& coeffRef(int row, int col)
>>                {
>>                        return this->derived().data().coeffRef(row, col);
>>                }
>>
>>                inline const Scalar coeff(int row, int col) const
>>                {
>>                        return this->derived().data().coeff(row, col);
>>                }
>>
>>                inline Scalar& coeffRef(int index)
>>                {
>>                        return this->derived().data().coeffRef(index);
>>                }
>>
>>                inline const Scalar coeff(int index) const
>>                {
>>                        return this->derived().data().coeff(index);
>>                }
>>
>>                template<class OtherDerived> EIGEN_STRONG_INLINE
>> AngularVelocity<Scalar> bracket(const AngularVelocityBase<OtherDerived>&
>> ang) const {
>>                        return this->cross(ang);
>>                }
>>
>>                // applyAdjoint or adjoint ??
>>                template<class OtherDerived> EIGEN_STRONG_INLINE const
>> AngularVelocity<Scalar> adjoint(const AngularVelocityBase<OtherDerived>&
>> ang) const {
>>                        return this->bracket(ang);
>>                }
>>
>>                template<class OtherDerived> EIGEN_STRONG_INLINE const
>> Torque<Scalar> coadjoint(const TorqueBase<OtherDerived>& t) const {
>>                        return Torque<Scalar>(-this->cross(t));
>>                }
>>
>>                const EIGEN_STRONG_INLINE Matrix<Scalar, 3, 3> adjoint()
>> const;
>>                const EIGEN_STRONG_INLINE Matrix<Scalar, 3, 3> coadjoint()
>> const{
>>                        return -this->adjoint();
>>                }
>>        };
>>
>>        /**************************************
>>                                        AngularVelocity
>>        **************************************/
>>  template<typename _Scalar>
>>  struct ei_traits<AngularVelocity<_Scalar> > : ei_traits<Matrix<_Scalar,3,1>
>>>
>>  {
>>    typedef Matrix<_Scalar,3,1> AngVelVectorType;
>>    enum{
>>      PacketAccess = Aligned
>>    };
>>  };
>>
>>        template<typename _Scalar>
>>        class AngularVelocity : public
>> AngularVelocityBase<AngularVelocity<_Scalar> >{
>>        public:
>>                _EIGEN_GENERIC_PUBLIC_INTERFACE(AngularVelocity,
>> AngularVelocityBase<AngularVelocity>)
>>                EIGEN_INHERIT_ASSIGNMENT_OPERATORS(AngularVelocity)
>>
>>                typedef typename ei_traits<AngularVelocity<Scalar>
>>>::AngVelVectorType AngVelVectorType;
>>
>>                inline AngularVelocity() :
>> m_data(MatrixBase<AngularVelocity<Scalar> >::Zero()){}
>>                inline AngularVelocity(const Scalar vx, const Scalar vy,
>> const Scalar vz) : m_data(vx, vy, vz){}
>>
>>                template<typename OtherDerived>
>>                EIGEN_STRONG_INLINE AngularVelocity(const
>> MatrixBase<OtherDerived>& other)
>>                {
>>      this->Base::operator=(other);
>>                //  _check_template_params(); [XXX] ?
>>                //  _set_noalias(other);
>>                }
>>
>>    EIGEN_STRONG_INLINE AngularVelocity(const AngularVelocity& other)
>>    {
>>      Base::operator=(other);
>>    }
>>
>>                AngVelVectorType& data(){
>>                        return m_data;
>>                }
>>                const AngVelVectorType& data() const {
>>                        return m_data;
>>                }
>>
>>        protected:
>>                AngVelVectorType m_data;
>>        };
>>
>>        typedef AngularVelocity<double> AngularVelocityd;
>>        typedef AngularVelocity<float> AngularVelocityf;
>>
>>        /**************************************
>>                                        Map<AngularVelocity>
>>        **************************************/
>>
>>  template<typename _Scalar, int _PacketAccess>
>>  struct ei_traits<Map<AngularVelocity<_Scalar>, _PacketAccess> > :
>> ei_traits<Map<Matrix<_Scalar,3,1>, _PacketAccess> >
>>  {
>>    typedef Map<Matrix<_Scalar,3,1>, _PacketAccess > AngVelVectorType;
>>  };
>>
>>  template<typename _Scalar, int PacketAccess>
>>  class Map<AngularVelocity<_Scalar>, PacketAccess > : public
>> AngularVelocityBase<Map<AngularVelocity<_Scalar>, PacketAccess> > {
>>  private:
>>    typedef Map<AngularVelocity<_Scalar>, PacketAccess > MapAngVel;
>>  public:
>>    _EIGEN_GENERIC_PUBLIC_INTERFACE(MapAngVel,
>> AngularVelocityBase<MapAngVel>)
>>    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(MapAngVel)
>>
>>  private:
>>    Map(){};
>>
>>  public:
>>    typedef typename ei_traits<Map<AngularVelocity<Scalar>, PacketAccess>
>>>::AngVelVectorType AngVelVectorType;
>>
>>    inline Map(const Map<AngularVelocity<Scalar>, PacketAccess>& mt) :
>> m_data(mt.data().data()){};
>>    inline Map<AngularVelocity<_Scalar>, PacketAccess >(const _Scalar* data)
>> : m_data(data) {}
>>
>>    inline AngVelVectorType& data() { return m_data;}
>>    inline const AngVelVectorType& data() const { return m_data;}
>>
>>  protected:
>>    AngVelVectorType m_data;
>>  };
>>
>>        typedef Map<AngularVelocity<double> > AngularVelocityMapd;
>>        typedef Map<AngularVelocity<float> > AngularVelocityMapf;
>>        typedef Map<AngularVelocity<double>, Aligned>
>> AngularVelocityMapAlignedd;
>>        typedef Map<AngularVelocity<float>, Aligned>
>> AngularVelocityMapAlignedf;
>>
>>        template<class Derived>
>>        EIGEN_STRONG_INLINE const Matrix<typename ei_traits<Derived>::Scalar,
>> 3, 3> AngularVelocityBase<Derived>::adjoint() const{
>>                Matrix<Scalar, 3, 3> m;
>>
>>                m << 0, -this->coeff(2), this->coeff(1),                // 0
>> -z  y
>>                                 this->coeff(2), 0, -this->coeff(0),    // z
>>  0 -x
>>                                 -this->coeff(1), this->coeff(0);
>>   //-y  x  0
>>
>>                return m;
>>        }
>> }
>>
>> #endif
>>
>>
>