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

void WorldConstructor::commonConstruction(World* world)
{
	DecoConfig::GetSingleton()->GetDouble("Sim", "TimeStep", msTimeStep);
	world->setTimeStep(msTimeStep);
	// Set gravity of the world
	world->setGravity(Eigen::Vector3d(0.0, -9.81, 0));
	dart::constraint::ContactConstraint::setErrorReductionParameter(0.0);
	dart::constraint::ContactConstraint::setMaxErrorReductionVelocity(0.1);

	// // Load ground and Atlas robot and add them to the world
	DartLoader urdfLoader;
	Skeleton* ground = urdfLoader.parseSkeleton(
		DATA_DIR"/sdf/ground.urdf");
	Skeleton* robot 
		= urdfLoader.parseSkeleton(
		DATA_DIR"/urdf/BioloidGP/BioloidGP.URDF");

	int isHybridDynamics = 0;
	DecoConfig::GetSingleton()->GetInt("Sim", "HybridDynamics", isHybridDynamics);
	if (isHybridDynamics)
	{
		dart::dynamics::Joint* joint0 = robot->getJoint(0);
		joint0->setActuatorType(dart::dynamics::Joint::PASSIVE);
		for (size_t i = 1; i < robot->getNumBodyNodes(); ++i)
		{
			dart::dynamics::Joint* joint = robot->getJoint(i);
			joint->setActuatorType(dart::dynamics::Joint::VELOCITY);
		}

	}
	

	robot->enableSelfCollision();

	world->addSkeleton(robot);
	world->addSkeleton(ground);

	// Create a humanoid controller
	msHumanoid = new bioloidgp::robot::HumanoidController(robot, world->getConstraintSolver());


}

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

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