/**
assuming Link::v,w is already computed by calcForwardKinematics(true);
assuming Link::wc is already computed by calcCenterOfMass();
*/
void Body::calcTotalMomentum(Vector3& out_P, Vector3& out_L)
{
out_P.setZero();
out_L.setZero();
Vector3 dwc; // Center of mass speed in world frame
Vector3 P; // Linear momentum of the link
Vector3 L; // Angular momentum with respect to the world frame origin
Vector3 Llocal; // Angular momentum with respect to the center of mass of the link
int n = linkTraverse_.numLinks();
for(int i=0; i < n; i++){
Link* link = linkTraverse_[i];
dwc = link->v() + link->w().cross(link->R() * link->c());
P = link->m() * dwc;
//L = cross(link->wc, P) + link->R * link->I * trans(link->R) * link->w;
Llocal.noalias() = link->I() * link->R().transpose() * link->w();
L .noalias() = link->wc().cross(P) + link->R() * Llocal;
out_P += P;
out_L += L;
}
}
const Vector3& Body::calcCenterOfMass()
{
double m = 0.0;
Vector3 mc = Vector3::Zero();
int n = linkTraverse_.numLinks();
for(int i=0; i < n; i++){
Link* link = linkTraverse_[i];
link->wc().noalias() = link->R() * link->c() + link->p();
mc.noalias() += link->m() * link->wc();
m += link->m();
}
impl->centerOfMass = mc / m;
impl->mass = m;
return impl->centerOfMass;
}
