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