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