26 using namespace Eigen;
32 this->M=Matrix3d::Identity()*rbi.
m;
33 this->I=Map<const Matrix3d>(rbi.
I.
data);
34 this->H << 0,-rbi.
h[2],rbi.
h[1],
44 ArticulatedBodyInertia::ArticulatedBodyInertia(
const Matrix3d& M,
const Matrix3d& H,
const Matrix3d& I)
82 Map<Matrix3d> E(X.
M.
data);
84 rcross << 0,-X.
p[2],X.
p[1],
88 Matrix3d HrM=I.
H-rcross*I.
M;
89 return ArticulatedBodyInertia(E*I.
M*E.transpose(),E*HrM*E.transpose(),E*(I.
I-rcross*I.
H.transpose()+HrM*rcross)*E.transpose());
93 Map<const Matrix3d> E(M.
data);
102 rcross << 0,-p[2],p[1],
106 Matrix3d HrM=this->H-rcross*this->M;
represents rotations in 3 dimensional space.
Vector vel
The velocity of that point.
6D Inertia of a rigid body
Vector torque
Torque that is applied at the origin of the current ref frame.
Rotation M
Orientation of the Frame.
ArticulatedBodyInertia operator+(const RigidBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
represents both translational and rotational velocities.
6D Inertia of a articulated body
A concrete implementation of a 3 dimensional vector class.
Vector rot
The rotational velocity of that point.
ArticulatedBodyInertia operator*(const Rotation &M, const ArticulatedBodyInertia &I)
Vector p
origine of the Frame
Frame Inverse() const
Gives back inverse transformation of a Frame.
Vector force
Force that is applied at the origin of the current ref frame.
represents a frame transformation in 3D space (rotation + translation)
represents both translational and rotational acceleration.
ArticulatedBodyInertia operator-(const RigidBodyInertia &Ia, const ArticulatedBodyInertia &Ib)