void Link::calcSubMassInertia(Matrix33& subIw)
{
subIw = R*I*R.transpose();
if (subm!=0.0) subIw += m*hat(wc - submwc/subm).transpose()*hat(wc - submwc/subm);
if (child){
Matrix33 childsubIw;
child->calcSubMassInertia(childsubIw);
subIw += childsubIw;
if (child->subm!=0.0) subIw += child->subm*hat(child->submwc/child->subm - submwc/subm).transpose()*hat(child->submwc/child->subm - submwc/subm);
Link *l = child->sibling;
while (l){
Matrix33 lsubIw;
l->calcSubMassInertia(lsubIw);
subIw += lsubIw;
if (l->subm!=0.0) subIw += l->subm*hat(l->submwc/l->subm - submwc/subm).transpose()*hat(l->submwc/l->subm - submwc/subm);
l = l->sibling;
}
}
}
void Body::calcAngularMomentumJacobian(Link *base, dmatrix &H)
{
// prepare subm, submwc
JointPathPtr jp;
dmatrix M;
calcCMJacobian(base, M);
M.conservativeResize(3, numJoints());
M *= totalMass();
if (base){
jp = getJointPath(rootLink(), base);
Link *skip = jp->joint(0);
skip->subm = rootLink()->m;
skip->submwc = rootLink()->m*rootLink()->wc;
Link *l = rootLink()->child;
if (l){
if (l != skip) {
l->calcSubMassCM();
skip->subm += l->subm;
skip->submwc += l->submwc;
}
l = l->sibling;
while(l){
if (l != skip){
l->calcSubMassCM();
skip->subm += l->subm;
skip->submwc += l->submwc;
}
l = l->sibling;
}
}
// assuming there is no branch between base and root
for (unsigned int i=1; i<jp->numJoints(); i++){
l = jp->joint(i);
l->subm = l->parent->m + l->parent->subm;
l->submwc = l->parent->m*l->parent->wc + l->parent->submwc;
}
H.resize(3, numJoints());
}else{
rootLink()->calcSubMassCM();
H.resize(3, numJoints()+6);
}
// compute Jacobian
std::vector<int> sgn(numJoints(), 1);
if (jp) {
for (unsigned int i=0; i<jp->numJoints(); i++) sgn[jp->joint(i)->jointId] = -1;
}
for (unsigned int i=0; i<numJoints(); i++){
Link *j = joint(i);
switch(j->jointType){
case Link::ROTATIONAL_JOINT:
{
Vector3 omega(sgn[j->jointId]*j->R*j->a);
Vector3 Mcol = M.col(j->jointId);
Matrix33 jsubIw;
j->calcSubMassInertia(jsubIw);
Vector3 dp = jsubIw*omega;
if (j->subm!=0) dp += (j->submwc/j->subm).cross(Mcol);
H.col(j->jointId) = dp;
break;
}
case Link::SLIDE_JOINT:
{
if(j->subm!=0){
Vector3 Mcol =M.col(j->jointId);
Vector3 dp = (j->submwc/j->subm).cross(Mcol);
H.col(j->jointId) = dp;
}
break;
}
default:
std::cerr << "calcCMJacobian() : unsupported jointType("
<< j->jointType << ")" << std::endl;
}
}
if (!base){
int c = numJoints();
H.block(0, c, 3, 3).setZero();
Matrix33 Iw;
rootLink_->calcSubMassInertia(Iw);
H.block(0, c+3, 3, 3) = Iw;
Vector3 cm = calcCM();
Matrix33 cm_cross;
cm_cross <<
0.0, -cm(2), cm(1),
cm(2), 0.0, -cm(0),
-cm(1), cm(0), 0.0;
H.block(0,0,3,c) -= cm_cross * M;
}
}
