예제 #1
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;
}
예제 #2
0
//==============================================================================
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;
}
예제 #3
0
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;
}
예제 #4
0
//==============================================================================
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;
}
예제 #5
0
//==============================================================================
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;
}
예제 #6
0
//==============================================================================
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));
  }
}
예제 #7
0
BodyNode* addBodyNode(BodyNode* bn, const std::string& name)
{
  BodyNode* result = bn->createChildJointAndBodyNodePair<JointType>().second;
  result->setName(name);
  return result;
}
예제 #8
0
파일: Main.cpp 프로젝트: sonyomega/dart
/* ********************************************************************************************* */
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;
}
예제 #9
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);
        }
    }
}
예제 #10
0
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);
  }
}
예제 #11
0
// 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;
}
예제 #12
0
// 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;
}
예제 #13
0
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);
}
예제 #14
0
//==============================================================================
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;
}
예제 #15
0
파일: Main.cpp 프로젝트: amehlberg17/dart
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;
}
예제 #16
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;
}
예제 #17
0
//==============================================================================
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";
      }
    }
  }
}