#include <SpatialInertia.h>

Inheritance diagram for SpatialInertia< T >:

Collaboration diagram for SpatialInertia< T >:

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	SpatialInertia (T mass, const Vec3< T > &com, const Mat3< T > &inertia)

	SpatialInertia (const Mat6< T > &inertia)

	SpatialInertia ()

	SpatialInertia (const MassProperties< T > &a)

	SpatialInertia (const Mat4< T > &P)

MassProperties< T >	asMassPropertyVector ()

const Mat6< T > &	getMatrix () const

void	setMatrix (const Mat6< T > &mat)

void	addMatrix (const Mat6< T > &mat)

T	getMass ()

Vec3< T >	getCOM ()

Mat3< T >	getInertiaTensor ()

Mat4< T >	getPseudoInertia ()

SpatialInertia	flipAlongAxis (CoordinateAxis axis)

Private Attributes
Mat6< T >	_inertia

Detailed Description

template<typename T>
class SpatialInertia< T >

Representation of Rigid Body Inertia as a 6x6 Spatial Inertia Tensor

Definition at line 25 of file SpatialInertia.h.

Constructor & Destructor Documentation

template<typename T>

EIGEN_MAKE_ALIGNED_OPERATOR_NEW SpatialInertia< T >::SpatialInertia	(	T	mass,
		const Vec3< T > &	com,
		const Mat3< T > &	inertia
	)

inline

Construct spatial inertia from mass, center of mass, and 3x3 rotational inertia

Definition at line 32 of file SpatialInertia.h.

                                                                      {
     Mat3<T> cSkew = vectorToSkewMat(com);
     _inertia.template topLeftCorner<3, 3>() =
         inertia + mass * cSkew * cSkew.transpose();
     _inertia.template topRightCorner<3, 3>() = mass * cSkew;
     _inertia.template bottomLeftCorner<3, 3>() = mass * cSkew.transpose();
     _inertia.template bottomRightCorner<3, 3>() = mass * Mat3<T>::Identity();
   }

template<typename T>

SpatialInertia< T >::SpatialInertia ( const Mat6< T > & inertia )

inlineexplicit

Construct spatial inertia from 6x6 matrix

Definition at line 44 of file SpatialInertia.h.

44 { _inertia = inertia; }

SpatialInertia::_inertia

Mat6< T > _inertia

Definition: SpatialInertia.h:172

template<typename T>

SpatialInertia< T >::SpatialInertia ( )

inline

If no argument is given, zero.

Definition at line 49 of file SpatialInertia.h.

49 { _inertia = Mat6<T>::Zero(); }

Mat6

typename Eigen::Matrix< T, 6, 6 > Mat6

Definition: cppTypes.h:70

SpatialInertia::_inertia

Mat6< T > _inertia

Definition: SpatialInertia.h:172

template<typename T>

SpatialInertia< T >::SpatialInertia ( const MassProperties< T > & a )

inlineexplicit

Construct spatial inertia from mass property vector

Definition at line 54 of file SpatialInertia.h.

                                                       {
     _inertia(0, 0) = a(4);
     _inertia(0, 1) = a(9);
     _inertia(0, 2) = a(8);
     _inertia(1, 0) = a(9);
     _inertia(1, 1) = a(5);
     _inertia(1, 2) = a(7);
     _inertia(2, 0) = a(8);
     _inertia(2, 1) = a(7);
     _inertia(2, 2) = a(6);
     Mat3<T> cSkew = vectorToSkewMat(Vec3<T>(a(1), a(2), a(3)));
     _inertia.template topRightCorner<3, 3>() = cSkew;
     _inertia.template bottomLeftCorner<3, 3>() = cSkew.transpose();
     _inertia.template bottomRightCorner<3, 3>() = a(0) * Mat3<T>::Identity();
   }

template<typename T>

SpatialInertia< T >::SpatialInertia ( const Mat4< T > & P )

inlineexplicit

Construct spatial inertia from pseudo-inertia. This is described in Linear Matrix Inequalities for Physically Consistent Inertial Parameter Identification: A Statistical Perspective on the Mass Distribution, by Wensing, Kim, Slotine

Parameters

P

