| Re: [eigen] Eigen and rigid body simulation |
[ Thread Index |
Date Index
| More lists.tuxfamily.org/eigen Archives
]
- To: eigen@xxxxxxxxxxxxxxxxxxx
- Subject: Re: [eigen] Eigen and rigid body simulation
- From: Benoit Jacob <jacob.benoit.1@xxxxxxxxx>
- Date: Thu, 26 Nov 2009 09:16:29 -0500
- Dkim-signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=gmail.com; s=gamma; h=domainkey-signature:mime-version:received:in-reply-to:references :date:message-id:subject:from:to:content-type :content-transfer-encoding; bh=WcW33TmsJiBTaA57ibF/MNwoSNdXM6wpkUUIr1bSzww=; b=FuW7FEbSyH9LR+k1qT1tEIjUNA8ocVUweYeFZnWOVcui98UTx0aBNCHwUJ9Ky2qx5z djiValO5cdG9TVLX8c7Ri5BBBzv3Gpr6YB5WJ7B3bEN5o2akK+j5FAFr63yIODCITkI/ STq5QGLbzYXu35LvH/3HJfTp7LehBDQwNDi+U=
- Domainkey-signature: a=rsa-sha1; c=nofws; d=gmail.com; s=gamma; h=mime-version:in-reply-to:references:date:message-id:subject:from:to :content-type:content-transfer-encoding; b=g3fVdH2rSWj/FpcdkBGt526Pe2FoOSCmWLf516V0D9NUIvixlc2HYiHqhE5ucL47ar o8why+sAEBPgpzH4i4cBOXOVmSQpzBW79H3JJB0dfiIZYHmwbyHa/x2O/VWCw71koZ78 9ENfftto44fmI70ENW3KWYcS+Zw/uJWo5u9ek=
2009/11/26 Mathieu Gautier <mathieu.gautier@xxxxxx>:
>> We seem to agree about what is 'in scope' and what is not. I think it
>> would be even more clear if you show us what you expect to add. I don't mean
>> a final patch that would probably require a lot of work, but examples of
>> code and API that you expect to provide in the module. Anything that you can
>> do without spending time on it.
>
> OK, I have already prototyped the classes I have described in my first mail.
> See the attached files. Rotation3D inherits from Quaternion and add two
> methods. I think that, if, this code is added to eigen, that Quaternion and
> Rotation3D may be merged in one class.
I haven't yet looked at your code, but I am not convinced about having
a whole new class named Rotation3D.
I guess that my main issue is the naming. We already have RotationBase
and several classes representing 3D rotations, so your class must have
a different name.
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.
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
>
>