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

Skeleton* createFreeFloatingTwoLinkRobot(Vector3d dim1,
                                         Vector3d dim2, TypeOfDOF type2,
                                         bool finished = true)
{
  Skeleton* robot = new Skeleton();

  // Create the first link, the joint with the ground and its shape
  double mass = 1.0;
  BodyNode* node = new BodyNode("link1");
  Joint* joint = new FreeJoint();
  joint->setName("joint1");
  Shape* shape = new BoxShape(dim1);
  node->setLocalCOM(Vector3d(0.0, 0.0, dim1(2)/2.0));
  node->addVisualizationShape(shape);
  node->addCollisionShape(shape);
  node->setMass(mass);
  node->setParentJoint(joint);
  robot->addBodyNode(node);

  // Create the second link, the joint with link1 and its shape
  BodyNode* parent_node = robot->getBodyNode("link1");
  node = new BodyNode("link2");
  joint = create1DOFJoint(0.0, -DART_PI, DART_PI, type2);
  joint->setName("joint2");
  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
  T.translate(Eigen::Vector3d(0.0, 0.0, dim1(2)));
  joint->setTransformFromParentBodyNode(T);
  shape = new BoxShape(dim2);
  node->setLocalCOM(Vector3d(0.0, 0.0, dim2(2)/2.0));
  node->addVisualizationShape(shape);
  node->addCollisionShape(shape);
  node->setMass(mass);
  node->setParentJoint(joint);
  parent_node->addChildBodyNode(node);
  robot->addBodyNode(node);

  // If finished, initialize the skeleton
  if(finished)
  {
    addEndEffector(robot, node, dim2);
    robot->init();
  }
  return robot;
}

void CollisionInterface::addRigidBody(RigidBody *_rb, const std::string& name) {
    Skeleton *skel = new Skeleton();
    BodyNode *bn = new BodyNode();
    bn->setParentJoint( new dart::dynamics::FreeJoint("freeJoint") );
    bn->addCollisionShape(_rb->mShape);
    skel->addBodyNode( bn );
    skel->setName( name );
    skel->disableSelfCollision();
    skel->init();
    mCollisionChecker->addCollisionSkeletonNode(bn);
    mNodeMap[bn] = _rb;
}

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

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

/* ********************************************************************************************* */
int main(int argc, char* argv[]) {

    // Create Left Leg skeleton
    Skeleton LeftLegSkel;

    // Pointers to be used during the Skeleton building
    Matrix3d inertiaMatrix;
    inertiaMatrix << 0, 0, 0, 0, 0, 0, 0, 0, 0;
    double mass = 1.0;

    // ***** BodyNode 1: Left Hip Yaw (LHY) ***** *
    BodyNode* node = new BodyNode("LHY");
    Joint* joint = create1DOFJoint(NULL, node, 0.0, 0.0, DART_PI, DOF_YAW);
    joint->setName("LHY");
    Shape* shape = new BoxShape(Vector3d(0.3, 0.3, 1.0));
    node->addVisualizationShape(shape);
    node->addCollisionShape(shape);
    node->setMass(mass);
    LeftLegSkel.addBodyNode(node);

    // ***** BodyNode 2: Left Hip Roll (LHR) whose parent is: LHY *****\

    BodyNode* parent_node = LeftLegSkel.getBodyNode("LHY");
    node = new BodyNode("LHR");
    joint = create1DOFJoint(parent_node, node, 0.0, 0.0, DART_PI, DOF_ROLL);
    joint->setName("LHR");
    Eigen::Isometry3d T(Eigen::Translation3d(0.0, 0.0, 0.5));
    joint->setTransformFromParentBodyNode(T);
    shape = new BoxShape(Vector3d(0.3, 0.3, 1.0));
    shape->setOffset(Vector3d(0.0, 0.0, 0.5));
    node->setLocalCOM(shape->getOffset());
    node->setMass(mass);
    node->addVisualizationShape(shape);
    node->addCollisionShape(shape);
    LeftLegSkel.addBodyNode(node);

    // ***** BodyNode 3: Left Hip Pitch (LHP) whose parent is: LHR *****
    parent_node = LeftLegSkel.getBodyNode("LHR");
    node = new BodyNode("LHP");
    joint = create1DOFJoint(parent_node, node, 0.0, 0.0, DART_PI, DOF_ROLL);
    joint->setName("LHP");
    T = Eigen::Translation3d(0.0, 0.0, 1.0);
    joint->setTransformFromParentBodyNode(T);
    shape = new BoxShape(Vector3d(0.3, 0.3, 1.0));
    shape->setOffset(Vector3d(0.0, 0.0, 0.5));
    node->setLocalCOM(shape->getOffset());
    node->setMass(mass);
    Shape* shape1 = new EllipsoidShape(Vector3d(0.3, 0.3, 1.0));
    shape1->setOffset(Vector3d(0.0, 0.0, 0.5));
    node->addVisualizationShape(shape1);
    node->addCollisionShape(shape);
    LeftLegSkel.addBodyNode(node);

    // Initialize the skeleton
    LeftLegSkel.initDynamics();

    // Window stuff
    MyWindow window(&LeftLegSkel);
    glutInit(&argc, argv);
    window.initWindow(640, 480, "Skeleton example");
    glutMainLoop();

    return 0;
}

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