//==============================================================================
void DynamicsTest::compareAccelerations(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 -------------------------------------
const double TOLERANCE = 1.0e-2;
#ifndef NDEBUG // Debug mode
int nRandomItr = 2;
#else
int nRandomItr = 10;
#endif
double qLB = -0.5 * DART_PI;
double qUB = 0.5 * DART_PI;
double dqLB = -0.5 * DART_PI;
double dqUB = 0.5 * DART_PI;
double ddqLB = -0.5 * DART_PI;
double ddqUB = 0.5 * DART_PI;
Vector3d gravity(0.0, -9.81, 0.0);
double timeStep = 1.0e-6;
// load skeleton
World* world = SkelParser::readWorld(_fileName);
assert(world != NULL);
world->setGravity(gravity);
world->setTimeStep(timeStep);
//------------------------------ Tests ---------------------------------------
for (int i = 0; i < world->getNumSkeletons(); ++i)
{
Skeleton* skeleton = world->getSkeleton(i);
assert(skeleton != NULL);
int dof = skeleton->getNumGenCoords();
for (int j = 0; j < nRandomItr; ++j)
{
// Generate a random state and ddq
VectorXd q = VectorXd(dof);
VectorXd dq = VectorXd(dof);
VectorXd ddq = VectorXd(dof);
for (int k = 0; k < dof; ++k)
{
q[k] = math::random(qLB, qUB);
dq[k] = math::random(dqLB, dqUB);
ddq[k] = math::random(ddqLB, ddqUB);
// q[k] = 0.0;
// dq[k] = 0.0;
// ddq[k] = 0.0;
}
VectorXd x = VectorXd::Zero(dof * 2);
x << q, dq;
skeleton->setState(x, true, true, false);
skeleton->setGenAccs(ddq, true);
// Integrate state
skeleton->integrateConfigs(timeStep);
skeleton->integrateGenVels(timeStep);
VectorXd qNext = skeleton->getConfigs();
VectorXd dqNext = skeleton->getGenVels();
VectorXd xNext = VectorXd::Zero(dof * 2);
xNext << qNext, dqNext;
// For each body node
for (int k = 0; k < skeleton->getNumBodyNodes(); ++k)
{
BodyNode* bn = skeleton->getBodyNode(k);
int nDepGenCoord = bn->getNumDependentGenCoords();
// Calculation of velocities and Jacobian at k-th time step
skeleton->setState(x, true, true, false);
skeleton->setGenAccs(ddq, true);
Vector6d vBody1 = bn->getBodyVelocity();
Vector6d vWorld1 = bn->getWorldVelocity();
MatrixXd JBody1 = bn->getBodyJacobian();
MatrixXd JWorld1 = bn->getWorldJacobian();
Isometry3d T1 = bn->getWorldTransform();
// Get accelerations and time derivatives of Jacobians at k-th time step
Vector6d aBody1 = bn->getBodyAcceleration();
Vector6d aWorld1 = bn->getWorldAcceleration();
MatrixXd dJBody1 = bn->getBodyJacobianTimeDeriv();
MatrixXd dJWorld1 = bn->getWorldJacobianTimeDeriv();
// Calculation of velocities and Jacobian at (k+1)-th time step
skeleton->setState(xNext, true, true, false);
skeleton->setGenAccs(ddq, true);
Vector6d vBody2 = bn->getBodyVelocity();
Vector6d vWorld2 = bn->getWorldVelocity();
MatrixXd JBody2 = bn->getBodyJacobian();
MatrixXd JWorld2 = bn->getWorldJacobian();
Isometry3d T2 = bn->getWorldTransform();
// Get accelerations and time derivatives of Jacobians at k-th time step
Vector6d aBody2 = bn->getBodyAcceleration();
Vector6d aWorld2 = bn->getWorldAcceleration();
MatrixXd dJBody2 = bn->getBodyJacobianTimeDeriv();
MatrixXd dJWorld2 = bn->getWorldJacobianTimeDeriv();
// Calculation of approximated accelerations and time derivatives of
// Jacobians
Vector6d aBodyApprox = (vBody2 - vBody1) / timeStep;
Vector6d aWorldApprox = (vWorld2 - vWorld1) / timeStep;
// TODO(JS): Finite difference of Jacobian test is not implemented yet.
// MatrixXd dJBodyApprox = (JBody2 - JBody1) / timeStep;
// MatrixXd dJWorldApprox = (JWorld2 - JWorld1) / timeStep;
// MatrixXd dJBodyApprox = MatrixXd::Zero(6, nDepGenCoord);
// MatrixXd dJWorldApprox = MatrixXd::Zero(6, nDepGenCoord);
// for (int l = 0; l < nDepGenCoord; ++l)
// {
// skeleton->setConfig(q);
// Jacobian JBody_a = bn->getBodyJacobian();
// int idx = bn->getDependentGenCoordIndex(l);
// VectorXd qGrad = q;
// qGrad[idx] = qNext[idx];
// skeleton->setConfig(qGrad);
// Jacobian JBody_b = bn->getBodyJacobian();
// Jacobian dJBody_dq = (JBody_b - JBody_a) / (qNext[idx] - q[idx]);
// dJBodyApprox += dJBody_dq * dq[idx];
// }
// Comparing two velocities
EXPECT_TRUE(equals(aBody1, aBodyApprox, TOLERANCE));
EXPECT_TRUE(equals(aBody2, aBodyApprox, TOLERANCE));
EXPECT_TRUE(equals(aWorld1, aWorldApprox, TOLERANCE));
EXPECT_TRUE(equals(aWorld2, aWorldApprox, TOLERANCE));
// EXPECT_TRUE(equals(dJBody1, dJBodyApprox, TOLERANCE));
// EXPECT_TRUE(equals(dJBody2, dJBodyApprox, TOLERANCE));
// EXPECT_TRUE(equals(dJWorld1, dJWorldApprox, TOLERANCE));
// EXPECT_TRUE(equals(dJWorld2, dJWorldApprox, TOLERANCE));
// Debugging code
if (!equals(aBody1, aBodyApprox, TOLERANCE))
{
cout << "aBody1 :" << aBody1.transpose() << endl;
cout << "aBodyApprox:" << aBodyApprox.transpose() << endl;
}
if (!equals(aBody2, aBodyApprox, TOLERANCE))
{
cout << "aBody2 :" << aBody2.transpose() << endl;
cout << "aBodyApprox:" << aBodyApprox.transpose() << endl;
}
if (!equals(aWorld1, aWorldApprox, TOLERANCE))
{
cout << "aWorld1 :" << aWorld1.transpose() << endl;
cout << "aWorldApprox:" << aWorldApprox.transpose() << endl;
}
if (!equals(aWorld2, aWorldApprox, TOLERANCE))
{
cout << "aWorld2 :" << aWorld2.transpose() << endl;
cout << "aWorldApprox:" << aWorldApprox.transpose() << endl;
}
// if (!equals(dJBody1, dJBodyApprox, TOLERANCE))
// {
// cout << "Name :" << bn->getName() << endl;
// cout << "dJBody1 :" << endl << dJBody1 << endl;
// cout << "dJBodyApprox:" << endl << dJBodyApprox << endl;
// }
// if (!equals(dJBody2, dJBodyApprox, TOLERANCE))
// {
// cout << "dJBody2:" << endl << dJBody2.transpose() << endl;
// cout << "dJBodyApprox:" << endl << dJBodyApprox.transpose() << endl;
// }
// if (!equals(dJWorld1, dJWorldApprox, TOLERANCE))
// {
// cout << "dJWorld1 :" << endl << dJWorld1 << endl;
// cout << "dJWorldApprox:" << endl << dJWorldApprox << endl;
// }
// if (!equals(dJWorld2, dJWorldApprox, TOLERANCE))
// {
// cout << "dJWorld2 :" << endl << dJWorld2 << endl;
// cout << "dJWorldApprox:" << endl << dJWorldApprox << endl;
// }
}
}
}
delete world;
}
//==============================================================================
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;
}
Skeleton* createFreeFloatingTwoLinkRobot(Vector3d dim1,
Vector3d dim2, TypeOfDOF type2,
bool finished = true)
{
Skeleton* robot = new Skeleton();
// Create the first link, the joint with the ground and its shape
double mass = 1.0;
BodyNode* node = new BodyNode("link1");
Joint* joint = new FreeJoint();
joint->setName("joint1");
Shape* shape = new BoxShape(dim1);
node->setLocalCOM(Vector3d(0.0, 0.0, dim1(2)/2.0));
node->addVisualizationShape(shape);
node->addCollisionShape(shape);
node->setMass(mass);
node->setParentJoint(joint);
robot->addBodyNode(node);
// Create the second link, the joint with link1 and its shape
BodyNode* parent_node = robot->getBodyNode("link1");
node = new BodyNode("link2");
joint = create1DOFJoint(0.0, -DART_PI, DART_PI, type2);
joint->setName("joint2");
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.translate(Eigen::Vector3d(0.0, 0.0, dim1(2)));
joint->setTransformFromParentBodyNode(T);
shape = new BoxShape(dim2);
node->setLocalCOM(Vector3d(0.0, 0.0, dim2(2)/2.0));
node->addVisualizationShape(shape);
node->addCollisionShape(shape);
node->setMass(mass);
node->setParentJoint(joint);
parent_node->addChildBodyNode(node);
robot->addBodyNode(node);
// If finished, initialize the skeleton
if(finished)
{
addEndEffector(robot, node, dim2);
robot->init();
}
return robot;
}
//==============================================================================
void DynamicsTest::compareVelocities(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 -------------------------------------
const double TOLERANCE = 1.0e-6;
#ifndef NDEBUG // Debug mode
int nRandomItr = 10;
#else
int nRandomItr = 1;
#endif
double qLB = -0.5 * DART_PI;
double qUB = 0.5 * DART_PI;
double dqLB = -0.5 * DART_PI;
double dqUB = 0.5 * DART_PI;
double ddqLB = -0.5 * DART_PI;
double ddqUB = 0.5 * DART_PI;
Vector3d gravity(0.0, -9.81, 0.0);
// load skeleton
World* world = SkelParser::readWorld(_fileName);
assert(world != NULL);
world->setGravity(gravity);
//------------------------------ Tests ---------------------------------------
for (int i = 0; i < world->getNumSkeletons(); ++i)
{
Skeleton* skeleton = world->getSkeleton(i);
assert(skeleton != NULL);
int dof = skeleton->getNumGenCoords();
for (int j = 0; j < nRandomItr; ++j)
{
// Generate a random state
VectorXd q = VectorXd(dof);
VectorXd dq = VectorXd(dof);
VectorXd ddq = VectorXd(dof);
for (int k = 0; k < dof; ++k)
{
q[k] = math::random(qLB, qUB);
dq[k] = math::random(dqLB, dqUB);
ddq[k] = math::random(ddqLB, ddqUB);
}
VectorXd state = VectorXd::Zero(dof * 2);
state << q, dq;
skeleton->setState(state, true, true, true);
skeleton->setGenAccs(ddq, true);
skeleton->computeInverseDynamics(false, false);
// For each body node
for (int k = 0; k < skeleton->getNumBodyNodes(); ++k)
{
BodyNode* bn = skeleton->getBodyNode(k);
// Calculation of velocities using recursive method
Vector6d vBody = bn->getBodyVelocity();
Vector6d vWorld = bn->getWorldVelocity();
Vector6d aBody = bn->getBodyAcceleration();
Vector6d aWorld = bn->getWorldAcceleration();
// Calculation of velocities using Jacobian and dq
MatrixXd JBody = bn->getBodyJacobian();
MatrixXd JWorld = bn->getWorldJacobian();
MatrixXd dJBody = bn->getBodyJacobianTimeDeriv();
MatrixXd dJWorld = bn->getWorldJacobianTimeDeriv();
Vector6d vBody2 = Vector6d::Zero();
Vector6d vWorld2 = Vector6d::Zero();
Vector6d aBody2 = Vector6d::Zero();
Vector6d aWorld2 = Vector6d::Zero();
for (int l = 0; l < bn->getNumDependentGenCoords(); ++l)
{
int idx = bn->getDependentGenCoordIndex(l);
vBody2 += JBody.col(l) * dq[idx];
vWorld2 += JWorld.col(l) * dq[idx];
aBody2 += dJBody.col(l) * dq[idx] + JBody.col(l) * ddq[idx];
aWorld2 += dJWorld.col(l) * dq[idx] + JWorld.col(l) * ddq[idx];
}
// Comparing two velocities
EXPECT_TRUE(equals(vBody, vBody2, TOLERANCE));
EXPECT_TRUE(equals(vWorld, vWorld2, TOLERANCE));
EXPECT_TRUE(equals(aBody, aBody2, TOLERANCE));
EXPECT_TRUE(equals(aWorld, aWorld2, TOLERANCE));
// Debugging code
if (!equals(vBody, vBody2, TOLERANCE))
{
cout << "vBody : " << vBody.transpose() << endl;
cout << "vBody2: " << vBody2.transpose() << endl;
}
if (!equals(vWorld, vWorld2, TOLERANCE))
{
cout << "vWorld : " << vWorld.transpose() << endl;
cout << "vWorld2: " << vWorld2.transpose() << endl;
}
if (!equals(aBody, aBody2, TOLERANCE))
{
cout << "aBody : " << aBody.transpose() << endl;
cout << "aBody2: " << aBody2.transpose() << endl;
}
if (!equals(aWorld, aWorld2, TOLERANCE))
{
cout << "aWorld : " << aWorld.transpose() << endl;
cout << "aWorld2: " << aWorld2.transpose() << endl;
}
}
}
}
delete world;
}
//==============================================================================
void ConstraintTest::SingleContactTest(const std::string& _fileName)
{
using namespace std;
using namespace Eigen;
using namespace dart::math;
using namespace dart::collision;
using namespace dart::constraint;
using namespace dart::dynamics;
using namespace dart::simulation;
using namespace dart::utils;
//----------------------------------------------------------------------------
// Settings
//----------------------------------------------------------------------------
// Number of random state tests for each skeletons
#ifndef NDEBUG // Debug mode
// size_t testCount = 1;
#else
// size_t testCount = 1;
#endif
World* world = new World;
EXPECT_TRUE(world != NULL);
world->setGravity(Vector3d(0.0, -10.00, 0.0));
world->setTimeStep(0.001);
world->getConstraintSolver()->setCollisionDetector(
new DARTCollisionDetector());
Skeleton* sphereSkel = createSphere(0.05, Vector3d(0.0, 1.0, 0.0));
BodyNode* sphere = sphereSkel->getBodyNode(0);
Joint* sphereJoint = sphere->getParentJoint();
sphereJoint->setVelocity(3, random(-2.0, 2.0)); // x-axis
sphereJoint->setVelocity(5, random(-2.0, 2.0)); // z-axis
world->addSkeleton(sphereSkel);
EXPECT_EQ(sphereSkel->getGravity(), world->getGravity());
assert(sphere);
Skeleton* boxSkel = createBox(Vector3d(1.0, 1.0, 1.0),
Vector3d(0.0, 1.0, 0.0));
BodyNode* box = boxSkel->getBodyNode(0);
Joint* boxJoint = box->getParentJoint();
boxJoint->setVelocity(3, random(-2.0, 2.0)); // x-axis
boxJoint->setVelocity(5, random(-2.0, 2.0)); // z-axis
// world->addSkeleton(boxSkel);
// EXPECT_EQ(boxSkel->getGravity(), world->getGravity());
// assert(box);
Skeleton* groundSkel = createGround(Vector3d(10000.0, 0.1, 10000.0),
Vector3d(0.0, -0.05, 0.0));
groundSkel->setMobile(false);
// BodyNode* ground = groundSkel->getBodyNode(0);
world->addSkeleton(groundSkel);
EXPECT_EQ(groundSkel->getGravity(), world->getGravity());
// assert(ground);
EXPECT_EQ(world->getNumSkeletons(), 2);
ConstraintSolver* cs = world->getConstraintSolver();
CollisionDetector* cd = cs->getCollisionDetector();
// Lower and upper bound of configuration for system
// double lb = -1.5 * DART_PI;
// double ub = 1.5 * DART_PI;
int maxSteps = 500;
for (int i = 0; i < maxSteps; ++i)
{
// Vector3d pos1 = sphere->getWorldTransform().translation();
// Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);
// std::cout << "pos1:" << pos1.transpose() << std::endl;
// std::cout << "vel1:" << vel1.transpose() << std::endl;
cd->detectCollision(true, true);
if (cd->getNumContacts() == 0)
{
world->step();
continue;
}
// for (size_t j = 0; j < cd->getNumContacts(); ++j)
// {
// Contact contact = cd->getContact(j);
// Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
// Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);
// std::cout << "pos1:" << pos1.transpose() << std::endl;
// std::cout << "vel1:" << vel1.transpose() << std::endl;
// }
world->step();
for (size_t j = 0; j < cd->getNumContacts(); ++j)
{
Contact contact = cd->getContact(j);
Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);
// std::cout << "pos1:" << pos1.transpose() << std::endl;
// std::cout << "pos1[1]: " << pos1[1] << std::endl;
// std::cout << "pos1:" << pos1.transpose() << std::endl;
std::cout << "vel1:" << vel1.transpose() << ", pos1[1]: " << pos1[1] << std::endl;
// EXPECT_NEAR(pos1[0], 0.0, 1e-9);
// EXPECT_NEAR(pos1[1], -0.05, 1e-2);
// EXPECT_NEAR(pos1[2], 0.0, 1e-9);
// EXPECT_NEAR(vel1[0], 0.0, 1e-9);
// EXPECT_NEAR(vel1[1], 0.0, 1e-9);
// EXPECT_NEAR(vel1[2], 0.0, 1e-9);
// if (!equals(vel1, Vector3d(0.0, 0.0, 0.0)))
// std::cout << "vel1:" << vel1.transpose() << std::endl;
// EXPECT_EQ(vel1, Vector3d::Zero());
}
// std::cout << std::endl;
break;
}
delete world;
}
//==============================================================================
void JOINTS::kinematicsTest(Joint* _joint)
{
assert(_joint);
int numTests = 1;
_joint->setTransformFromChildBodyNode(
math::expMap(Eigen::Vector6d::Random()));
_joint->setTransformFromParentBodyNode(
math::expMap(Eigen::Vector6d::Random()));
BodyNode* bodyNode = new BodyNode();
bodyNode->setParentJoint(_joint);
Skeleton skeleton;
skeleton.addBodyNode(bodyNode);
skeleton.init();
int dof = _joint->getNumDofs();
//--------------------------------------------------------------------------
//
//--------------------------------------------------------------------------
VectorXd q = VectorXd::Zero(dof);
VectorXd dq = VectorXd::Zero(dof);
for (int idxTest = 0; idxTest < numTests; ++idxTest)
{
double q_delta = 0.000001;
for (int i = 0; i < dof; ++i)
{
q(i) = random(-DART_PI*1.0, DART_PI*1.0);
dq(i) = random(-DART_PI*1.0, DART_PI*1.0);
}
skeleton.setPositions(q);
skeleton.setVelocities(dq);
skeleton.computeForwardKinematics(true, true, false);
if (_joint->getNumDofs() == 0)
return;
Eigen::Isometry3d T = _joint->getLocalTransform();
Jacobian J = _joint->getLocalJacobian();
Jacobian dJ = _joint->getLocalJacobianTimeDeriv();
//--------------------------------------------------------------------------
// Test T
//--------------------------------------------------------------------------
EXPECT_TRUE(math::verifyTransform(T));
//--------------------------------------------------------------------------
// Test analytic Jacobian and numerical Jacobian
// J == numericalJ
//--------------------------------------------------------------------------
Jacobian numericJ = Jacobian::Zero(6,dof);
for (int i = 0; i < dof; ++i)
{
// a
Eigen::VectorXd q_a = q;
_joint->setPositions(q_a);
skeleton.computeForwardKinematics(true, false, false);
Eigen::Isometry3d T_a = _joint->getLocalTransform();
// b
Eigen::VectorXd q_b = q;
q_b(i) += q_delta;
_joint->setPositions(q_b);
skeleton.computeForwardKinematics(true, false, false);
Eigen::Isometry3d T_b = _joint->getLocalTransform();
//
Eigen::Isometry3d Tinv_a = T_a.inverse();
Eigen::Matrix4d Tinv_a_eigen = Tinv_a.matrix();
// dTdq
Eigen::Matrix4d T_a_eigen = T_a.matrix();
Eigen::Matrix4d T_b_eigen = T_b.matrix();
Eigen::Matrix4d dTdq_eigen = (T_b_eigen - T_a_eigen) / q_delta;
//Matrix4d dTdq_eigen = (T_b_eigen * T_a_eigen.inverse()) / dt;
// J(i)
Eigen::Matrix4d Ji_4x4matrix_eigen = Tinv_a_eigen * dTdq_eigen;
Eigen::Vector6d Ji;
Ji[0] = Ji_4x4matrix_eigen(2,1);
Ji[1] = Ji_4x4matrix_eigen(0,2);
Ji[2] = Ji_4x4matrix_eigen(1,0);
Ji[3] = Ji_4x4matrix_eigen(0,3);
Ji[4] = Ji_4x4matrix_eigen(1,3);
Ji[5] = Ji_4x4matrix_eigen(2,3);
numericJ.col(i) = Ji;
}
if (dynamic_cast<BallJoint*>(_joint) || dynamic_cast<FreeJoint*>(_joint))
{
// Skip this test for BallJoint and FreeJoint since Jacobian of BallJoint
// and FreeJoint is not obtained by the above method.
}
else
{
for (int i = 0; i < dof; ++i)
for (int j = 0; j < 6; ++j)
EXPECT_NEAR(J.col(i)(j), numericJ.col(i)(j), JOINT_TOL);
}
//--------------------------------------------------------------------------
// Test first time derivative of analytic Jacobian and numerical Jacobian
// dJ == numerical_dJ
//--------------------------------------------------------------------------
Jacobian numeric_dJ = Jacobian::Zero(6,dof);
for (int i = 0; i < dof; ++i)
{
// a
Eigen::VectorXd q_a = q;
_joint->setPositions(q_a);
skeleton.computeForwardKinematics(true, false, false);
Jacobian J_a = _joint->getLocalJacobian();
// b
Eigen::VectorXd q_b = q;
q_b(i) += q_delta;
_joint->setPositions(q_b);
skeleton.computeForwardKinematics(true, false, false);
Jacobian J_b = _joint->getLocalJacobian();
//
Jacobian dJ_dq = (J_b - J_a) / q_delta;
// J(i)
numeric_dJ += dJ_dq * dq(i);
}
if (dynamic_cast<BallJoint*>(_joint) || dynamic_cast<FreeJoint*>(_joint))
{
// Skip this test for BallJoint and FreeJoint since time derivative of
// Jacobian of BallJoint and FreeJoint is not obtained by the above
// method.
}
else
{
for (int i = 0; i < dof; ++i)
for (int j = 0; j < 6; ++j)
EXPECT_NEAR(dJ.col(i)(j), numeric_dJ.col(i)(j), JOINT_TOL);
}
}
// Forward kinematics test with high joint position
double posMin = -1e+64;
double posMax = +1e+64;
for (int idxTest = 0; idxTest < numTests; ++idxTest)
{
for (int i = 0; i < dof; ++i)
q(i) = random(posMin, posMax);
skeleton.setPositions(q);
skeleton.computeForwardKinematics(true, false, false);
if (_joint->getNumDofs() == 0)
return;
Eigen::Isometry3d T = _joint->getLocalTransform();
EXPECT_TRUE(math::verifyTransform(T));
}
}
BodyNode* addBodyNode(BodyNode* bn, const std::string& name)
{
BodyNode* result = bn->createChildJointAndBodyNodePair<JointType>().second;
result->setName(name);
return result;
}
/* ********************************************************************************************* */
int main(int argc, char* argv[]) {
// Create Left Leg skeleton
Skeleton LeftLegSkel;
// Pointers to be used during the Skeleton building
Matrix3d inertiaMatrix;
inertiaMatrix << 0, 0, 0, 0, 0, 0, 0, 0, 0;
double mass = 1.0;
// ***** BodyNode 1: Left Hip Yaw (LHY) ***** *
BodyNode* node = new BodyNode("LHY");
Joint* joint = create1DOFJoint(NULL, node, 0.0, 0.0, DART_PI, DOF_YAW);
joint->setName("LHY");
Shape* shape = new BoxShape(Vector3d(0.3, 0.3, 1.0));
node->addVisualizationShape(shape);
node->addCollisionShape(shape);
node->setMass(mass);
LeftLegSkel.addBodyNode(node);
// ***** BodyNode 2: Left Hip Roll (LHR) whose parent is: LHY *****\
BodyNode* parent_node = LeftLegSkel.getBodyNode("LHY");
node = new BodyNode("LHR");
joint = create1DOFJoint(parent_node, node, 0.0, 0.0, DART_PI, DOF_ROLL);
joint->setName("LHR");
Eigen::Isometry3d T(Eigen::Translation3d(0.0, 0.0, 0.5));
joint->setTransformFromParentBodyNode(T);
shape = new BoxShape(Vector3d(0.3, 0.3, 1.0));
shape->setOffset(Vector3d(0.0, 0.0, 0.5));
node->setLocalCOM(shape->getOffset());
node->setMass(mass);
node->addVisualizationShape(shape);
node->addCollisionShape(shape);
LeftLegSkel.addBodyNode(node);
// ***** BodyNode 3: Left Hip Pitch (LHP) whose parent is: LHR *****
parent_node = LeftLegSkel.getBodyNode("LHR");
node = new BodyNode("LHP");
joint = create1DOFJoint(parent_node, node, 0.0, 0.0, DART_PI, DOF_ROLL);
joint->setName("LHP");
T = Eigen::Translation3d(0.0, 0.0, 1.0);
joint->setTransformFromParentBodyNode(T);
shape = new BoxShape(Vector3d(0.3, 0.3, 1.0));
shape->setOffset(Vector3d(0.0, 0.0, 0.5));
node->setLocalCOM(shape->getOffset());
node->setMass(mass);
Shape* shape1 = new EllipsoidShape(Vector3d(0.3, 0.3, 1.0));
shape1->setOffset(Vector3d(0.0, 0.0, 0.5));
node->addVisualizationShape(shape1);
node->addCollisionShape(shape);
LeftLegSkel.addBodyNode(node);
// Initialize the skeleton
LeftLegSkel.initDynamics();
// Window stuff
MyWindow window(&LeftLegSkel);
glutInit(&argc, argv);
window.initWindow(640, 480, "Skeleton example");
glutMainLoop();
return 0;
}
/* ********************************************************************************************* */
TEST(KINEMATICS, TRANS_AND_DERIV) {
using namespace Eigen;
using namespace kinematics;
FileInfoSkel<Skeleton> modelFile;
bool loadModelResult = modelFile.loadFile(DART_DATA_PATH"skel/SehoonVSK3.vsk");
ASSERT_TRUE(loadModelResult);
Skeleton* skel = modelFile.getSkel();
EXPECT_TRUE(skel != NULL);
/* LOG(INFO) << "# Dofs = " << skel->getNumDofs(); */
/* LOG(INFO) << "# Nodes = " << skel->getNumNodes(); */
FileInfoDof dofFile(skel);
bool loadDofResult = dofFile.loadFile(DART_DATA_PATH"dof/init_Tpose.dof");
ASSERT_TRUE(loadDofResult);
/* LOG(INFO) << "# frames = " << dofFile.getNumFrames(); */
VectorXd pose = dofFile.getPoseAtFrame(0);
skel->setPose(pose, true, true);
const int nodeIndex = 1;
BodyNode* node = skel->getNode(nodeIndex);
EXPECT_TRUE(node != NULL);
const double TOLERANCE = 0.000001;
// Check the one global transform matrix
const Matrix4d& W = skel->getNode(20)->getWorldTransform();
Matrix4d W_truth;
W_truth << -0.110662, 0.991044, 0.074734, 0.642841
, -0.987564, -0.118099, 0.103775, 0.327496
, 0.111672, -0.0623207, 0.991789, -0.12855
, 0, 0, 0, 1;
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
EXPECT_NEAR(W(i, j), W_truth(i, j), TOLERANCE);
}
}
// Check the derivative of one global transform matrix
// const Matrix4d& Wq = skel->getNode(10)->mWq.at(4);
const Matrix4d& Wq = skel->getNode(10)->getDerivWorldTransform(4);
Matrix4d Wq_truth;
Wq_truth << 0.121382, 0.413015, 0.902378, 0.161838
,-0.0175714, 0.00698451, 0.0149899, 0.00571836
,-0.992336, 0.0556535, 0.107702, 0.175059
,0, 0, 0, 0;
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
EXPECT_NEAR(Wq(i, j), Wq_truth(i, j), TOLERANCE);
}
}
// Check Jacobians
BodyNode *nodecheck = skel->getNode(23);
// Linear Jacobian
const MatrixXd Jv = nodecheck->getJacobianLinear();
MatrixXd Jv_truth = MatrixXd::Zero(Jv.rows(), Jv.cols());
Jv_truth<<1, 0, 0, 0.013694, -0.0194371, -0.422612, 0.0157281, -0.00961473, -0.421925, 0.0026645, -0.0048767, -0.179914, -0.0013539, -0.00132969, -0.00885535,
0, 1, 0, 0.0321939, 0.00226492, -0.135736, 0.0330525, 0.00462613, -0.135448, 0.0181491, 0.00758086, -0.122187, 0.00532694, 0.0049221, -0.128917,
0, 0, 1, 0.423948, 0.130694, 0.0166546, 0.42642, 0.118176, 0.0221309, 0.180296, 0.120584, 0.0105059, 0.0838259, 0.081304, 0.00719314;
for (int i = 0; i < Jv.rows(); i++) {
for (int j = 0; j < Jv.cols(); j++) {
EXPECT_NEAR(Jv(i, j), Jv_truth(i, j), TOLERANCE);
}
}
// Angular Jacobian
const MatrixXd Jwbody = nodecheck->getWorldTransform().topLeftCorner<3,3>().transpose()
* nodecheck->getJacobianAngular();
MatrixXd Jwbody_truth = MatrixXd::Zero(Jwbody.rows(), Jwbody.cols());
Jwbody_truth << 0, 0, 0, 0.0818662, 0.996527, 0.000504051, 0.110263, 0.993552, 0.0249018, 0.0772274, 0.995683, 0.0423836, 0.648386, 0.628838, 0.0338389,
0, 0, 0, -0.995079, 0.082001, -0.0518665, -0.992054, 0.111479, -0.0554717, -0.996033, 0.078601, -0.0313861, -0.629201, 0.649145, -0.00994729,
0, 0, 0, -0.0518265, 0.00391001, 0.998406, -0.0579139, -0.0187244, 0.998021, -0.0343359, -0.0398718, 0.998564, -0.0262454, -0.0235626, 0.999159;
for (int i = 0; i < Jwbody.rows(); i++) {
for (int j = 0; j < Jwbody.cols(); j++) {
EXPECT_NEAR(Jwbody(i, j), Jwbody_truth(i, j), TOLERANCE);
}
}
}
TEST(Skeleton, Restructuring)
{
std::vector<SkeletonPtr> skeletons = getSkeletons();
#ifndef NDEBUG
size_t numIterations = 10;
#else
size_t numIterations = 2*skeletons.size();
#endif
// Test moves within the current Skeleton
for(size_t i=0; i<numIterations; ++i)
{
size_t index = floor(math::random(0, skeletons.size()));
index = std::min(index, skeletons.size()-1);
SkeletonPtr skeleton = skeletons[index];
SkeletonPtr original = skeleton->clone();
size_t maxNode = skeleton->getNumBodyNodes()-1;
BodyNode* bn1 = skeleton->getBodyNode(floor(math::random(0, maxNode)));
BodyNode* bn2 = skeleton->getBodyNode(ceil(math::random(0, maxNode)));
if(bn1 == bn2)
{
--i;
continue;
}
BodyNode* child = bn1->descendsFrom(bn2)? bn1 : bn2;
BodyNode* parent = child == bn1? bn2 : bn1;
child->moveTo(parent);
EXPECT_TRUE(skeleton->getNumBodyNodes() == original->getNumBodyNodes());
if(skeleton->getNumBodyNodes() == original->getNumBodyNodes())
{
for(size_t j=0; j<skeleton->getNumBodyNodes(); ++j)
{
// Make sure no BodyNodes have been lost or gained in translation
std::string name = original->getBodyNode(j)->getName();
BodyNode* bn = skeleton->getBodyNode(name);
EXPECT_FALSE(bn == nullptr);
if(bn)
EXPECT_TRUE(bn->getName() == name);
name = skeleton->getBodyNode(j)->getName();
bn = original->getBodyNode(name);
EXPECT_FALSE(bn == nullptr);
if(bn)
EXPECT_TRUE(bn->getName() == name);
// Make sure no Joints have been lost or gained in translation
name = original->getJoint(j)->getName();
Joint* joint = skeleton->getJoint(name);
EXPECT_FALSE(joint == nullptr);
if(joint)
EXPECT_TRUE(joint->getName() == name);
name = skeleton->getJoint(j)->getName();
joint = original->getJoint(name);
EXPECT_FALSE(joint == nullptr);
if(joint)
EXPECT_TRUE(joint->getName() == name);
}
}
EXPECT_TRUE(skeleton->getNumDofs() == original->getNumDofs());
for(size_t j=0; j<skeleton->getNumDofs(); ++j)
{
std::string name = original->getDof(j)->getName();
DegreeOfFreedom* dof = skeleton->getDof(name);
EXPECT_FALSE(dof == nullptr);
if(dof)
EXPECT_TRUE(dof->getName() == name);
name = skeleton->getDof(j)->getName();
dof = original->getDof(name);
EXPECT_FALSE(dof == nullptr);
if(dof)
EXPECT_TRUE(dof->getName() == name);
}
}
// Test moves between Skeletons
for(size_t i=0; i<numIterations; ++i)
{
size_t fromIndex = floor(math::random(0, skeletons.size()));
fromIndex = std::min(fromIndex, skeletons.size()-1);
SkeletonPtr fromSkel = skeletons[fromIndex];
if(fromSkel->getNumBodyNodes() == 0)
{
--i;
continue;
}
size_t toIndex = floor(math::random(0, skeletons.size()));
toIndex = std::min(toIndex, skeletons.size()-1);
SkeletonPtr toSkel = skeletons[toIndex];
if(toSkel->getNumBodyNodes() == 0)
{
--i;
continue;
}
BodyNode* childBn = fromSkel->getBodyNode(
floor(math::random(0, fromSkel->getNumBodyNodes()-1)));
BodyNode* parentBn = toSkel->getBodyNode(
floor(math::random(0, toSkel->getNumBodyNodes()-1)));
if(fromSkel == toSkel)
{
if(childBn == parentBn)
{
--i;
continue;
}
if(parentBn->descendsFrom(childBn))
{
BodyNode* tempBn = childBn;
childBn = parentBn;
parentBn = tempBn;
SkeletonPtr tempSkel = fromSkel;
fromSkel = toSkel;
toSkel = tempSkel;
}
}
BodyNode* originalParent = childBn->getParentBodyNode();
std::vector<BodyNode*> subtree;
constructSubtree(subtree, childBn);
// Move to a new Skeleton
childBn->moveTo(parentBn);
// Make sure all the objects have moved
for(size_t j=0; j<subtree.size(); ++j)
{
BodyNode* bn = subtree[j];
EXPECT_TRUE(bn->getSkeleton() == toSkel);
}
// Move to the Skeleton's root while producing a new Joint type
sub_ptr<Joint> originalJoint = childBn->getParentJoint();
childBn->moveTo<FreeJoint>(nullptr);
// The original parent joint should be deleted now
EXPECT_TRUE(originalJoint == nullptr);
// The BodyNode should now be a root node
EXPECT_TRUE(childBn->getParentBodyNode() == nullptr);
// The subtree should still be in the same Skeleton
for(size_t j=0; j<subtree.size(); ++j)
{
BodyNode* bn = subtree[j];
EXPECT_TRUE(bn->getSkeleton() == toSkel);
}
// Create some new Skeletons and mangle them all up
childBn->copyTo<RevoluteJoint>(fromSkel, originalParent);
SkeletonPtr temporary = childBn->split("temporary");
SkeletonPtr other_temporary =
childBn->split<PrismaticJoint>("other temporary");
SkeletonPtr another_temporary = childBn->copyAs("another temporary");
SkeletonPtr last_temporary = childBn->copyAs<ScrewJoint>("last temporary");
childBn->copyTo(another_temporary->getBodyNode(
another_temporary->getNumBodyNodes()-1));
childBn->copyTo<PlanarJoint>(another_temporary->getBodyNode(0));
childBn->copyTo<TranslationalJoint>(temporary, nullptr);
childBn->moveTo(last_temporary,
last_temporary->getBodyNode(last_temporary->getNumBodyNodes()-1));
childBn->moveTo<BallJoint>(last_temporary, nullptr);
childBn->moveTo<EulerJoint>(last_temporary,
last_temporary->getBodyNode(0));
childBn->changeParentJointType<FreeJoint>();
// Test the non-recursive copying
if(toSkel->getNumBodyNodes() > 1)
{
SkeletonPtr singleBodyNode =
toSkel->getBodyNode(0)->copyAs("single", false);
EXPECT_TRUE(singleBodyNode->getNumBodyNodes() == 1);
std::pair<Joint*, BodyNode*> singlePair =
toSkel->getBodyNode(0)->copyTo(nullptr, false);
EXPECT_TRUE(singlePair.second->getNumChildBodyNodes() == 0);
}
// Check that the mangled Skeletons are all self-consistent
check_self_consistency(fromSkel);
check_self_consistency(toSkel);
check_self_consistency(temporary);
check_self_consistency(other_temporary);
check_self_consistency(another_temporary);
check_self_consistency(last_temporary);
}
}
// Current code only works for the left leg with only one constraint
VectorXd MyWorld::updateGradients() {
// compute c(q)
mC = mSkel->getMarker(mConstrainedMarker)->getWorldPosition() - mTarget;
// compute J(q)
Vector4d offset;
offset << mSkel->getMarker(mConstrainedMarker)->getLocalPosition(), 1; // Create a vector in homogeneous coordinates
// w.r.t ankle dofs
BodyNode *node = mSkel->getMarker(mConstrainedMarker)->getBodyNode();
Joint *joint = node->getParentJoint();
Matrix4d worldToParent = node->getParentBodyNode()->getTransform().matrix();
Matrix4d parentToJoint = joint->getTransformFromParentBodyNode().matrix();
Matrix4d dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
Matrix4d R = joint->getTransform(1).matrix();
Matrix4d jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
Vector4d jCol = worldToParent * parentToJoint * dR * R * jointToChild * offset;
int colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol
dR = joint->getTransformDerivative(1);
R = joint->getTransform(0).matrix();
jCol = worldToParent * parentToJoint * R * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(1);
mJ.col(colIndex) = jCol.head(3);
offset = parentToJoint * joint->getTransform(0).matrix() * joint->getTransform(1).matrix() * jointToChild * offset; // Update offset so it stores the chain below the parent joint
// w.r.t knee dof
node = node->getParentBodyNode(); // return NULL if node is the root node
joint = node->getParentJoint();
worldToParent = node->getParentBodyNode()->getTransform().matrix();
parentToJoint = joint->getTransformFromParentBodyNode().matrix();
dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
jCol = worldToParent * parentToJoint * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol
offset = parentToJoint * joint->getTransform(0).matrix() * jointToChild * offset;
// w.r.t hip dofs
node = node->getParentBodyNode();
joint = node->getParentJoint();
worldToParent = node->getParentBodyNode()->getTransform().matrix();
parentToJoint = joint->getTransformFromParentBodyNode().matrix();
dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
Matrix4d R1 = joint->getTransform(1).matrix();
Matrix4d R2 = joint->getTransform(2).matrix();
jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
jCol = worldToParent * parentToJoint * dR * R1 * R2 * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of J
R1 = joint->getTransform(0).matrix();
dR = joint->getTransformDerivative(1);
R2 = joint->getTransform(2).matrix();
jCol = worldToParent * parentToJoint * R1 * dR * R2 * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(1);
mJ.col(colIndex) = jCol.head(3);
R1 = joint->getTransform(0).matrix();
R2 = joint->getTransform(1).matrix();
dR = joint->getTransformDerivative(2);
jCol = worldToParent * parentToJoint * R1 * R2 * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(2);
mJ.col(colIndex) = jCol.head(3);
// compute gradients
VectorXd gradients = 2 * mJ.transpose() * mC;
return gradients;
}
// Current code only works for the left leg with only one constraint
VectorXd MyWorld::updateGradients() {
mJ.setZero();
mC.setZero();
// compute c(q)
//std::cout << "HAMYDEBUG: mConstrainedMarker = " << getMarker(mConstrainedMarker) << std::endl;
mC = getMarker(mConstrainedMarker)->getWorldPosition() - mTarget;
std::cout << "C(q) = " << mC << std::endl;
// compute J(q)
Vector4d offset;
offset << getMarker(mConstrainedMarker)->getLocalPosition(), 1; // Create a vector in homogeneous coordinates
//Setup vars
BodyNode *node = getMarker(mConstrainedMarker)->getBodyNode();
Joint *joint = node->getParentJoint();
Matrix4d worldToParent;
Matrix4d parentToJoint;
//Declare Vars
Matrix4d dR; // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
Matrix4d R;
Matrix4d R1;
Matrix4d R2;
Matrix4d jointToChild;
Vector4d jCol;
int colIndex;
//TODO: Might want to change this to check if root using given root fcn
//Iterate until we get to the root node
while(true) {
//std::cout << "HAMY DEBUG: Beginning new looop" << std::endl;
if(node->getParentBodyNode() == NULL) {
worldToParent = worldToParent.setIdentity();
} else {
worldToParent = node->getParentBodyNode()->getTransform().matrix();
}
parentToJoint = joint->getTransformFromParentBodyNode().matrix();
// Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
//TODO: R1, R2, ... Rn code depending on DOFs
int nDofs = joint->getNumDofs();
//std::cout << "HAMY: nDofs=" << nDofs << std::endl;
//Can only have up to 3 DOFs on any one piece
switch(nDofs){
case 1: //std::cout << "HAMY: 1 nDOF" << std::endl;
dR = joint->getTransformDerivative(0);
jCol = worldToParent * parentToJoint * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol
offset = parentToJoint * joint->getTransform(0).matrix() * jointToChild * offset;
break;
case 2: //std::cout << "HAMY: 2 nDOF" << std::endl;
dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
R = joint->getTransform(1).matrix();
jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
jCol = worldToParent * parentToJoint * dR * R * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol
dR = joint->getTransformDerivative(1);
R = joint->getTransform(0).matrix();
jCol = worldToParent * parentToJoint * R * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(1);
mJ.col(colIndex) = jCol.head(3);
offset = parentToJoint * joint->getTransform(0).matrix() * joint->getTransform(1).matrix() * jointToChild * offset; // Upd
break;
case 3: //std::cout << "HAMY: 3 nDOF" << std::endl;
dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
R1 = joint->getTransform(1).matrix();
R2 = joint->getTransform(2).matrix();
jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
jCol = worldToParent * parentToJoint * dR * R1 * R2 * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of J
R1 = joint->getTransform(0).matrix();
dR = joint->getTransformDerivative(1);
R2 = joint->getTransform(2).matrix();
jCol = worldToParent * parentToJoint * R1 * dR * R2 * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(1);
mJ.col(colIndex) = jCol.head(3);
R1 = joint->getTransform(0).matrix();
R2 = joint->getTransform(1).matrix();
dR = joint->getTransformDerivative(2);
jCol = worldToParent * parentToJoint * R1 * R2 * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(2);
mJ.col(colIndex) = jCol.head(3);
offset = parentToJoint * joint->getTransform(0).matrix() * joint->getTransform(1).matrix() * joint->getTransform(2).matrix() * jointToChild * offset;
break;
default: //std::cout << "HAMY: Unexpected nDOF = " << nDofs << std::endl;
break;
}
if(node != mSkel->getRootBodyNode()) {
//std::cout << "HAMY DEBUG: Not root, continue loop" << std::endl;
node = node->getParentBodyNode(); // return NULL if node is the root node
joint = node->getParentJoint();
} else {
break;
}
}
// compute gradients
VectorXd gradients = 2 * mJ.transpose() * mC;
return gradients;
}
void MyWorld::createMarkers() {
Vector3d offset(0.2, 0.0, 0.0);
BodyNode* bNode = mSkel->getBodyNode("h_heel_right");
Marker* m = new Marker("right_foot", offset, bNode);
mMarkers.push_back(m);
bNode->addMarker(m);
offset = Vector3d(0.2, 0.0, 0.0);
bNode = mSkel->getBodyNode("h_heel_left");
m = new Marker("left_foot", offset, bNode);
mMarkers.push_back(m);
bNode->addMarker(m);
offset = Vector3d(0.065, -0.3, 0.0);
bNode = mSkel->getBodyNode("h_thigh_right");
m = new Marker("right_thigh", offset, bNode);
mMarkers.push_back(m);
bNode->addMarker(m);
offset = Vector3d(0.065, -0.3, 0.0);
bNode = mSkel->getBodyNode("h_thigh_left");
m = new Marker("left_thigh", offset, bNode);
mMarkers.push_back(m);
bNode->addMarker(m);
offset = Vector3d(0.0, 0.0, 0.13);
bNode = mSkel->getBodyNode("h_pelvis");
m = new Marker("pelvis_right", offset, bNode);
mMarkers.push_back(m);
bNode->addMarker(m);
offset = Vector3d(0.0, 0.0, -0.13);
bNode = mSkel->getBodyNode("h_pelvis");
m = new Marker("pelvis_left", offset, bNode);
mMarkers.push_back(m);
bNode->addMarker(m);
offset = Vector3d(0.075, 0.1, 0.0);
bNode = mSkel->getBodyNode("h_abdomen");
m = new Marker("abdomen", offset, bNode);
mMarkers.push_back(m);
bNode->addMarker(m);
offset = Vector3d(0.0, 0.18, 0.075);
bNode = mSkel->getBodyNode("h_head");
m = new Marker("head_right", offset, bNode);
mMarkers.push_back(m);
bNode->addMarker(m);
offset = Vector3d(0.0, 0.18, -0.075);
bNode = mSkel->getBodyNode("h_head");
m = new Marker("head_left", offset, bNode);
mMarkers.push_back(m);
bNode->addMarker(m);
offset = Vector3d(0.0, 0.22, 0.0);
bNode = mSkel->getBodyNode("h_scapula_right");
m = new Marker("right_scapula", offset, bNode);
mMarkers.push_back(m);
bNode->addMarker(m);
offset = Vector3d(0.0, 0.22, 0.0);
bNode = mSkel->getBodyNode("h_scapula_left");
m = new Marker("left_scapula", offset, bNode);
mMarkers.push_back(m);
bNode->addMarker(m);
offset = Vector3d(0.0, -0.2, 0.05);
bNode = mSkel->getBodyNode("h_bicep_right");
m = new Marker("right_bicep", offset, bNode);
mMarkers.push_back(m);
bNode->addMarker(m);
offset = Vector3d(0.0, -0.2, -0.05);
bNode = mSkel->getBodyNode("h_bicep_left");
m = new Marker("left_bicep", offset, bNode);
mMarkers.push_back(m);
bNode->addMarker(m);
offset = Vector3d(0.0, -0.1, 0.025);
bNode = mSkel->getBodyNode("h_hand_right");
m = new Marker("right_hand", offset, bNode);
mMarkers.push_back(m);
bNode->addMarker(m);
offset = Vector3d(0.0, -0.1, -0.025);
bNode = mSkel->getBodyNode("h_hand_left");
m = new Marker("left_hand", offset, bNode);
mMarkers.push_back(m);
bNode->addMarker(m);
}
//==============================================================================
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;
}
int main(int argc, char* argv[]) {
using dart::dynamics::BodyNode;
using dart::dynamics::FreeJoint;
using dart::dynamics::MeshShape;
using dart::dynamics::Skeleton;
using dart::simulation::World;
using dart::utils::SkelParser;
// Create and initialize the world
World* myWorld = dart::utils::SkelParser::readSkelFile(
DART_DATA_PATH"/skel/mesh_collision.skel");
// Create a skeleton
Skeleton* MeshSkel = new Skeleton("Mesh Skeleton");
// Always set the root node ( 6DOF for rotation and translation )
FreeJoint* joint;
BodyNode* node;
// Set the initial Rootnode that controls the position and orientation of the
// whole robot
node = new BodyNode("rootBodyNode");
joint = new FreeJoint("rootJoint");
// Add joint to the body node
node->setParentJoint(joint);
// Load a Mesh3DTriangle to save in Shape
const aiScene* m3d = MeshShape::loadMesh(DART_DATA_PATH"/obj/foot.obj");
// Create Shape and assign it to node
MeshShape* Shape0 = new MeshShape(Eigen::Vector3d(1.0, 1.0, 1.0), m3d);
node->addVisualizationShape(Shape0);
node->addCollisionShape(Shape0);
node->setInertia(0.000416667, 0.000416667, 0.000416667);
node->setMass(1.0); // 1 Kg according to cube1.skel
// Add node to Skel
MeshSkel->addBodyNode(node);
// Add MeshSkel to the world
myWorld->addSkeleton(MeshSkel);
// Verify that our skeleton has something inside :)
std::printf("Our skeleton has %d nodes \n", MeshSkel->getNumBodyNodes());
// std::printf("Our skeleton has %d joints \n", MeshSkel->getNumJoints());
std::printf("Our skeleton has %d DOFs \n", MeshSkel->getNumGenCoords());
MyWindow window;
window.setWorld(myWorld);
std::cout << "space bar: simulation on/off" << std::endl;
std::cout << "'s': simulate one step" << std::endl;
std::cout << "'p': playback/stop" << std::endl;
std::cout << "'[' and ']': play one frame backward and forward" << std::endl;
std::cout << "'v': visualization on/off" << std::endl;
std::cout << "'1' and '2': programmed interaction" << std::endl;
glutInit(&argc, argv);
window.initWindow(640, 480, "meshCollision");
glutMainLoop();
aiReleaseImport(m3d);
return 0;
}
//==============================================================================
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;
}
//==============================================================================
TEST_F(JOINTS, CONVENIENCE_FUNCTIONS)
{
// -- set up the root BodyNode
BodyNode* root = new BodyNode("root");
WeldJoint* rootjoint = new WeldJoint("base");
root->setParentJoint(rootjoint);
// -- set up the FreeJoint
BodyNode* freejoint_bn = new BodyNode("freejoint_bn");
FreeJoint* freejoint = new FreeJoint("freejoint");
freejoint_bn->setParentJoint(freejoint);
root->addChildBodyNode(freejoint_bn);
freejoint->setTransformFromParentBodyNode(random_transform());
freejoint->setTransformFromChildBodyNode(random_transform());
// -- set up the EulerJoint
BodyNode* eulerjoint_bn = new BodyNode("eulerjoint_bn");
EulerJoint* eulerjoint = new EulerJoint("eulerjoint");
eulerjoint_bn->setParentJoint(eulerjoint);
root->addChildBodyNode(eulerjoint_bn);
eulerjoint->setTransformFromParentBodyNode(random_transform());
eulerjoint->setTransformFromChildBodyNode(random_transform());
// -- set up the BallJoint
BodyNode* balljoint_bn = new BodyNode("balljoint_bn");
BallJoint* balljoint = new BallJoint("balljoint");
balljoint_bn->setParentJoint(balljoint);
root->addChildBodyNode(balljoint_bn);
balljoint->setTransformFromParentBodyNode(random_transform());
balljoint->setTransformFromChildBodyNode(random_transform());
// -- set up Skeleton and compute forward kinematics
Skeleton* skel = new Skeleton;
skel->addBodyNode(root);
skel->addBodyNode(freejoint_bn);
skel->addBodyNode(eulerjoint_bn);
skel->addBodyNode(balljoint_bn);
skel->init();
// Test a hundred times
for(size_t n=0; n<100; ++n)
{
// -- convert transforms to positions and then positions back to transforms
Eigen::Isometry3d desired_freejoint_tf = random_transform();
freejoint->setPositions(FreeJoint::convertToPositions(desired_freejoint_tf));
Eigen::Isometry3d actual_freejoint_tf = FreeJoint::convertToTransform(
freejoint->getPositions());
Eigen::Isometry3d desired_eulerjoint_tf = random_transform();
desired_eulerjoint_tf.translation() = Eigen::Vector3d::Zero();
eulerjoint->setPositions(
eulerjoint->convertToPositions(desired_eulerjoint_tf.linear()));
Eigen::Isometry3d actual_eulerjoint_tf = eulerjoint->convertToTransform(
eulerjoint->getPositions());
Eigen::Isometry3d desired_balljoint_tf = random_transform();
desired_balljoint_tf.translation() = Eigen::Vector3d::Zero();
balljoint->setPositions(
BallJoint::convertToPositions(desired_balljoint_tf.linear()));
Eigen::Isometry3d actual_balljoint_tf = BallJoint::convertToTransform(
balljoint->getPositions());
skel->computeForwardKinematics(true, false, false);
// -- collect everything so we can cycle through the tests
std::vector<Joint*> joints;
std::vector<BodyNode*> bns;
std::vector<Eigen::Isometry3d> desired_tfs;
std::vector<Eigen::Isometry3d> actual_tfs;
joints.push_back(freejoint);
bns.push_back(freejoint_bn);
desired_tfs.push_back(desired_freejoint_tf);
actual_tfs.push_back(actual_freejoint_tf);
joints.push_back(eulerjoint);
bns.push_back(eulerjoint_bn);
desired_tfs.push_back(desired_eulerjoint_tf);
actual_tfs.push_back(actual_eulerjoint_tf);
joints.push_back(balljoint);
bns.push_back(balljoint_bn);
desired_tfs.push_back(desired_balljoint_tf);
actual_tfs.push_back(actual_balljoint_tf);
for(size_t i=0; i<joints.size(); ++i)
{
Joint* joint = joints[i];
BodyNode* bn = bns[i];
Eigen::Isometry3d tf = desired_tfs[i];
bool check_transform_conversion =
equals(predict_joint_transform(joint, tf).matrix(),
get_relative_transform(bn, bn->getParentBodyNode()).matrix());
EXPECT_TRUE(check_transform_conversion);
if(!check_transform_conversion)
{
std::cout << "[" << joint->getName() << " Failed]\n";
std::cout << "Predicted:\n" << predict_joint_transform(joint, tf).matrix()
<< "\n\nActual:\n"
<< get_relative_transform(bn, bn->getParentBodyNode()).matrix()
<< "\n\n";
}
bool check_full_cycle = equals(desired_tfs[i].matrix(),
actual_tfs[i].matrix());
EXPECT_TRUE(check_full_cycle);
if(!check_full_cycle)
{
std::cout << "[" << joint->getName() << " Failed]\n";
std::cout << "Desired:\n" << desired_tfs[i].matrix()
<< "\n\nActual:\n" << actual_tfs[i].matrix()
<< "\n\n";
}
}
}
}
