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(); }