C++ EnvironmentBasePtr::StopSimulation方法代码示例

 static void SetUpTestCase() {
   RAVELOG_DEBUG("SetupTestCase\n");
   RaveInitialize(false, verbose ? Level_Debug : Level_Info);
   env = RaveCreateEnvironment();
   env->StopSimulation();
   env->Load("robots/pr2-beta-static.zae") ;
   env->Load(string(DATA_DIR) + "/table.xml");
   if (plotting) {
     viewer.reset(new OSGViewer(env));
     viewer->UpdateSceneData();
     env->AddViewer(viewer);
   }
 }

int main() {
  RaveInitialize(false, OpenRAVE::Level_Debug);
  env = RaveCreateEnvironment();
  env->StopSimulation();
//  bool success = env->Load("data/pr2test2.env.xml");
  {
    bool success = env->Load("/home/joschu/Proj/drc/gfe.xml");
    FAIL_IF_FALSE(success);
  }
  {
    bool success = env->Load("/home/joschu/Proj/trajopt/data/test2.env.xml");
    FAIL_IF_FALSE(success);
  }
  vector<RobotBasePtr> robots;
  env->GetRobots(robots);
  RobotBasePtr robot = robots[0];
  vector<RobotBase::ManipulatorPtr> manips = robot->GetManipulators();


  cc = CollisionChecker::GetOrCreate(*env);
  viewer.reset(new OSGViewer(env));
  env->AddViewer(viewer);



  ManipulatorControl mc(manips[manips.size()-1], viewer);
  DriveControl dc(robot, viewer);
  StatePrinter sp(robot);
  viewer->AddKeyCallback('a', boost::bind(&StatePrinter::PrintAll, &sp));
  viewer->AddKeyCallback('q', &PlotCollisionGeometry);
  viewer->AddKeyCallback('=', boost::bind(&AdjustTransparency, .05));
  viewer->AddKeyCallback('-', boost::bind(&AdjustTransparency, -.05));

  viewer->Idle();

  env.reset();
  viewer.reset();
  RaveDestroy();

}

