/** calculate the mass matrix using the unit vector method \todo replace the unit vector method here with a more efficient method that only requires O(n) computation time The motion equation (dv != dvo) | | | dv | | | | fext | | out_M | * | dw | + | b1 | = | tauext | | | |ddq | | | | u | */ void Body::calcMassMatrix(dmatrix& out_M) { // buffers for the unit vector method dmatrix b1; dvector ddqorg; dvector uorg; Vector3 dvoorg; Vector3 dworg; Vector3 root_w_x_v; Vector3 g(0, 0, 9.8); uint nJ = numJoints(); int totaldof = nJ; if( !isStaticModel_ ) totaldof += 6; out_M.resize(totaldof,totaldof); b1.resize(totaldof, 1); // preserve and clear the joint accelerations ddqorg.resize(nJ); uorg.resize(nJ); for(uint i = 0; i < nJ; ++i){ Link* ptr = joint(i); ddqorg[i] = ptr->ddq; uorg [i] = ptr->u; ptr->ddq = 0.0; } // preserve and clear the root link acceleration dvoorg = rootLink_->dvo; dworg = rootLink_->dw; root_w_x_v = rootLink_->w.cross(rootLink_->vo + rootLink_->w.cross(rootLink_->p)); rootLink_->dvo = g - root_w_x_v; // dv = g, dw = 0 rootLink_->dw.setZero(); setColumnOfMassMatrix(b1, 0); if( !isStaticModel_ ){ for(int i=0; i < 3; ++i){ rootLink_->dvo[i] += 1.0; setColumnOfMassMatrix(out_M, i); rootLink_->dvo[i] -= 1.0; } for(int i=0; i < 3; ++i){ rootLink_->dw[i] = 1.0; Vector3 dw_x_p = rootLink_->dw.cross(rootLink_->p); // spatial acceleration caused by ang. acc. rootLink_->dvo -= dw_x_p; setColumnOfMassMatrix(out_M, i + 3); rootLink_->dvo += dw_x_p; rootLink_->dw[i] = 0.0; } } for(uint i = 0; i < nJ; ++i){ Link* ptr = joint(i); ptr->ddq = 1.0; int j = i + 6; setColumnOfMassMatrix(out_M, j); out_M(j, j) += ptr->Jm2; // motor inertia ptr->ddq = 0.0; } // subtract the constant term for(size_t i = 0; i < (size_t)out_M.cols(); ++i){ out_M.col(i) -= b1; } // recover state for(uint i = 0; i < nJ; ++i){ Link* ptr = joint(i); ptr->ddq = ddqorg[i]; ptr->u = uorg [i]; } rootLink_->dvo = dvoorg; rootLink_->dw = dworg; }
void Body::calcCMJacobian(Link *base, dmatrix &J) { // prepare subm, submwc JointPathPtr jp; 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 (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; } J.resize(3, numJoints()); }else{ rootLink()->calcSubMassCM(); J.resize(3, numJoints()+6); } // compute Jacobian std::vector<int> sgn(numJoints(), 1); if (jp) { for (int i=0; i<jp->numJoints(); i++) sgn[jp->joint(i)->jointId] = -1; } for (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 arm((j->submwc-j->subm*j->p)/totalMass_); Vector3 dp(omega.cross(arm)); J.col(j->jointId) = dp; break; } default: std::cerr << "calcCMJacobian() : unsupported jointType(" << j->jointType << std::endl; } } if (!base){ int c = numJoints(); J(0, c ) = 1.0; J(0, c+1) = 0.0; J(0, c+2) = 0.0; J(1, c ) = 0.0; J(1, c+1) = 1.0; J(1, c+2) = 0.0; J(2, c ) = 0.0; J(2, c+1) = 0.0; J(2, c+2) = 1.0; Vector3 dp(rootLink()->submwc/totalMass_ - rootLink()->p); J(0, c+3) = 0.0; J(0, c+4) = dp(2); J(0, c+5) = -dp(1); J(1, c+3) = -dp(2); J(1, c+4) = 0.0; J(1, c+5) = dp(0); J(2, c+3) = dp(1); J(2, c+4) = -dp(0); J(2, c+5) = 0.0; } }
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; } }