| Re: [eigen] Eigen and rigid body simulation |
[ Thread Index |
Date Index
| More lists.tuxfamily.org/eigen Archives
]
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.
--
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