//==============================================================================
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;
}
