C++ MyWindow::initWindow方法代码示例

int main(int argc, char* argv[])
{
  // load a skeleton file
  // create and initialize the world
  dart::simulation::World* myWorld
      = dart::utils::SkelParser::readWorld(
          DART_DATA_PATH"skel/soft_open_chain.skel");
  assert(myWorld != NULL);

  int dof = myWorld->getSkeleton("skeleton 1")->getNumGenCoords();
  Eigen::VectorXd initPose = Eigen::VectorXd::Zero(dof);
  for (int i = 0; i < 3; i++)
    initPose[i] = dart::math::random(-0.5, 0.5);
  myWorld->getSkeleton("skeleton 1")->setConfigs(initPose, true, true, false);

  // create a window and link it to the world
  MyWindow window;
  window.setWorld(myWorld);

  std::cout << "space bar: simulation on/off" << 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'--'4': programmed interaction" << std::endl;

  glutInit(&argc, argv);
  window.initWindow(640, 480, "Soft Open Chain");
  glutMainLoop();

  return 0;
}

int main(int argc, char* argv[])
{
    // load a skeleton file
    // create and initialize the world
    dart::simulation::World* myWorld
            = dart::utils::readSkelFile(DART_DATA_PATH"/skel/chain.skel");
    assert(myWorld != NULL);
    
    // create and initialize the world
    Eigen::Vector3d gravity(0.0, -9.81, 0.0);
    myWorld->setGravity(gravity);
    myWorld->setTimeStep(1.0/2000);

    int dof =  myWorld->getSkeleton(0)->getDOF();
    Eigen::VectorXd initPose(dof);
    for (int i = 0; i < dof; i++)
        initPose[i] = random(-0.5, 0.5);
    myWorld->getSkeleton(0)->setPose(initPose);

    // create a window and link it to the world
    MyWindow window;
    window.setWorld(myWorld);
  
    glutInit(&argc, argv);
    window.initWindow(640, 480, "Forward Simulation");
    glutMainLoop();

    return 0;
}

int main(int argc, char* argv[]) {
  // create and initialize the world
  dart::simulation::World *myWorld
      = dart::utils::SkelParser::readWorld(
          DART_DATA_PATH"/skel/bullet_collision.skel");
  assert(myWorld != NULL);
  Eigen::Vector3d gravity(0.0, -9.81, 0.0);
  myWorld->setGravity(gravity);

  // create a window and link it to the world
  MyWindow window;
  window.setWorld(myWorld);

  std::cout << "space bar: simulation on/off" << 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'--'4': programmed interaction" << std::endl;
  std::cout << "'q': spawn a random cube" << std::endl;
  std::cout << "'w': delete a spawned cube" << std::endl;

  glutInit(&argc, argv);
  window.initWindow(640, 480, "Bullet Collision");
  glutMainLoop();

  return 0;
}

int main(int argc, char* argv[])
{
  // load a skeleton file
  // create and initialize the world
  dart::simulation::SoftWorld *myWorld
      = dart::utils::SoftSkelParser::readSoftFile(
          DART_DATA_PATH"skel/test/test_single_pendulum.skel");
  assert(myWorld != NULL);

  // create a window and link it to the world
  MyWindow window;
  window.setWorld(myWorld);

  std::cout << "space bar: simulation on/off" << 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'--'4': programmed interaction" << std::endl;

  glutInit(&argc, argv);
  window.initWindow(640, 480, "Soft Single Pendulum");
  glutMainLoop();

  return 0;
}

