bool GSystem::calcProductOfInvMassAndMatrix2(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();
}
setAllJointsPrescribed(false);
initDynamicsWithZeroGravityAndVelocity();
for (i=0; i<A.ColSize(); i++) {
set_tau(&(A[i*A.RowSize()]));
calcDynamicsWithZeroGravityAndVelocity();
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]);
}
return true;
}
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 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
}
void Skeleton::initDynamics()
{
initKinematics();
int DOF = getDOF();
mM = Eigen::MatrixXd::Zero(DOF, DOF);
mMInv = Eigen::MatrixXd::Zero(DOF, DOF);
mC = Eigen::MatrixXd::Zero(DOF, DOF);
mCvec = Eigen::VectorXd::Zero(DOF);
mG = Eigen::VectorXd::Zero(DOF);
mCg = Eigen::VectorXd::Zero(DOF);
set_tau(Eigen::VectorXd::Zero(DOF));
mFext = Eigen::VectorXd::Zero(DOF);
mFc = Eigen::VectorXd::Zero(DOF);
mDampingForce = Eigen::VectorXd::Zero(DOF);
// calculate mass
// init the dependsOnDof stucture for each bodylink
mTotalMass = 0.0;
for(int i = 0; i < getNumBodyNodes(); i++)
mTotalMass += getBodyNode(i)->getMass();
}
void Skeleton::clearInternalForces()
{
set_tau(Eigen::VectorXd::Zero(getDOF()));
}
void Skeleton::setInternalForces(const Eigen::VectorXd& _forces)
{
set_tau(_forces);
}
