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

Skeleton * BVHParser::createSkeleton()
{
	Skeleton * s = new Skeleton();
	
	// set default pose...
	Joint * b = createJoint(_root);
	if( !s->setJoints(b) )
	{
		delete s;
		return 0;
	}
	
	Pose * pose = new Pose(s->getNumJoints());
	
	for(int i = 0; i < _linearNodes.size(); i++ )
	{
		BVHNode * n = _linearNodes[i];
		pose->transforms[i].rotation.identity();
		pose->transforms[i].position(0,0,0);
	}
	
	s->pose = pose;
    s->init();
    
	return s;
}

Skeleton* Mapper::newSkeleton(string id) {
	Skeleton skeleton;
	skeleton.init(id);
	skeleton.setSmoothing(defaultSmoothing);
	skeletons->push_back(skeleton);
	return &skeletons->at(skeletons->size()-1);
}

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

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

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