void Skeleton::clearExternalForces()
{
int nNodes = getNumBodyNodes();
for (int i = 0; i < nNodes; i++)
mBodyNodes[i]->clearExternalForces();
}
//==============================================================================
void Linkage::satisfyCriteria()
{
std::vector<BodyNode*> bns = mCriteria.satisfy();
while(getNumBodyNodes() > 0)
unregisterBodyNode(mBodyNodes.back());
for(BodyNode* bn : bns)
{
registerBodyNode(bn);
}
update();
}
int Skeleton::getBodyNodeIndex(const std::string& _name) const
{
const int nNodes = getNumBodyNodes();
for(int i = 0; i < nNodes; i++)
{
BodyNode* node = getBodyNode(i);
if (_name == node->getName())
return i;
}
return -1;
}
Eigen::Vector3d Skeleton::getWorldCOM()
{
Eigen::Vector3d com(0, 0, 0);
assert(mTotalMass != 0);
const int nNodes = getNumBodyNodes();
for(int i = 0; i < nNodes; i++)
{
BodyNode* bodyNode = getBodyNode(i);
com += (bodyNode->getMass() * bodyNode->getWorldCOM());
}
return com / mTotalMass;
}
void Skeleton::initKinematics()
{
mRootBodyNode = mBodyNodes[0];
mToRootBody = mFrame.inverse() * mRootBodyNode->getWorldInvTransform();
// init the dependsOnDof stucture for each bodylink
for(int i = 0; i < getNumBodyNodes(); i++)
{
mBodyNodes.at(i)->setSkeleton(this);
mBodyNodes.at(i)->setDependDofList();
mBodyNodes.at(i)->init();
}
updateForwardKinematics();
}
//==============================================================================
void ReferentialSkeleton::clearCollidingBodies()
{
for (auto i = 0u; i < getNumBodyNodes(); ++i)
{
auto bodyNode = getBodyNode(i);
DART_SUPPRESS_DEPRECATED_BEGIN
bodyNode->setColliding(false);
DART_SUPPRESS_DEPRECATED_END
auto softBodyNode = bodyNode->asSoftBodyNode();
if (softBodyNode)
{
auto& pointMasses = softBodyNode->getPointMasses();
for (auto pointMass : pointMasses)
pointMass->setColliding(false);
}
}
}
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 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();
}