int main(int argc, char* argv[])
{
    // load a skeleton file
    FileInfoSkel<SkeletonDynamics> model;
    model.loadFile(DART_DATA_PATH"/skel/Chain.skel", SKEL);
    
    // create and initialize the world
    World *myWorld = new World();
    Vector3d gravity(0.0, -9.81, 0.0);
    myWorld->setGravity(gravity);
    myWorld->setTimeStep(1.0/2000);

    myWorld->addSkeleton((SkeletonDynamics*)model.getSkel());
    int nDof =  myWorld->getSkeleton(0)->getNumDofs();
    VectorXd initPose(nDof);
    for (int i = 0; i < nDof; i++)
        initPose[i] = random(-0.5, 0.5);
    myWorld->getSkeleton(0)->setPose(initPose);

    // create a window and link it to the world
    MyWindow window;
    window.setWorld(myWorld);
  
    glutInit(&argc, argv);
    window.initWindow(640, 480, "Forward Simulation");
    glutMainLoop();

    return 0;
}

//==============================================================================
int main(int argc, char* argv[])
{
  // create and initialize the world
  auto world = std::make_shared<simulation::World>();
  assert(world != nullptr);

  // create and initialize the world
  Eigen::Vector3d gravity(0.0, -9.81, 0.0);
  //world->setGravity(Eigen::Vector3d::Zero());
  world->setGravity(gravity);
  world->setTimeStep(1.0/1000);

  const auto numLinks = 10;
  const auto l = 1.0;
  auto skel = createNLinkRobot(numLinks, Vector3d(0.3, 0.3, l), DOF_ROLL);
//  auto skel = createNLinkRobot(numLinks, Vector3d(0.3, 0.3, l), BALL);
  world->addSkeleton(skel);

  const auto posLower = math::constantsd::pi() * -0.1;
  const auto posUpper = math::constantsd::pi() *  0.1;
  const auto velLower = math::constantsd::pi() * -0.1;
  const auto velUpper = math::constantsd::pi() *  0.1;
  for (auto i = 0u; i < numLinks; ++i)
  {
    auto joint = skel->getJoint(i);

    const auto pos = math::random(posLower, posUpper);
    const auto vel = math::random(velLower, velUpper);

    for (auto j = 0u; j < joint->getNumDofs(); ++j)
    {
      joint->setPosition(j, pos);
      joint->setVelocity(j, vel);
    }
  }

  world->dm_initialize_RIQN_DRNEA();

//  while (world->getTime() < 60.0)
//  {
//    std::cout << "time: " << world->getTime() << std::endl;
//    world->dm_step();
//  }

//  std::cout << "E: " << world->getSkeleton(0)->getTotalEnergy() << std::endl;

  // create a window and link it to the world
  MyWindow window;
  window.setWorld(world);

  glutInit(&argc, argv);
  window.initWindow(640, 480, "Forward Simulation");
  glutMainLoop();

  return 0;
}

int main(int argc, char* argv[])
{
  // create and initialize the world
  dart::simulation::WorldPtr myWorld
      = dart::utils::SkelParser::readWorld("dart://sample/skel/fullbody1.skel");
  assert(myWorld != nullptr);
  Eigen::Vector3d gravity(0.0, -9.81, 0.0);
  myWorld->setGravity(gravity);

  dart::dynamics::SkeletonPtr skel = myWorld->getSkeleton(1);

  std::vector<std::size_t> genCoordIds;
  genCoordIds.push_back(1);
  genCoordIds.push_back(6);  // left hip
  genCoordIds.push_back(9);  // left knee
  genCoordIds.push_back(10); // left ankle
  genCoordIds.push_back(13); // right hip
  genCoordIds.push_back(16); // right knee
  genCoordIds.push_back(17); // right ankle
  genCoordIds.push_back(21); // lower back
  Eigen::VectorXd initConfig(8);
  initConfig << -0.2, 0.15, -0.4, 0.25, 0.15, -0.4, 0.25, 0.0;
  skel->setPositions(genCoordIds, initConfig);

  dart::dynamics::Joint* joint0 = skel->getJoint(0);
  joint0->setActuatorType(dart::dynamics::Joint::PASSIVE);
  for (std::size_t i = 1; i < skel->getNumBodyNodes(); ++i)
  {
    dart::dynamics::Joint* joint = skel->getJoint(i);
    joint->setActuatorType(dart::dynamics::Joint::VELOCITY);
  }

  // create a window and link it to the world
  MyWindow window;
  window.setWorld(myWorld);

  std::cout << "space bar: simulation on/off" << 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'--'4': programmed interaction" << std::endl;
  std::cout << "'h': harness on/off" << std::endl;

  glutInit(&argc, argv);
  window.initWindow(640, 480, "Hybrid Dynamics");
  glutMainLoop();

  return 0;
}

