void Skeleton::computeForwardDynamicsID2(
const Eigen::Vector3d& _gravity, bool _equationsOfMotion)
{
int n = getDOF();
// skip immobile objects in forward simulation
if (getImmobileState() == true || n == 0)
{
return;
}
// Save current tau
Eigen::VectorXd tau_old = get_tau();
// Set ddq as zero
set_ddq(Eigen::VectorXd::Zero(n));
//
mM = Eigen::MatrixXd::Zero(n,n);
// M(q) * ddq + b(q,dq) = tau
computeInverseDynamicsLinear(_gravity);
Eigen::VectorXd b = get_tau();
mCg = b;
// Calcualtion M column by column
for (int i = 0; i < n; ++i)
{
Eigen::VectorXd basis = Eigen::VectorXd::Zero(n);
basis(i) = 1;
set_ddq(basis);
computeInverseDynamicsLinear(_gravity);
mM.col(i) = get_tau() - b;
}
// Restore the torque
set_tau(tau_old);
// Evaluate external forces in generalized coordinate.
updateExternalForces();
Eigen::VectorXd qddot = this->getInvMassMatrix()
* (-this->getCombinedVector()
+ this->getExternalForces()
+ this->getInternalForces()
+ this->getDampingForces()
+ this->getConstraintForces() );
//mMInv = mM.inverse();
mMInv = mM.ldlt().solve(Eigen::MatrixXd::Identity(n,n));
this->set_ddq(qddot);
clearExternalForces();
}
bool GSystem::calcProductOfInvMassAndMatrix(RMatrix &invM_A, const RMatrix &A)
{
if ( (size_t)A.RowSize() != pCoordinates.size() ) return false;
invM_A.SetZero(A.RowSize(), A.ColSize());
int i;
std::list<GJoint *>::iterator iter_pjoint;
std::vector<bool> isprescribed(pJoints.size());
// save current info
for (i=0, iter_pjoint = pJoints.begin(); iter_pjoint != pJoints.end(); i++, iter_pjoint++) {
isprescribed[i] = (*iter_pjoint)->isPrescribed();
}
Vec3 g = getGravity();
// set all joint unprescribed and set zero gravity
setAllJointsPrescribed(false);
setGravity(Vec3(0,0,0));
update_joint_local_info_short();
for (std::list<GBody *>::iterator iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); iter_pbody++) {
(*iter_pbody)->update_base_joint_info();
(*iter_pbody)->update_T();
(*iter_pbody)->set_eta_zero();
}
for (std::list<GBody *>::reverse_iterator riter_pbody = pBodies.rbegin(); riter_pbody != pBodies.rend(); riter_pbody++) {
(*riter_pbody)->update_aI();
(*riter_pbody)->update_Psi();
(*riter_pbody)->update_Pi();
}
for (i=0; i<A.ColSize(); i++) {
set_ddq(Zeros(pCoordinates.size(),1)); // this isn't necessary for real tree structure systems, but works for the cut joints in closed-loop
set_tau(&(A[i*A.RowSize()]));
for (std::list<GBody *>::reverse_iterator riter_pbody = pBodies.rbegin(); riter_pbody != pBodies.rend(); riter_pbody++) {
(*riter_pbody)->update_aB_zeroV_zeroeta();
(*riter_pbody)->update_beta_zeroeta();
}
for (std::list<GBody *>::iterator iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); iter_pbody++) {
(*iter_pbody)->update_ddq();
(*iter_pbody)->update_dV(true);
}
get_ddq(&(invM_A[i*invM_A.RowSize()]));
}
// restore
for (i=0, iter_pjoint = pJoints.begin(); iter_pjoint != pJoints.end(); i++, iter_pjoint++) {
(*iter_pjoint)->setPrescribed(isprescribed[i]);
}
setGravity(g);
return true;
}
void GSystem::getEquationsOfMotion(RMatrix &M, RMatrix &b)
{
RMatrix ddq = get_ddq(), tau = get_tau(); // save current ddq and tau
int n = getNumCoordinates();
M.ReNew(n,n);
set_ddq(Zeros(n,1));
GSystem::calcInverseDynamics();
b = get_tau();
for (int i=0; i<n; i++) {
RMatrix unit = Zeros(n,1);
unit[i] = 1;
set_ddq(unit);
GSystem::calcInverseDynamics();
get_tau(&M[i*n]);
for (int j=0; j<n; j++) {
M[i*n+j] -= b[j];
}
}
set_ddq(ddq); set_tau(tau); // restore ddq and tau
}
void Skeleton::computeEquationsOfMotionID(
const Eigen::Vector3d& _gravity)
{
int n = getDOF();
// skip immobile objects in forward simulation
if (getImmobileState() == true || n == 0)
{
return;
}
// Save current tau
Eigen::VectorXd tau_old = get_tau();
// Set ddq as zero
set_ddq(Eigen::VectorXd::Zero(n));
// M(q) * ddq + b(q,dq) = tau
computeInverseDynamicsLinear(_gravity, true);
mCg = get_tau();
// Calcualtion mass matrix, M
mM = Eigen::MatrixXd::Zero(n,n);
for (int i = 0; i < getNumBodyNodes(); i++)
{
BodyNode *nodei = getBodyNode(i);
nodei->updateMassMatrix();
nodei->aggregateMass(mM);
}
// Inverse of mass matrix
mMInv = mM.ldlt().solve(Eigen::MatrixXd::Identity(n,n));
// Restore the torque
set_tau(tau_old);
// Evaluate external forces in generalized coordinate.
updateExternalForces();
// Update damping forces
updateDampingForces();
}
Eigen::VectorXd Skeleton::computeInverseDynamicsLinear(
const Eigen::Vector3d& _gravity,
const Eigen::VectorXd* _qdot,
const Eigen::VectorXd* _qdotdot,
bool _computeJacobians,
bool _withExternalForces,
bool _withDampingForces)
{
int n = getDOF();
if (_qdot == NULL)
set_dq(Eigen::VectorXd::Zero(n));
if (_qdotdot == NULL)
set_ddq(Eigen::VectorXd::Zero(n));
_updateJointKinematics();
// Forward recursion
for (std::vector<dynamics::BodyNode*>::iterator itrBody = mBodyNodes.begin();
itrBody != mBodyNodes.end();
++itrBody) {
(*itrBody)->updateTransform();
(*itrBody)->updateVelocity(_computeJacobians);
(*itrBody)->updateEta();
(*itrBody)->updateAcceleration(_computeJacobians);
}
// Backward recursion
for (std::vector<dynamics::BodyNode*>::reverse_iterator ritrBody
= mBodyNodes.rbegin();
ritrBody != mBodyNodes.rend();
++ritrBody)
{
(*ritrBody)->updateBodyForce(_gravity,
_withExternalForces);
(*ritrBody)->updateGeneralizedForce(_withDampingForces);
}
return get_tau();
}