diff --git a/src/multibody/joint/joint-planar.hpp b/src/multibody/joint/joint-planar.hpp
index 14002f74edd6b866a99970fd6232e2f4ed6fca51..8d6b7a56cb81475c7a3ee178b3f35fde3fd6a1a9 100644
--- a/src/multibody/joint/joint-planar.hpp
+++ b/src/multibody/joint/joint-planar.hpp
@@ -163,9 +163,9 @@ namespace se3
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
- Eigen::Matrix <Inertia::Scalar, 6, 3> operator* (const Inertia & Y, const JointPlanar::ConstraintPlanar &)
+ Eigen::Matrix <Inertia::Scalar_t, 6, 3> operator* (const Inertia & Y, const JointPlanar::ConstraintPlanar &)
{
- Eigen::Matrix <Inertia::Scalar, 6, 3> M;
+ Eigen::Matrix <Inertia::Scalar_t, 6, 3> M;
const double mass = Y.mass ();
const Inertia::Vector3 & com = Y.lever ();
const Symmetric3 & inertia = Y.inertia ();
diff --git a/src/spatial/inertia.hpp b/src/spatial/inertia.hpp
index dbea407489c528bba4482e84fa769c826e145b00..4f21929572ef9c46ff454c2f6b6b8e523ec2b2a4 100644
--- a/src/spatial/inertia.hpp
+++ b/src/spatial/inertia.hpp
@@ -23,148 +23,254 @@
#include "pinocchio/spatial/motion.hpp"
+
namespace se3
{
- template<typename _Scalar, int _Options>
- class InertiaTpl
+ template<class C> struct traitsInertia {};
+
+ template< class Derived>
+ class InertiaBase
+ {
+ protected:
+
+ typedef Derived Derived_t;
+ SPATIAL_TYPEDEF_ARG(Derived_t);
+
+ public:
+ Derived_t & derived() { return *static_cast<Derived_t*>(this); }
+ const Derived_t& derived() const { return *static_cast<const Derived_t*>(this); }
+
+ Scalar_t mass() const { return static_cast<const Derived_t*>(this)->mass(); }
+ const Vector3 & lever() const { return static_cast<const Derived_t*>(this)->lever(); }
+ const Symmetric3 & inertia() const { return static_cast<const Derived_t*>(this)->inertia(); }
+
+ Matrix6 matrix() const
+ {
+ return derived().matrix_impl();
+ }
+
+ operator Matrix6 () const { return matrix(); }
+
+
+ Derived_t& operator= (const Derived_t& clone){return derived().__equl__(clone);}
+ Derived_t& operator== (const Derived_t& other){return derived().isEqual(other);}
+ Derived_t& operator+= (const Derived_t & Yb) { return derived().__pequ__(Yb); }
+ Derived_t operator+(const Derived_t & Yb) const { return derived().__plus__(Yb); }
+ Force operator*(const Motion & v) const { return derived().__mult__(v); }
+
+ /// aI = aXb.act(bI)
+ Derived_t se3Action(const SE3 & M) const
+ {
+ return derived().se3Action_impl(M);
+ }
+
+ /// bI = aXb.actInv(aI)
+ Derived_t se3ActionInverse(const SE3 & M) const
+ {
+ return derived().se3ActionInverse_impl(M);
+ }
+
+ void disp
+ (std::ostream & os) const
+ {
+ os << "base disp" << std::endl;
+ static_cast<const Derived_t*>(this)->disp_impl(os);
+ }
+
+ friend std::ostream & operator << (std::ostream & os,const InertiaBase<Derived_t> & X)
+ {
+ os << "base <<" << std::endl;
+ X.disp(os);
+ return os;
+ }
+
+ };
+
+
+ template<typename T, int U>
+ struct traits< InertiaTpl<T, U> >
{
+ typedef T Scalar_t;
+ typedef Eigen::Matrix<T,3,1,U> Vector3;
+ typedef Eigen::Matrix<T,4,1,U> Vector4;
+ typedef Eigen::Matrix<T,6,1,U> Vector6;
+ typedef Eigen::Matrix<T,3,3,U> Matrix3;
+ typedef Eigen::Matrix<T,4,4,U> Matrix4;
+ typedef Eigen::Matrix<T,6,6,U> Matrix6;
+ typedef Matrix6 ActionMatrix_t;
+ typedef Vector3 Angular_t;
+ typedef Vector3 Linear_t;
+ typedef Eigen::Quaternion<T,U> Quaternion_t;
+ typedef SE3Tpl<T,U> SE3;
+ typedef ForceTpl<T,U> Force;
+ typedef MotionTpl<T,U> Motion;
+ typedef Symmetric3Tpl<T,U> Symmetric3;
+ enum {
+ LINEAR = 0,
+ ANGULAR = 3
+ };
+ // typedef typename Derived<T, U>::Vector3 Linear_t;
+ };
+
+ template<typename _Scalar, int _Options>
+ class InertiaTpl : public InertiaBase< InertiaTpl< _Scalar, _Options > >
+ {
public:
- typedef _Scalar Scalar;
- enum { Options = _Options };
- typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
- typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
- typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
- typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
- typedef MotionTpl<Scalar,Options> Motion;
- typedef ForceTpl<Scalar,Options> Force;
- typedef SE3Tpl<Scalar,Options> SE3;
- typedef InertiaTpl<Scalar,Options> Inertia;
- enum { LINEAR = 0, ANGULAR = 3 };
- typedef Symmetric3Tpl<Scalar,Options> Symmetric3;
+ friend class InertiaBase< InertiaTpl< _Scalar, _Options > >;
+ SPATIAL_TYPEDEF_ARG(InertiaTpl);
+
+
public:
// Constructors
- InertiaTpl() : m(), c(), I() {}
- InertiaTpl(Scalar m_,
- const Vector3 &c_,
- const Matrix3 &I_)
- : m(m_),
- c(c_),
- I(I_) {}
- InertiaTpl(Scalar _m,
- const Vector3 &_c,
- const Symmetric3 &_I)
- : m(_m),
- c(_c),
- I(_I) { }
+ InertiaTpl() : m(), c(), I() {}
+
+ InertiaTpl(Scalar_t m_,
+ const Vector3 &c_,
+ const Matrix3 &I_)
+ : m(m_),
+ c(c_),
+ I(I_)
+ {
+
+ }
+
+ InertiaTpl(Scalar_t _m,
+ const Vector3 &_c,
+ const Symmetric3 &_I)
+ : m(_m),
+ c(_c),
+ I(_I)
+ {
+
+ }
InertiaTpl(const InertiaTpl & clone) // Clone constructor for std::vector
: m(clone.m),
- c(clone.c),
- I(clone.I) {}
- InertiaTpl& operator= (const InertiaTpl& clone) // Copy operator for std::vector
- {
- m=clone.m; c=clone.c; I=clone.I;
- return *this;
- }
- /* Requiered by std::vector boost::python bindings. */
- bool operator==( const InertiaTpl& Y2 )
- { return (m==Y2.m) && (c==Y2.c) && (I==Y2.I); }
- template<typename S2,int O2>
- InertiaTpl( const InertiaTpl<S2,O2> & clone )
- : m(clone.mass()),
- c(clone.lever()),
- I(clone.inertia().matrix()) {}
+ c(clone.c),
+ I(clone.I)
+ {
+
+ }
+
+ template<typename S2,int O2>
+ InertiaTpl( const InertiaTpl<S2,O2> & clone )
+ : m(clone.mass()),
+ c(clone.lever()),
+ I(clone.inertia().matrix())
+ {
+
+ }
+
+
// Initializers
- static Inertia Zero()
- {
- return InertiaTpl(0.,
- Vector3::Zero(),
- Symmetric3::Zero());
- }
- static Inertia Identity()
- {
- return InertiaTpl(1.,
- Vector3::Zero(),
- Symmetric3::Identity());
- }
- static Inertia Random()
- {
- /* We have to shoot "I" definite positive and not only symmetric. */
- return InertiaTpl(Eigen::internal::random<Scalar>()+1,
- Vector3::Random(),
- Symmetric3::RandomPositive());
- }
+ static InertiaTpl Zero()
+ {
+ return InertiaTpl(0.,
+ Vector3::Zero(),
+ Symmetric3::Zero());
+ }
+
+ static InertiaTpl Identity()
+ {
+ return InertiaTpl(1.,
+ Vector3::Zero(),
+ Symmetric3::Identity());
+ }
+
+ static InertiaTpl Random()
+ {
+ /* We have to shoot "I" definite positive and not only symmetric. */
+ return InertiaTpl(Eigen::internal::random<Scalar_t>()+1,
+ Vector3::Random(),
+ Symmetric3::RandomPositive());
+ }
+
+
+ Matrix6 matrix_impl() const
+ {
+ Matrix6 M;
+ const Matrix3 & c_cross = (skew(c));
+ M.template block<3,3>(LINEAR, LINEAR ).template setZero ();
+ M.template block<3,3>(LINEAR, LINEAR ).template diagonal ().template fill (m);
+ M.template block<3,3>(ANGULAR,LINEAR ) = m * c_cross;
+ M.template block<3,3>(LINEAR, ANGULAR) = -M.template block<3,3> (ANGULAR, LINEAR);
+ M.template block<3,3>(ANGULAR,ANGULAR) = (Matrix3)(I - M.template block<3,3>(ANGULAR, LINEAR) * c_cross);
+
+ return M;
+ }
+
+ // Arithmetic operators
+ InertiaTpl& __equl__ (const InertiaTpl& clone) // Copy operator for std::vector
+ {
+ m=clone.m; c=clone.c; I=clone.I;
+ return *this;
+ }
+
+ /* Requiered by std::vector boost::python bindings. */
+ bool isEqual( const InertiaTpl& Y2 )
+ {
+ return (m==Y2.m) && (c==Y2.c) && (I==Y2.I);
+ }
+
+ InertiaTpl __plus__(const InertiaTpl &Yb) const
+ {
+ /* Y_{a+b} = ( m_a+m_b,
+ * (m_a*c_a + m_b*c_b ) / (m_a + m_b),
+ * I_a + I_b - (m_a*m_b)/(m_a+m_b) * AB_x * AB_x )
+ */
+
+ const double & mab = m+Yb.m;
+ const Vector3 & AB = (c-Yb.c).eval();
+ return InertiaTpl( mab,
+ (m*c+Yb.m*Yb.c)/mab,
+ I+Yb.I - (m*Yb.m/mab)* typename Symmetric3::SkewSquare(AB));
+ }
+
+ InertiaTpl& __pequ__(const InertiaTpl &Yb)
+ {
+ const InertiaTpl& Ya = *this;
+ const double & mab = Ya.m+Yb.m;
+ const Vector3 & AB = (Ya.c-Yb.c).eval();
+ c *= m; c += Yb.m*Yb.c; c /= mab;
+ I += Yb.I; I -= (Ya.m*Yb.m/mab)* typename Symmetric3::SkewSquare(AB);
+ m = mab;
+ return *this;
+ }
+
+ Force __mult__(const Motion &v) const
+ {
+ const Vector3 & mcxw = m*c.cross(v.angular());
+ return Force( m*v.linear()-mcxw,
+ m*c.cross(v.linear()) + I*v.angular() - c.cross(mcxw) );
+ }
// Getters
- Scalar mass() const { return m; }
+ Scalar_t mass() const { return m; }
const Vector3 & lever() const { return c; }
const Symmetric3 & inertia() const { return I; }
- Matrix6 matrix() const
- {
- Matrix6 M;
- const Matrix3 & c_cross = (skew(c));
- M.template block<3,3>(LINEAR, LINEAR ).template setZero ();
- M.template block<3,3>(LINEAR, LINEAR ).template diagonal ().template fill (m);
- M.template block<3,3>(ANGULAR,LINEAR ) = m * c_cross;
- M.template block<3,3>(LINEAR, ANGULAR) = -M.template block<3,3> (ANGULAR, LINEAR);
- M.template block<3,3>(ANGULAR,ANGULAR) = (Matrix3)(I - M.template block<3,3>(ANGULAR, LINEAR) * c_cross);
-
- return M;
- }
- operator Matrix6 () const { return matrix(); }
-
- // Arithmetic operators
- friend Inertia operator+(const InertiaTpl &Ya, const InertiaTpl &Yb)
- {
- /* Y_{a+b} = ( m_a+m_b,
- * (m_a*c_a + m_b*c_b ) / (m_a + m_b),
- * I_a + I_b - (m_a*m_b)/(m_a+m_b) * AB_x * AB_x )
- */
-
- const double & mab = Ya.m+Yb.m;
- const Vector3 & AB = (Ya.c-Yb.c).eval();
- return InertiaTpl( mab,
- (Ya.m*Ya.c+Yb.m*Yb.c)/mab,
- Ya.I+Yb.I - (Ya.m*Yb.m/mab)* typename Symmetric3::SkewSquare(AB));
- }
- Inertia& operator+=(const InertiaTpl &Yb)
- {
- const Inertia& Ya = *this;
- const double & mab = Ya.m+Yb.m;
- const Vector3 & AB = (Ya.c-Yb.c).eval();
- c *= m; c += Yb.m*Yb.c; c /= mab;
- I += Yb.I; I -= (Ya.m*Yb.m/mab)* typename Symmetric3::SkewSquare(AB);
- m = mab;
- return *this;
- }
- Force operator*(const Motion &v) const
- {
- const Vector3 & mcxw = m*c.cross(v.angular());
- return Force( m*v.linear()-mcxw,
- m*c.cross(v.linear()) + I*v.angular() - c.cross(mcxw) );
- }
/// aI = aXb.act(bI)
- Inertia se3Action(const SE3 & M) const
- {
- /* The multiplication RIR' has a particular form that could be used, however it
- * does not seems to be more efficient, see http://stackoverflow.com/questions/
- * 13215467/eigen-best-way-to-evaluate-asa-transpose-and-store-the-result-in-a-symmetric .*/
- return Inertia( m,
- M.translation()+M.rotation()*c,
- I.rotate(M.rotation()) );
- }
- /// bI = aXb.actInv(aI)
- Inertia se3ActionInverse(const SE3 & M) const
- {
- return Inertia( m,
- M.rotation().transpose()*(c-M.translation()),
- I.rotate(M.rotation().transpose()) );
- }
+ InertiaTpl se3Action_impl(const SE3 & M) const
+ {
+ /* The multiplication RIR' has a particular form that could be used, however it
+ * does not seems to be more efficient, see http://stackoverflow.com/questions/
+ * 13215467/eigen-best-way-to-evaluate-asa-transpose-and-store-the-result-in-a-symmetric .*/
+ return InertiaTpl(m,
+ M.translation()+M.rotation()*c,
+ I.rotate(M.rotation()) );
+ }
+
+ /// bI = aXb.actInv(aI)
+ InertiaTpl se3ActionInverse_impl(const SE3 & M) const
+ {
+ return InertiaTpl(m,
+ M.rotation().transpose()*(c-M.translation()),
+ I.rotate(M.rotation().transpose()) );
+ }
//
Force vxiv( const Motion& v ) const
@@ -172,20 +278,19 @@ namespace se3
const Vector3 & mcxw = m*c.cross(v.angular());
const Vector3 & mv_mcxw = m*v.linear()-mcxw;
return Force( v.angular().cross(mv_mcxw),
- v.angular().cross(c.cross(mv_mcxw)+I*v.angular())-v.linear().cross(mcxw) );
+ v.angular().cross(c.cross(mv_mcxw)+I*v.angular())-v.linear().cross(mcxw) );
}
- friend std::ostream & operator<< (std::ostream &os, const Inertia &I)
- {
- os << "m =" << I.m << ";\n"
- << "c = [\n" << I.c.transpose() << "]';\n"
- << "I = [\n" << (Matrix3)I.I << "];";
- return os;
- }
+ void disp_impl(std::ostream &os) const
+ {
+ os << "m =" << m << ";\n"
+ << "c = [\n" << c.transpose() << "]';\n"
+ << "I = [\n" << (Matrix3)I << "];";
+ }
public:
private:
- Scalar m;
+ Scalar_t m;
Vector3 c;
Symmetric3 I;
};