int main(int argc, char* argv[]) {
  // create and initialize the world
  dart::simulation::World* myWorld
      = dart::utils::SkelParser::readSkelFile(
          DART_DATA_PATH"skel/fullbody1.skel");
  assert(myWorld != NULL);

  Eigen::Vector3d gravity(0.0, -9.81, 0.0);
  myWorld->setGravity(gravity);

  std::vector<int> genCoordIds;
  genCoordIds.push_back(1);
  genCoordIds.push_back(6);   // left hip
  genCoordIds.push_back(14);   // left knee
  genCoordIds.push_back(17);  // left ankle
  genCoordIds.push_back(9);  // right hip
  genCoordIds.push_back(15);  // right knee
  genCoordIds.push_back(19);  // right ankle
  genCoordIds.push_back(13);  // lower back
  Eigen::VectorXd initConfig(8);
  initConfig << -0.1, 0.2, -0.5, 0.3, 0.2, -0.5, 0.3, -0.1;
  myWorld->getSkeleton(1)->setConfig(genCoordIds, initConfig);

  // create controller
  Controller* myController = new Controller(myWorld->getSkeleton(1),
                                            myWorld->getConstraintHandler(),
                                            myWorld->getTimeStep());

  // create a window and link it to the world
  MyWindow window;
  window.setWorld(myWorld);
  window.setController(myController);

  std::cout << "space bar: simulation on/off" << 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'--'4': programmed interaction" << std::endl;

  glutInit(&argc, argv);
  window.initWindow(640, 480, "Balance");
  glutMainLoop();

  return 0;
}

int main(int argc, char* argv[])
{
  // Create a window for rendering and UI
  MyWindow window;

  // Create a world with 3 particles
  MyWorld *world = new MyWorld(3);

  // Link the world to the window and start running the glut event loop
  window.setWorld(world);
    
  std::cout << "space bar: simulation on/off" << std::endl;
  glutInit(&argc, argv);
  window.initWindow(640, 480, "Galileo's experiment"); 
  int a = 0;
  glutMainLoop();

  return 0;
}

