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