Twist operator/(const Wrench& h, const RigidBodyInertia& I)
{
//For mathematical details, check:
//Featherstone Book (RBDA, 2008) formuma 2.74, page 36
double m = I.getMass();
Vector com_kdl = I.getCOG();
RotationalInertia Irot = I.getRotationalInertia();
if( m == 0 ) { return Twist::Zero(); }
Eigen::Matrix3d Ic, Ic_inverse, skew_com;
Eigen::Vector3d com;
com = Eigen::Map<Eigen::Vector3d>(com_kdl.data);
skew_com = skew(com);
Ic = Eigen::Map<Eigen::Matrix3d>(Irot.data) + m*skew_com*skew_com;
Ic_inverse = Ic.inverse();
Twist return_value;
Eigen::Map<Eigen::Vector3d>(return_value.vel.data) = ((1/m)*Eigen::Matrix3d::Identity() - skew_com*Ic_inverse*skew_com) * Eigen::Map<const Eigen::Vector3d>(h.force.data) +
skew_com*Ic_inverse*Eigen::Map<const Eigen::Vector3d>(h.torque.data);
Eigen::Map<Eigen::Vector3d>(return_value.rot.data) = (-Ic_inverse*skew_com)*Eigen::Map<const Eigen::Vector3d>(h.force.data)
+ Ic_inverse*Eigen::Map<const Eigen::Vector3d>(h.torque.data);
return return_value;
}
Matrix<double,10,1> Vectorize(const RigidBodyInertia & rbd_inertia)
{
Matrix<double,10,1> ret;
Vector MCOG = rbd_inertia.getMass()*rbd_inertia.getCOG();
//In RigidBodyInertia, the rotational inertia is stored with
//respect to the link frame of refence, not the COG one
ret << rbd_inertia.getMass(),
Map<Vector3d>(MCOG.data),
vech(rbd_inertia.getRotationalInertia());
return ret;
}