void GSystem::calcDerivative_PositionCOMGlobal_Dq(RMatrix &DpDq_)
{
int i;
std::list<GBody *>::iterator iter_pbody;
std::list<GCoordinate *>::iterator iter_pcoord;
std::vector<SE3> M(pBodies.size());
std::vector<GConstraintJointLoop::JOINTLOOP_CONSTRAINT_TYPE> jlc_type(pBodies.size());
Vec3 DpDqi;
for (i=0, iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); i++, iter_pbody++) {
// save previous settings
M[i] = (*iter_pbody)->fwdJointChain.M1;
jlc_type[i] = (*iter_pbody)->fwdJointChain.jointLoopConstraintType;
// prerequisites for (*iter_pbody)->getDerivative_PositionCOMGlobal_Dq(...)
(*iter_pbody)->fwdJointChain.setM(SE3());
(*iter_pbody)->fwdJointChain.jointLoopConstraintType = GConstraintJointLoop::JOINTLOOP_ORIENTATION_POSITION;
(*iter_pbody)->fwdJointChain.update_J();
}
// calculate the derivative
DpDq_.SetZero(3, getNumCoordinates());
for (i=0, iter_pcoord = pCoordinates.begin(); iter_pcoord != pCoordinates.end(); i++, iter_pcoord++) {
DpDqi = getDerivative_PositionCOMGlobal_Dq(*iter_pcoord);
//DpDq_.Push(0, i, convert_to_RMatrix(DpDqi));
matSet(&DpDq_[3*i], DpDqi.GetArray(), 3);
}
// restore the previous settings
for (i=0, iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); i++, iter_pbody++) {
(*iter_pbody)->fwdJointChain.setM(M[i]);
(*iter_pbody)->fwdJointChain.setJointLoopConstraintType(jlc_type[i]);
}
}
