// TODO: Use link lengths in expectations explicitly
TEST(FORWARD_KINEMATICS, TWO_ROLLS) {
// Create the world
const double link1 = 1.5, link2 = 1.0;
Skeleton* robot = createTwoLinkRobot(Vector3d(0.3, 0.3, link1), DOF_ROLL, Vector3d(0.3, 0.3, link2),
DOF_ROLL);
// Set the test cases with the joint values and the expected end-effector positions
const size_t numTests = 2;
Vector2d joints [numTests] = {Vector2d(0.0, DART_PI/2.0), Vector2d(3*DART_PI/4.0, -DART_PI/4.0)};
Vector3d expectedPos [numTests] = {Vector3d(0.0, -1.0, 1.5), Vector3d(0.0, -2.06, -1.06)};
// Check each case by setting the joint values and obtaining the end-effector position
for (size_t i = 0; i < numTests; i++) {
robot->setConfig(twoLinkIndices, joints[i]);
Vector3d actual = robot->getBodyNode("ee")->getWorldTransform().translation();
bool equality = equals(actual, expectedPos[i], 1e-3);
EXPECT_TRUE(equality);
if(!equality) {
std::cout << "Joint values: " << joints[i].transpose() << std::endl;
std::cout << "Actual pos: " << actual.transpose() << std::endl;
std::cout << "Expected pos: " << expectedPos[i].transpose() << std::endl;
}
}
}
//==============================================================================
TEST(InverseKinematics, FittingVelocity)
{
const double TOLERANCE = 1e-4;
#ifdef BUILD_TYPE_RELEASE
const size_t numRandomTests = 100;
#else
const size_t numRandomTests = 10;
#endif
// Create two link robot
const double l1 = 1.5;
const double l2 = 1.0;
Skeleton* robot = createFreeFloatingTwoLinkRobot(
Vector3d(0.3, 0.3, l1),
Vector3d(0.3, 0.3, l2), DOF_ROLL);
robot->init();
BodyNode* body1 = robot->getBodyNode(0);
// BodyNode* body2 = robot->getBodyNode(1);
Joint* joint1 = body1->getParentJoint();
// Joint* joint2 = body2->getParentJoint();
//------------------------- Free joint test ----------------------------------
// The parent joint of body1 is free joint so body1 should be able to
// transform to arbitrary tramsformation.
for (size_t i = 0; i < numRandomTests; ++i)
{
// Test for linear velocity
Vector3d desiredVel = Vector3d::Random();
body1->fitWorldLinearVel(desiredVel);
Vector3d fittedLinVel = body1->getWorldVelocity().tail<3>();
Vector3d fittedAngVel = body1->getWorldVelocity().head<3>();
Vector3d diff = fittedLinVel - desiredVel;
EXPECT_NEAR(diff.dot(diff), 0.0, TOLERANCE);
EXPECT_NEAR(fittedAngVel.dot(fittedAngVel), 0.0, TOLERANCE);
joint1->setGenVels(Vector6d::Zero(), true, true);
robot->setState(robot->getState(), true, true, false);
// Test for angular velocity
desiredVel = Vector3d::Random();
body1->fitWorldAngularVel(desiredVel);
fittedLinVel = body1->getWorldVelocity().tail<3>();
fittedAngVel = body1->getWorldVelocity().head<3>();
diff = fittedAngVel - desiredVel;
EXPECT_NEAR(fittedLinVel.dot(fittedLinVel), 0.0, TOLERANCE);
EXPECT_NEAR(diff.dot(diff), 0.0, TOLERANCE);
joint1->setGenVels(Vector6d::Zero(), true, true);
robot->setState(robot->getState(), true, true, false);
}
}
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;
}
//==============================================================================
TEST(InverseKinematics, FittingTransformation)
{
const double TOLERANCE = 1e-6;
#ifdef BUILD_TYPE_RELEASE
const size_t numRandomTests = 100;
#else
const size_t numRandomTests = 10;
#endif
// Create two link robot
const double l1 = 1.5;
const double l2 = 1.0;
Skeleton* robot = createFreeFloatingTwoLinkRobot(
Vector3d(0.3, 0.3, l1),
Vector3d(0.3, 0.3, l2), DOF_ROLL);
robot->init();
size_t dof = robot->getNumGenCoords();
VectorXd oldConfig = robot->getConfigs();
BodyNode* body1 = robot->getBodyNode(0);
BodyNode* body2 = robot->getBodyNode(1);
// Joint* joint1 = body1->getParentJoint();
Joint* joint2 = body2->getParentJoint();
//------------------------- Free joint test ----------------------------------
// The parent joint of body1 is free joint so body1 should be able to
// transform to arbitrary tramsformation.
for (size_t i = 0; i < numRandomTests; ++i)
{
// Get desiredT2 by transforming body1 to arbitrary transformation
Isometry3d desiredT1 = math::expMap(Vector6d::Random());
body1->fitWorldTransform(desiredT1);
// Check
Isometry3d newT1 = body1->getWorldTransform();
EXPECT_NEAR(math::logMap(newT1.inverse() * desiredT1).norm(),
0.0, TOLERANCE);
// Set to initial configuration
robot->setConfigs(oldConfig, true, false, false);
}
//----------------------- Revolute joint test ---------------------------------
// The parent joint of body2 is revolute joint so body2 can rotate along the
// axis of the revolute joint.
for (size_t i = 0; i < numRandomTests; ++i)
{
// Store the original transformation and joint angle
Isometry3d oldT2 = body2->getWorldTransform();
VectorXd oldQ2 = joint2->getConfigs();
// Get desiredT2 by rotating the revolute joint with random angle
joint2->setConfigs(VectorXd::Random(1), true, false, false);
Isometry3d desiredT2 = body2->getWorldTransform();
// Transform body2 to the original transofrmation and check if it is done
// well
joint2->setConfigs(oldQ2, true, false, false);
EXPECT_NEAR(
math::logMap(oldT2.inverse() * body2->getWorldTransform()).norm(),
0.0, TOLERANCE);
// Try to find optimal joint angle
body2->fitWorldTransform(desiredT2);
// Check
Isometry3d newT2 = body2->getWorldTransform();
EXPECT_NEAR(math::logMap(newT2.inverse() * desiredT2).norm(),
0.0, TOLERANCE);
}
//---------------- Revolute joint test with joint limit ----------------------
for (size_t i = 0; i < numRandomTests; ++i)
{
// Set joint limit
joint2->getGenCoord(0)->setConfigMin(DART_RADIAN * 0.0);
joint2->getGenCoord(0)->setConfigMax(DART_RADIAN * 15.0);
// Store the original transformation and joint angle
Isometry3d oldT2 = body2->getWorldTransform();
VectorXd oldQ2 = joint2->getConfigs();
// Get desiredT2 by rotating the revolute joint with random angle out of
// the joint limit range
joint2->getGenCoord(0)->setConfig(math::random(DART_RADIAN * 15.5, DART_PI));
robot->setConfigs(robot->getConfigs(), true, false, false);
Isometry3d desiredT2 = body2->getWorldTransform();
// Transform body2 to the original transofrmation and check if it is done
// well
joint2->setConfigs(oldQ2, true, false, false);
EXPECT_NEAR(
math::logMap(oldT2.inverse() * body2->getWorldTransform()).norm(),
0.0, TOLERANCE);
// Try to find optimal joint angle without joint limit constraint
body2->fitWorldTransform(desiredT2, BodyNode::IKP_PARENT_JOINT, false);
// Check if the optimal body2 transformation is reached to the desired one
Isometry3d newT2 = body2->getWorldTransform();
EXPECT_NEAR(math::logMap(newT2.inverse() * desiredT2).norm(),
0.0, TOLERANCE);
// Try to find optimal joint angle with joint limit constraint
body2->fitWorldTransform(desiredT2, BodyNode::IKP_PARENT_JOINT, true);
// Check if the optimal joint anlge is in the range
double newQ2 = joint2->getGenCoord(0)->getConfig();
EXPECT_GE(newQ2, DART_RADIAN * 0.0);
EXPECT_LE(newQ2, DART_RADIAN * 15.0);
}
}
//==============================================================================
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::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;
}
/* ********************************************************************************************* */
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;
}
