/// Add a DOF to a given joint
Joint* create1DOFJoint(BodyNode* parent, BodyNode* child, double val, double min, double max, int type) {
// Create the transformation based on the type
Joint* newJoint = NULL;
if(type == DOF_X)
newJoint = new PrismaticJoint(parent, child, Eigen::Vector3d(1.0, 0.0, 0.0));
else if(type == DOF_Y)
newJoint = new PrismaticJoint(parent, child, Eigen::Vector3d(0.0, 1.0, 0.0));
else if(type == DOF_Z)
newJoint = new PrismaticJoint(parent, child, Eigen::Vector3d(0.0, 0.0, 1.0));
else if(type == DOF_YAW)
newJoint = new RevoluteJoint(parent, child, Eigen::Vector3d(0.0, 0.0, 1.0));
else if(type == DOF_PITCH)
newJoint = new RevoluteJoint(parent, child, Eigen::Vector3d(0.0, 1.0, 0.0));
else if(type == DOF_ROLL)
newJoint = new RevoluteJoint(parent, child, Eigen::Vector3d(1.0, 0.0, 0.0));
// Add the transformation to the joint, set the min/max values and set it to the skeleton
newJoint->getGenCoord(0)->set_q(val);
newJoint->getGenCoord(0)->set_qMin(min);
newJoint->getGenCoord(0)->set_qMax(max);
return newJoint;
}
//==============================================================================
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;
}
//==============================================================================
void DynamicsTest::centerOfMass(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 = 10;
#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;
// Lower and upper bound of joint damping and stiffness
double lbD = 0.0;
double ubD = 10.0;
double lbK = 0.0;
double ubK = 10.0;
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)
{
dtmsg << "Skeleton [" << skel->getName() << "] is skipped since it has "
<< "0 DOF." << endl;
continue;
}
for (int j = 0; j < nRandomItr; ++j)
{
// Random joint stiffness and damping coefficient
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)
{
joint->setDampingCoefficient(l, random(lbD, ubD));
joint->setSpringStiffness (l, random(lbK, ubK));
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->setRestPosition (l, random(lbRP, ubRP));
}
}
// Set random states
VectorXd x = skel->getState();
for (int k = 0; k < x.size(); ++k)
x[k] = random(lb, ub);
skel->setState(x, true, true, false);
VectorXd tau = skel->getGenForces();
for (int k = 0; k < tau.size(); ++k)
tau[k] = random(lb, ub);
skel->setGenForces(tau);
skel->computeForwardDynamics();
VectorXd q = skel->getConfigs();
VectorXd dq = skel->getGenVels();
VectorXd ddq = skel->getGenAccs();
VectorXd com = skel->getWorldCOM();
VectorXd dcom = skel->getWorldCOMVelocity();
VectorXd ddcom = skel->getWorldCOMAcceleration();
MatrixXd comJ = skel->getWorldCOMJacobian();
MatrixXd comdJ = skel->getWorldCOMJacobianTimeDeriv();
VectorXd dcom2 = comJ * dq;
VectorXd ddcom2 = comdJ * dq + comJ * ddq;
EXPECT_TRUE(equals(dcom, dcom2, 1e-6));
if (!equals(dcom, dcom2, 1e-6))
{
cout << "dcom :" << dcom.transpose() << endl;
cout << "dcom2:" << dcom2.transpose() << endl;
}
EXPECT_TRUE(equals(ddcom, ddcom2, 1e-6));
if (!equals(ddcom, ddcom2, 1e-6))
{
cout << "ddcom :" << ddcom.transpose() << endl;
cout << "ddcom2:" << ddcom2.transpose() << endl;
}
}
}
delete myWorld;
}
//==============================================================================
void DynamicsTest::compareEquationsOfMotion(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 = 5;
#else
int nRandomItr = 100;
#endif
// Lower and upper bound of configuration for system
double lb = -1.0 * DART_PI;
double ub = 1.0 * DART_PI;
// Lower and upper bound of joint damping and stiffness
double lbD = 0.0;
double ubD = 10.0;
double lbK = 0.0;
double ubK = 10.0;
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)
{
dtmsg << "Skeleton [" << skel->getName() << "] is skipped since it has "
<< "0 DOF." << endl;
continue;
}
for (int j = 0; j < nRandomItr; ++j)
{
// Random joint stiffness and damping coefficient
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)
{
joint->setDampingCoefficient(l, random(lbD, ubD));
joint->setSpringStiffness (l, random(lbK, ubK));
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->setRestPosition (l, random(lbRP, ubRP));
}
}
// Set random states
VectorXd x = skel->getState();
for (int k = 0; k < x.size(); ++k)
x[k] = random(lb, ub);
skel->setState(x, true, true, false);
//------------------------ Mass Matrix Test ----------------------------
// Get matrices
MatrixXd M = skel->getMassMatrix();
MatrixXd M2 = getMassMatrix(skel);
MatrixXd InvM = skel->getInvMassMatrix();
MatrixXd M_InvM = M * InvM;
MatrixXd InvM_M = InvM * M;
MatrixXd AugM = skel->getAugMassMatrix();
MatrixXd AugM2 = getAugMassMatrix(skel);
MatrixXd InvAugM = skel->getInvAugMassMatrix();
MatrixXd AugM_InvAugM = AugM * InvAugM;
MatrixXd InvAugM_AugM = InvAugM * AugM;
MatrixXd I = MatrixXd::Identity(dof, dof);
// Check if the number of generalized coordinates and dimension of mass
// matrix are same.
EXPECT_EQ(M.rows(), dof);
EXPECT_EQ(M.cols(), dof);
// Check mass matrix
EXPECT_TRUE(equals(M, M2, 1e-6));
if (!equals(M, M2, 1e-6))
{
cout << "M :" << endl << M << endl << endl;
cout << "M2:" << endl << M2 << endl << endl;
}
// Check augmented mass matrix
EXPECT_TRUE(equals(AugM, AugM2, 1e-6));
if (!equals(AugM, AugM2, 1e-6))
{
cout << "AugM :" << endl << AugM << endl << endl;
cout << "AugM2:" << endl << AugM2 << endl << endl;
}
// Check if both of (M * InvM) and (InvM * M) are identity.
EXPECT_TRUE(equals(M_InvM, I, 1e-6));
if (!equals(M_InvM, I, 1e-6))
{
cout << "InvM :" << endl << InvM << endl << endl;
}
EXPECT_TRUE(equals(InvM_M, I, 1e-6));
if (!equals(InvM_M, I, 1e-6))
{
cout << "InvM_M:" << endl << InvM_M << endl << endl;
}
// Check if both of (M * InvM) and (InvM * M) are identity.
EXPECT_TRUE(equals(AugM_InvAugM, I, 1e-6));
if (!equals(AugM_InvAugM, I, 1e-6))
{
cout << "AugM_InvAugM :" << endl << AugM_InvAugM << endl << endl;
}
EXPECT_TRUE(equals(InvAugM_AugM, I, 1e-6));
if (!equals(InvAugM_AugM, I, 1e-6))
{
cout << "InvAugM_AugM:" << endl << InvAugM_AugM << endl << endl;
}
//------- Coriolis Force Vector and Combined Force Vector Tests --------
// Get C1, Coriolis force vector using recursive method
VectorXd C = skel->getCoriolisForceVector();
VectorXd Cg = skel->getCombinedVector();
// Get C2, Coriolis force vector using inverse dynamics algorithm
Vector3d oldGravity = skel->getGravity();
VectorXd oldTau = skel->getInternalForceVector();
VectorXd oldDdq = skel->getGenAccs();
// TODO(JS): Save external forces of body nodes
skel->clearInternalForces();
skel->clearExternalForces();
skel->setGenAccs(VectorXd::Zero(dof), true);
EXPECT_TRUE(skel->getInternalForceVector() == VectorXd::Zero(dof));
EXPECT_TRUE(skel->getExternalForceVector() == VectorXd::Zero(dof));
EXPECT_TRUE(skel->getGenAccs() == VectorXd::Zero(dof));
skel->setGravity(Vector3d::Zero());
EXPECT_TRUE(skel->getGravity() == Vector3d::Zero());
skel->computeInverseDynamics(false, false);
VectorXd C2 = skel->getGenForces();
skel->setGravity(oldGravity);
EXPECT_TRUE(skel->getGravity() == oldGravity);
skel->computeInverseDynamics(false, false);
VectorXd Cg2 = skel->getGenForces();
EXPECT_TRUE(equals(C, C2, 1e-6));
if (!equals(C, C2, 1e-6))
{
cout << "C :" << C.transpose() << endl;
cout << "C2:" << C2.transpose() << endl;
}
EXPECT_TRUE(equals(Cg, Cg2, 1e-6));
if (!equals(Cg, Cg2, 1e-6))
{
cout << "Cg :" << Cg.transpose() << endl;
cout << "Cg2:" << Cg2.transpose() << endl;
}
skel->setGenForces(oldTau);
skel->setGenAccs(oldDdq, false);
// TODO(JS): Restore external forces of body nodes
//------------------- Combined Force Vector Test -----------------------
// TODO(JS): Not implemented yet.
//---------------------- Damping Force Test ----------------------------
// TODO(JS): Not implemented yet.
//--------------------- External Force Test ----------------------------
// TODO(JS): Not implemented yet.
}
}
delete myWorld;
}