C++ Skeleton::getBodyNode方法代码示例

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

//==============================================================================
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
//.........这里部分代码省略.........

//==============================================================================
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;
//.........这里部分代码省略.........

//==============================================================================
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;
//.........这里部分代码省略.........

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