Definition at line 77 of file SpatialInertia.h.

                                             {
     Mat6<T> I;
     T m = P(3, 3);
     Vec3<T> h = P.template topRightCorner<3, 1>();
     Mat3<T> E = P.template topLeftCorner<3, 3>();
     Mat3<T> Ibar = E.trace() * Mat3<T>::Identity() - E;
     I.template topLeftCorner<3, 3>() = Ibar;
     I.template topRightCorner<3, 3>() = vectorToSkewMat(h);
     I.template bottomLeftCorner<3, 3>() = vectorToSkewMat(h).transpose();
     I.template bottomRightCorner<3, 3>() = m * Mat3<T>::Identity();
     _inertia = I;
   }

Member Function Documentation

template<typename T>

void SpatialInertia< T >::addMatrix ( const Mat6< T > & mat )

inline

Definition at line 108 of file SpatialInertia.h.

108 { _inertia += mat; }

SpatialInertia::_inertia

Mat6< T > _inertia

Definition: SpatialInertia.h:172

template<typename T>

MassProperties<T> SpatialInertia< T >::asMassPropertyVector ( )

inline

Convert spatial inertia to mass property vector

Definition at line 93 of file SpatialInertia.h.

                                            {
     MassProperties<T> a;
     Vec3<T> h = matToSkewVec(_inertia.template topRightCorner<3, 3>());
     a << _inertia(5, 5), h(0), h(1), h(2), _inertia(0, 0), _inertia(1, 1),
         _inertia(2, 2), _inertia(2, 1), _inertia(2, 0), _inertia(1, 0);
     return a;
   }

Here is the caller graph for this function:

template<typename T>

SpatialInertia SpatialInertia< T >::flipAlongAxis ( CoordinateAxis axis )

inline

Flip inertia matrix around an axis. This isn't efficient, but it works!

Definition at line 158 of file SpatialInertia.h.

                                                     {
     Mat4<T> P = getPseudoInertia();
     Mat4<T> X = Mat4<T>::Identity();
     if (axis == CoordinateAxis::X)
       X(0, 0) = -1;
     else if (axis == CoordinateAxis::Y)
       X(1, 1) = -1;
     else if (axis == CoordinateAxis::Z)
       X(2, 2) = -1;
     P = X * P * X;
     return SpatialInertia(P);
   }

template<typename T>

Vec3<T> SpatialInertia< T >::getCOM ( )

inline

Get center of mass location

Definition at line 118 of file SpatialInertia.h.

                    {
     T m = getMass();
     Mat3<T> mcSkew = _inertia.template topRightCorner<3, 3>();
     Vec3<T> com = matToSkewVec(mcSkew) / m;
     return com;
   }

Here is the caller graph for this function:

template<typename T>

Mat3<T> SpatialInertia< T >::getInertiaTensor ( )

inline

Get 3x3 rotational inertia

Definition at line 128 of file SpatialInertia.h.

                              {
     T m = getMass();
     Mat3<T> mcSkew = _inertia.template topRightCorner<3, 3>();
     Mat3<T> I_rot = _inertia.template topLeftCorner<3, 3>() -
                     mcSkew * mcSkew.transpose() / m;
     return I_rot;
   }

Here is the caller graph for this function:

template<typename T>

T SpatialInertia< T >::getMass ( )

inline

Get mass

Definition at line 113 of file SpatialInertia.h.

113 { return _inertia(5, 5); }

SpatialInertia::_inertia

Mat6< T > _inertia

Definition: SpatialInertia.h:172

Here is the caller graph for this function:

template<typename T>

const Mat6<T>& SpatialInertia< T >::getMatrix ( ) const

inline

Get 6x6 spatial inertia

Definition at line 104 of file SpatialInertia.h.

104 { return _inertia; }

SpatialInertia::_inertia

Mat6< T > _inertia

Definition: SpatialInertia.h:172

Here is the caller graph for this function:

template<typename T>

Mat4<T> SpatialInertia< T >::getPseudoInertia ( )

inline

Convert to 4x4 pseudo-inertia matrix. This is described in Linear Matrix Inequalities for Physically Consistent Inertial Parameter Identification: A Statistical Perspective on the Mass Distribution, by Wensing, Kim, Slotine

Definition at line 142 of file SpatialInertia.h.

                              {
     Vec3<T> h = matToSkewVec(_inertia.template topRightCorner<3, 3>());
     Mat3<T> Ibar = _inertia.template topLeftCorner<3, 3>();
     T m = _inertia(5, 5);
     Mat4<T> P;
     P.template topLeftCorner<3, 3>() =
         0.5 * Ibar.trace() * Mat3<T>::Identity() - Ibar;
     P.template topRightCorner<3, 1>() = h;
     P.template bottomLeftCorner<1, 3>() = h.transpose();
     P(3, 3) = m;
     return P;
   }