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

int main(int argc, char* argv[])
{
    // Create empty soft world
    World* myWorld = new World;

    // Load ground and Atlas robot and add them to the world
    DartLoader urdfLoader;
    Skeleton* ground = urdfLoader.parseSkeleton(
                           DART_DATA_PATH"sdf/atlas/ground.urdf");
//  Skeleton* atlas = SoftSdfParser::readSkeleton(
//        DART_DATA_PATH"sdf/atlas/atlas_v3_no_head.sdf");
    Skeleton* atlas
        = SoftSdfParser::readSkeleton(
              DART_DATA_PATH"sdf/atlas/atlas_v3_no_head_soft_feet.sdf");
    myWorld->addSkeleton(atlas);
    myWorld->addSkeleton(ground);

    // Set initial configuration for Atlas robot
    VectorXd q = atlas->getConfigs();
    q[0] = -0.5 * DART_PI;
    atlas->setConfigs(q, true, true, false);

    // Set gravity of the world
    myWorld->setGravity(Vector3d(0.0, -9.81, 0.0));

    // Create a window and link it to the world
    MyWindow window(new Controller(atlas, myWorld->getConstraintSolver()));
    window.setWorld(myWorld);

    // Print manual
    cout << "space bar: simulation on/off" << endl;
    cout << "'p': playback/stop" << endl;
    cout << "'[' and ']': play one frame backward and forward" << endl;
    cout << "'v': visualization on/off" << endl;
    cout << endl;
    cout << "'h': harness pelvis on/off" << endl;
    cout << "'j': harness left foot on/off" << endl;
    cout << "'k': harness right foot on/off" << endl;
    cout << "'r': reset robot" << endl;
    cout << "'n': transite to the next state manually" << endl;
    cout << endl;
    cout << "'1': standing controller" << endl;
    cout << "'2': walking controller" << endl;

    // Run glut loop
    glutInit(&argc, argv);
    window.initWindow(640, 480, "Atlas Robot");
    glutMainLoop();

    return 0;
}

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

//.........这里部分代码省略.........

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