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(); } double const duration = timer.elapsed(); RAVELOG_INFO( "Ran %d collision checks (%d in collision) in %f seconds (%f checks per second).\n", num_trials, num_collision, duration, num_trials / duration ); return 0; }
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; }