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;
}
bool divideJacobianInertia(const MomentumJacobian& src, const RigidBodyInertia& I, Jacobian& dest)
{
/** \todo if the inertia matrix is singular ? */
if(src.columns()!=dest.columns() || I.getMass() == 0)
return false;
for(unsigned int i=0;i<src.columns();i++)
dest.setColumn(i,src.getColumn(i)/I);
return true;
}
static PyObject *meth_RigidBodyInertia_RefPoint(PyObject *sipSelf, PyObject *sipArgs)
{
PyObject *sipParseErr = NULL;
{
const Vector* a0;
RigidBodyInertia *sipCpp;
if (sipParseArgs(&sipParseErr, sipArgs, "BJ9", &sipSelf, sipType_RigidBodyInertia, &sipCpp, sipType_Vector, &a0))
{
RigidBodyInertia*sipRes;
sipRes = new RigidBodyInertia(sipCpp->RefPoint(*a0));
return sipConvertFromNewType(sipRes,sipType_RigidBodyInertia,NULL);
}
}
/* Raise an exception if the arguments couldn't be parsed. */
sipNoMethod(sipParseErr, sipName_RigidBodyInertia, sipName_RefPoint, doc_RigidBodyInertia_RefPoint);
return NULL;
}
