//============================================================================== void DynamicsTest::testImpulseBasedDynamics(const std::string& _fileName) { using namespace std; using namespace Eigen; using namespace dart; using namespace math; using namespace dynamics; using namespace simulation; using namespace utils; //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeletons #ifndef NDEBUG // Debug mode int nRandomItr = 1; #else int nRandomItr = 100; #endif // Lower and upper bound of configuration for system double lb = -1.5 * DART_PI; double ub = 1.5 * DART_PI; simulation::World* myWorld = NULL; //----------------------------- Tests ---------------------------------------- // Check whether multiplication of mass matrix and its inverse is identity // matrix. myWorld = utils::SkelParser::readWorld(_fileName); EXPECT_TRUE(myWorld != NULL); for (int i = 0; i < myWorld->getNumSkeletons(); ++i) { dynamics::Skeleton* skel = myWorld->getSkeleton(i); int dof = skel->getNumGenCoords(); // int nBodyNodes = skel->getNumBodyNodes(); if (dof == 0 || !skel->isMobile()) { dtdbg << "Skeleton [" << skel->getName() << "] is skipped since it has " << "0 DOF or is immobile." << endl; continue; } for (int j = 0; j < nRandomItr; ++j) { // Set random configurations for (int k = 0; k < skel->getNumBodyNodes(); ++k) { BodyNode* body = skel->getBodyNode(k); Joint* joint = body->getParentJoint(); int localDof = joint->getNumGenCoords(); for (int l = 0; l < localDof; ++l) { double lbRP = joint->getGenCoord(l)->getPosMin(); double ubRP = joint->getGenCoord(l)->getPosMax(); if (lbRP < -DART_PI) lbRP = -DART_PI; if (ubRP > DART_PI) ubRP = DART_PI; joint->setConfig(l, random(lbRP, ubRP), true, false, false); } } skel->setConfigs(VectorXd::Zero(dof)); // TODO(JS): Just clear what should be skel->clearExternalForces(); skel->clearConstraintImpulses(); // Set random impulses VectorXd impulses = VectorXd::Zero(dof); for (size_t k = 0; k < impulses.size(); ++k) impulses[k] = random(lb, ub); skel->setConstraintImpulses(impulses); // Compute impulse-based forward dynamics skel->computeImpulseForwardDynamics(); // Compare resultant velocity change and invM * impulses VectorXd deltaVel1 = skel->getVelsChange(); MatrixXd invM = skel->getInvMassMatrix(); VectorXd deltaVel2 = invM * impulses; EXPECT_TRUE(equals(deltaVel1, deltaVel2, 1e-6)); if (!equals(deltaVel1, deltaVel2, 1e-6)) { cout << "deltaVel1: " << deltaVel1.transpose() << endl; cout << "deltaVel2: " << deltaVel2.transpose() << endl; } } } delete myWorld; }