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;
}
TEST(collision_checker, box_distance) {
EnvironmentBasePtr env = RaveCreateEnvironment();
ASSERT_TRUE(env->Load(data_dir() + "/box.xml"));
KinBodyPtr box0 = env->GetKinBody("box");
box0->SetName("box0");
ASSERT_TRUE(env->Load(DATA_DIR + string("/box.xml")));
KinBodyPtr box1 = env->GetKinBody("box");
box1->SetName("box1");
vector<KinBodyPtr> bodies; env->GetBodies(bodies);
RAVELOG_DEBUG("%i kinbodies in rave env\n", bodies.size());
CollisionCheckerPtr checker = CreateCollisionChecker(env);
#define EXPECT_NUM_COLLISIONS(n)\
vector<Collision> collisions;\
checker->AllVsAll(collisions);\
EXPECT_EQ(collisions.size(), n);\
collisions.clear();\
checker->BodyVsAll(*box0, collisions);\
EXPECT_EQ(collisions.size(), n);
{
checker->SetContactDistance(0);
box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(.9,0,0)));
box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0)));
EXPECT_NUM_COLLISIONS(1);
}
{
checker->SetContactDistance(0);
box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(1.1,0,0)));
box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0)));
EXPECT_NUM_COLLISIONS(0);
}
{
checker->SetContactDistance(.04);
box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(1.1,0,0)));
box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0)));
EXPECT_NUM_COLLISIONS(0);
}
{
checker->SetContactDistance(.1);
box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(1.09,0,0)));
box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0)));
EXPECT_NUM_COLLISIONS(1);
}
{
checker->SetContactDistance(.2);
box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(1.1,0,0)));
box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0)));
EXPECT_NUM_COLLISIONS(1);
}
{
env->Remove(box1);
EXPECT_NUM_COLLISIONS(0);
}
}