int main(int argc, char* argv[])
{
  // load a skeleton file
  // create and initialize the world
  dart::simulation::World* myWorld
    = utils::SkelParser::readWorld(DART_DATA_PATH"/skel/chain.skel");
  assert(myWorld != NULL);
    
  // create and initialize the world
  Eigen::Vector3d gravity(0.0, -9.81, 0.0);
  myWorld->setGravity(gravity);
  myWorld->setTimeStep(1.0/2000);

  int dof =  myWorld->getSkeleton(0)->getNumDofs();

  Eigen::VectorXd initPose(dof);
  initPose.setZero();
  initPose[20] = 3.14159 * 0.4;
  initPose[23] = 3.14159 * 0.4;
  initPose[26] = 3.14159 * 0.4;
  initPose[29] = 3.14159 * 0.4;
  myWorld->getSkeleton(0)->setPositions(initPose);
  myWorld->getSkeleton(0)->computeForwardKinematics(true, true, false);

  // create a ball joint constraint
  BodyNode *bd1 = myWorld->getSkeleton(0)->getBodyNode("link 6");
  BodyNode *bd2 = myWorld->getSkeleton(0)->getBodyNode("link 10");
  Eigen::Vector3d offset(0.0, 0.025, 0.0);
  Eigen::Vector3d jointPos = bd1->getTransform() * offset;
  BallJointConstraint *cl = new BallJointConstraint( bd1, bd2, jointPos);
  //WeldJointConstraint *cl = new WeldJointConstraint(bd1, bd2);
  myWorld->getConstraintSolver()->addConstraint(cl);

  // create a window and link it to the world
  MyWindow window;
  window.setWorld(myWorld);
  
  glutInit(&argc, argv);
  window.initWindow(640, 480, "Closed Loop");
  glutMainLoop();

  return 0;
}

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


	// load a robot
	DartLoader dart_loader;
	string robot_file = VRC_DATA_PATH;
	robot_file = argv[1];
	World *mWorld = dart_loader.parseWorld(VRC_DATA_PATH ROBOT_URDF);
	SkeletonDynamics *robot = mWorld->getSkeleton("atlas");
    
    atlas_state_t state;
    state.init(robot);
    atlas_kinematics_t kin;
    kin.init(robot);

    robot->setPose(robot->getPose().setZero());

    // load a skeleton file
    FileInfoSkel<SkeletonDynamics> model;
    model.loadFile(VRC_DATA_PATH"/models/skel/ground1.skel", SKEL);
	SkeletonDynamics *ground = dynamic_cast<SkeletonDynamics *>(model.getSkel());
	ground->setName("ground");
	mWorld->addSkeleton(ground);
    
    // Zero atlas's feet at ground
    kinematics::BodyNode* foot = robot->getNode(ROBOT_LEFT_FOOT);
    kinematics::Shape* shoe = foot->getCollisionShape();
    double body_z = -shoe->getTransform().matrix()(2,3) + shoe->getDim()(2)/2;
    body_z += -foot->getWorldTransform()(2,3);
    VectorXd robot_dofs = robot->getPose();
    cout << "body_z = " << body_z << endl;
    state.dofs(2) = body_z;
    robot->setPose(state.dart_pose());

    cout << robot->getNode(ROBOT_BODY)->getWorldTransform() << endl;

    // Zero ground
    double ground_z = ground->getNode("ground1_root")->getCollisionShape()->getDim()(2)/2;
    ground->getNode("ground1_root")->getVisualizationShape()->setColor(Vector3d(0.5,0.5,0.5));
    VectorXd ground_dofs = ground->getPose();
    ground_dofs(1) = foot->getWorldTransform()(1,3);
    ground_dofs(2) = -ground_z;
    cout << "ground z = " << ground_z << endl;
    ground->setPose(ground_dofs);

    ////////////////////////////////////////////////////////////////////////////
    /// COM IK
    ////////////////////////////////////////////////////////////////////////////

    Vector3d world_com;
    Isometry3d end_effectors[NUM_MANIPULATORS];
    IK_Mode mode[NUM_MANIPULATORS];

    BodyNode* left_foot = state.robot()->getNode(ROBOT_LEFT_FOOT);
    BodyNode* right_foot = state.robot()->getNode(ROBOT_RIGHT_FOOT);

    end_effectors[MANIP_L_FOOT] = state.robot()->getNode(ROBOT_LEFT_FOOT)->getWorldTransform();
    end_effectors[MANIP_R_FOOT] = state.robot()->getNode(ROBOT_RIGHT_FOOT)->getWorldTransform();

    mode[MANIP_L_FOOT] = IK_MODE_SUPPORT;
    mode[MANIP_R_FOOT] = IK_MODE_WORLD;
    mode[MANIP_L_HAND] = IK_MODE_FIXED;
    mode[MANIP_R_HAND] = IK_MODE_FIXED;

    world_com = state.robot()->getWorldCOM();
    world_com(2) -= 0.1;

    Isometry3d body;
    state.get_body(body);
    body.rotate(AngleAxisd(M_PI/4, Vector3d::UnitY()));
    state.set_body(body);

    kin.com_ik(world_com, end_effectors, mode, state);

    robot->setPose(state.dart_pose());

	MyWindow window;
	window.setWorld(mWorld);

    glutInit(&argc, argv);
    window.initWindow(640, 480, "Robot Viz");
    glutMainLoop();

}