int main(int argc, char **argv)
{
    std::string environment_uri;
    std::string robot_name;
    std::string plugin_name;
    size_t num_trials;
    bool self = false;
    bool profile = false;

    // Parse arguments.
    po::options_description desc("Profile OpenRAVE's memory usage.");
    desc.add_options()
        ("num-samples", po::value<size_t>(&num_trials)->default_value(10000),
            "number of samples to run")
        ("self", po::value<bool>(&self)->zero_tokens(),
            "run self-collision checks")
        ("profile", po::value<bool>(&profile)->zero_tokens(),
            "remove objects from environment")
        ("environment_uri",
            po::value<std::string>(&environment_uri)->required(),
            "number of samples to run")
        ("robot", po::value<std::string>(&robot_name)->required(),
            "robot_name")
        ("collision_checker",
            po::value<std::string>(&plugin_name)->required(),
            "collision checker name")
        ("help",
            "print usage information")
        ;

    po::positional_options_description pd;
    pd.add("environment_uri", 1);
    pd.add("robot", 1);
    pd.add("collision_checker", 1);

    po::variables_map vm;
    po::store(
        po::command_line_parser(argc, argv)
            .options(desc)
            .positional(pd).run(),
        vm
    );
    po::notify(vm);    

    if (vm.count("help")) {
        std::cout << desc << std::endl;
        return 1;
    }

    // Create the OpenRAVE environment.
    RaveInitialize(true);

    EnvironmentBasePtr const env = RaveCreateEnvironment();
    CollisionCheckerBasePtr const collision_checker
            = RaveCreateCollisionChecker(env, plugin_name);
    env->SetCollisionChecker(collision_checker);

    env->StopSimulation();

    // "/usr/share/openrave-0.9/data/wamtest1.env.xml"
    env->Load(environment_uri);
    KinBodyPtr const body = env->GetKinBody(robot_name);

    // Generate random configuations.
    std::vector<OpenRAVE::dReal> lower;
    std::vector<OpenRAVE::dReal> upper;
    body->GetDOFLimits(lower, upper);
	std::vector<std::vector<double> > data;
    data = benchmarks::DataUtils::GenerateRandomConfigurations(
            num_trials, lower, upper);

    //
    RAVELOG_INFO("Running %d collision checks.\n", num_trials);

    boost::timer const timer;
    if (profile) {
        std::string const prof_name = str(
                format("CheckCollision.%s.prof") %  plugin_name);
        RAVELOG_INFO("Writing gperftools information to '%s'.\n",
            prof_name.c_str()
        );

        ProfilerStart(prof_name.c_str());
    }

    size_t num_collision = 0;
    for (size_t i = 0; i < num_trials; ++i) {
        body->SetDOFValues(data[i]);

        bool is_collision;
        if (self) {
            is_collision = body->CheckSelfCollision();
        } else {
            is_collision = env->CheckCollision(body);
        }
        num_collision += !!is_collision;
    }

    if (profile) {
        ProfilerStop();
//.........这里部分代码省略.........

int main(int argc, char *argv[]) {
  int T = 4;
  bool sim_plotting = false;
  bool stage_plotting = false;
  bool first_step_only = false;

  {
    Config config;
    config.add(new Parameter<bool>("sim_plotting", &sim_plotting, "sim_plotting"));
    config.add(new Parameter<bool>("stage_plotting", &stage_plotting, "stage_plotting"));
    config.add(new Parameter<bool>("first_step_only", &first_step_only, "first_step_only"));
    CommandParser parser(config);
    parser.read(argc, argv, true);
  }

  string manip_name("base_point");
  string link_name("Base");

  RaveInitialize();
  EnvironmentBasePtr env = RaveCreateEnvironment();
  env->StopSimulation();
  env->Load(string(DATA_DIR) + "/point.env.xml");
  OSGViewerPtr viewer;
  RobotBasePtr robot = GetRobot(*env);


  Vector2d start = Vector2d::Zero();
  Matrix2d start_sigma = Matrix2d::Identity() * 0.2 * 0.2;

  deque<Vector2d> initial_controls;
  for (int i = 0; i < T; ++i) {
    //initial_controls.push_back((Vector2d(0, 5) - start) / T);//Vector2d::Zero());
    initial_controls.push_back(Vector2d::Zero());
  }

  Matrix4d goal_trans;
  goal_trans <<  1, 0, 0, 0,
                 0, 1, 0, 5,
                 0, 0, 1, 0,
                 0, 0, 0, 1;

  initialize_robot(robot, start);

  PointRobotBSPPlannerPtr planner(new PointRobotBSPPlanner());

  planner->start = start;
  planner->start_sigma = start_sigma;
  planner->goal_trans = goal_trans;
  planner->T = T;
  planner->controls = initial_controls;
  planner->robot = robot;
  planner->rad = RADFromName(manip_name, robot);
  planner->link = planner->rad->GetRobot()->GetLink(link_name);
  planner->method = BSP::DiscontinuousBeliefSpace;
  planner->initialize();

  cout << "dof: " << planner->rad->GetDOF() << endl;

  boost::function<void(OptProb*, DblVec&)> opt_callback;
  if (stage_plotting || sim_plotting) {
    viewer = OSGViewer::GetOrCreate(env);
    initialize_viewer(viewer);
  }

  if (stage_plotting) {
    opt_callback = boost::bind(&stage_plot_callback, 
                               planner, viewer, _1, _2);
  }

  while (!planner->finished()) {
    planner->solve(opt_callback, 1, 1);
    planner->simulate_execution();
    if (first_step_only) break;
    if (sim_plotting) {
      OpenRAVEPlotterMixin<PointRobotBSPPlanner>::sim_plot_callback(planner, planner->rad, viewer);
    }
  }

  RaveDestroy();
  return 0;
}