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();
}
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();
}
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
}
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();
}
void IAGRID::assign_tau(double* tau)
{
for(int m=0; m<M; m++)
tau[m] = get_tau(m);
}
Eigen::VectorXd Skeleton::getInternalForces() const
{
return get_tau();
}
