/** 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; }