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()
{
traj.clear();
unsigned int mainthreadsleft = numThreads;
// boost::shared_ptr<boost::thread> ( new boost::thread(boost::bind( &track_threads )));
Ta.trans = transA;
Ta.rot = quatA;
Tb.trans = transB;
Tb.rot = quatB;
std::string scenefilename = "scenes/test6dof.mujin.zae";
std::string viewername = "qtcoin";
RaveInitialize(true); // start openrave core
EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment
Transform robot_t;
RaveVector< dReal > transR(c, d, 0);
robot_t.trans = transR;
//boost::thread thviewer(boost::bind(SetViewer,penv,viewername));
penv->Load(scenefilename);
RobotBasePtr probot = penv->GetRobot("RV-4F");
//removing floor for collision checking
EnvironmentBasePtr pclondedenv = penv->CloneSelf(Clone_Bodies);
pclondedenv->Remove( pclondedenv->GetKinBody("floor"));
RobotBasePtr probot_clone = pclondedenv->GetRobot("RV-4F");
unsigned int tot = ((( abs(a) + abs(c) )/discretization_x )+1) * (((( abs(b) + abs(d) )/discretization_y )+1) * (( abs( z )/discretization_z )+1));
unsigned int tot_o = tot;
for (unsigned int i = 0 ; i <= (( abs(a) + abs(c) )/discretization_x );i++) {
for (unsigned int j = 0 ; j <= (( abs(b) + abs(d) )/discretization_y ); j++) {
for (unsigned int k = 0 ; k <= ( abs( z )/discretization_z ) ; k++) {
////std::cout << transR[0] << ";" << transR[1] << ";" << transR[2] << std::endl;
//boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
robot_t.trans = transR;
probot->SetTransform(robot_t);
probot_clone->SetTransform(robot_t);
if( pclondedenv->CheckCollision(RobotBaseConstPtr(probot_clone)) ){
//std::cout << "Robot in collision with the environment" << std::endl;
}
else {
do_task(Ta, Tb, penv,3);
}
tot -= 1;
std::cout << tot << "/" << tot_o << std::endl;
transR[2] = transR[2]+ discretization_z;
}
transR[2] = 0;
transR[1] = transR[1] + discretization_y;
robot_t.trans = transR;
}
transR[2] = 0;
transR[1] = c;
transR[0] = transR[0] + discretization_x;
robot_t.trans = transR;
}
//thviewer.join(); // wait for the viewer thread to exit
RaveDestroy(); // make sure to destroy the OpenRAVE runtime
penv->Destroy(); // destroy
return 0;
}
