示例#1
0
文件: Main.cpp 项目: amehlberg17/dart
int main(int argc, char* argv[]) {
  // load a world
  dart::simulation::World* myWorld =
      dart::utils::SdfParser::readSdfFile(
        DART_DATA_PATH"/sdf/double_pendulum_with_base.world");
  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, "SDFormat Loader");
  glutMainLoop();

  return 0;
}
示例#2
0
文件: Main.cpp 项目: dartsim/dart
int main(int argc, char* argv[])
{
  // load a skeleton file
  // create and initialize the world
  dart::simulation::WorldPtr myWorld
      = dart::utils::SkelParser::readWorld("dart://sample/skel/shapes.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 << "'q': spawn a random cube" << std::endl;
  std::cout << "'w': spawn a random ellipsoid" << std::endl;
  std::cout << "'e': spawn a random cylinder" << std::endl;
  std::cout << "'a': delete a spawned object at last" << std::endl;

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

  return 0;
}
示例#3
0
文件: main.cpp 项目: ehuang3/drchubo
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();

}
示例#4
0
文件: Main.cpp 项目: amehlberg17/dart
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;
}