Skip to content
Snippets Groups Projects
Commit 09421384 authored by Valenza Florian's avatar Valenza Florian
Browse files

[Major] Inertia is now CRTP

parent e7104e99
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/multibody/joint/joint-planar.hpp
+ 2
− 2
View file @ 09421384
......@@ -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 ();
......
src/spatial/inertia.hpp
+ 236
− 131
View file @ 09421384
......@@ -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;
};
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment