bool TaskSpaceRegion::Initialize(EnvironmentBasePtr penv_in) { _volume = 0; _sumbounds = 0; _dimensionality = 0; for(int i = 0; i < 6; i++) { //compute volume in whatever dimensionality this defines //when Bw values are backwards, it signifies an axis flip (not an error) if(Bw[i][1] != Bw[i][0]) { _volume = _volume * fabs(Bw[i][1] - Bw[i][0]); _sumbounds = _sumbounds + fabs(Bw[i][1] - Bw[i][0]); _dimensionality++; } } if( stricmp(relativebodyname.c_str(), "NULL") == 0 ) { prelativetolink.reset(); } else { KinBodyPtr pobject; pobject = penv_in->GetKinBody(relativebodyname.c_str()); if(pobject.get() == NULL) { RAVELOG_INFO("Error: could not find the specified object to attach frame\n"); return false; } //find the link vector<KinBody::LinkPtr> vlinks = pobject->GetLinks(); bool bGotLink = false; for(int j =0; j < vlinks.size(); j++) { if(strcmp(relativelinkname.c_str(), vlinks[j]->GetName().c_str()) == 0 ) { RAVELOG_INFO("frame link: %s:%s\n",vlinks[j]->GetParent()->GetName().c_str(),vlinks[j]->GetName().c_str() ); prelativetolink = vlinks[j]; bGotLink = true; break; } } if(!bGotLink) { RAVELOG_INFO("Error: could not find the specified link of the object to attach frame\n"); return false; } } //Print(); return true; }
TEST(cast, boxes) { EnvironmentBasePtr env = RaveCreateEnvironment(); ASSERT_TRUE(env->Load(data_dir() + "/box.xml")); ASSERT_TRUE(env->Load(data_dir() + "/boxbot.xml")); KinBodyPtr box = env->GetKinBody("box"); RobotBasePtr boxbot = env->GetRobot("boxbot"); RobotAndDOFPtr rad(new RobotAndDOF(boxbot, IntVec(), DOF_X | DOF_Y, Vector())); rad->GetRobot()->SetActiveDOFs(rad->GetJointIndices(), DOF_X | DOF_Y, Vector()); Json::Value root = readJsonFile(string(DATA_DIR) + "/box_cast_test.json"); DblVec start_dofs; start_dofs += -1.9, 0; rad->SetDOFValues(start_dofs); TrajOptProbPtr prob = ConstructProblem(root, env); TrajArray traj = prob->GetInitTraj(); //shouldn't be necessary: #if 0 ASSERT_TRUE(!!prob); double dist_pen = .02, coeff = 10; prob->addCost(CostPtr(new CollisionCost(dist_pen, coeff, rad, prob->GetVarRow(0), prob->GetVarRow(1)))); prob->addCost(CostPtr(new CollisionCost(dist_pen, coeff, rad, prob->GetVarRow(1), prob->GetVarRow(2)))); CollisionCheckerPtr checker = CollisionChecker::GetOrCreate(*prob->GetEnv()); { vector<Collision> collisions; checker->ContinuousCheckTrajectory(traj, *rad, collisions); } vector<Collision> collisions; cout << "traj: " << endl << traj << endl; checker->CastVsAll(*rad, rad->GetRobot()->GetLinks(), toDblVec(traj.row(0)), toDblVec(traj.row(1)), collisions); cout << collisions.size() << endl; #endif BasicTrustRegionSQP opt(prob); if (plotting) opt.addCallback(PlotCallback(*prob)); opt.initialize(trajToDblVec(prob->GetInitTraj())); opt.optimize(); if (plotting) PlotCallback(*prob)(NULL, opt.x()); }
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; }
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); } }
TEST(continuous_collisions, boxes) { EnvironmentBasePtr env = RaveCreateEnvironment(); ASSERT_TRUE(env->Load(data_dir() + "/box.xml")); ASSERT_TRUE(env->Load(data_dir() + "/boxbot.xml")); KinBodyPtr box = env->GetKinBody("box"); RobotBasePtr boxbot = env->GetRobot("boxbot"); CollisionCheckerPtr checker = CreateCollisionChecker(env); { RobotAndDOFPtr rad(new RobotAndDOF(boxbot, IntVec(), DOF_X | DOF_Y, Vector())); TrajArray traj(2,2); traj << -1.9,0, 0,1.9; vector<Collision> collisions; checker->ContinuousCheckTrajectory(traj, rad, collisions); ASSERT_EQ(collisions.size(), 1); Collision col = collisions[0]; Vector robot_normal = (float)(col.linkA == boxbot->GetLinks()[0].get() ? 1. : -1.) * col.normalB2A; EXPECT_VECTOR_NEAR(robot_normal, Vector(-1, 0, 0), 1e-4); } #define TRAJ_TEST_BOILERPLATE\ RobotAndDOFPtr rad(new RobotAndDOF(boxbot, IntVec(), DOF_X | DOF_Y, Vector()));\ vector<Collision> collisions;\ checker->CastVsAll(*rad, rad->GetRobot()->GetLinks(), toDblVec(traj.row(0)), toDblVec(traj.row(1)), collisions);\ ASSERT_EQ(collisions.size(), 1);\ Collision col = collisions[0];\ Vector robot_normal = (float)(col.linkA == boxbot->GetLinks()[0].get() ? 1. : -1.) * col.normalB2A; { TrajArray traj(2,2); traj << -1.9,0, 0,1.9; TRAJ_TEST_BOILERPLATE EXPECT_VECTOR_NEAR(robot_normal, Vector(-1/sqrtf(2), 1/sqrtf(2), 0), 1e-4); EXPECT_NEAR(col.time, .5, 1e-1); } { TrajArray traj(2,2); traj << 0, .9, 0,2; TRAJ_TEST_BOILERPLATE EXPECT_VECTOR_NEAR(robot_normal, Vector(0,1,0), 1e-4); EXPECT_NEAR(col.time, 0, 1e-6); } { TrajArray traj(2,2); traj << 0,2, 0,.9; TRAJ_TEST_BOILERPLATE EXPECT_VECTOR_NEAR(robot_normal, Vector(0,1,0), 1e-4); EXPECT_NEAR(col.time, 1, 1e-6); } { RobotAndDOFPtr rad(new RobotAndDOF(boxbot, IntVec(), DOF_X | DOF_Y | DOF_Z, Vector())); vector<Collision> collisions; vector<DblVec> multi_joints; multi_joints.push_back(toDblVec(Eigen::Vector3d(0,1.9,5))); multi_joints.push_back(toDblVec(Eigen::Vector3d(-1.9,0,5))); multi_joints.push_back(toDblVec(Eigen::Vector3d(0,-2,5))); multi_joints.push_back(toDblVec(Eigen::Vector3d(2,0,5))); multi_joints.push_back(toDblVec(Eigen::Vector3d(0,1.8,-5))); multi_joints.push_back(toDblVec(Eigen::Vector3d(-1.9,0,-5))); multi_joints.push_back(toDblVec(Eigen::Vector3d(0,-2,-5))); multi_joints.push_back(toDblVec(Eigen::Vector3d(2,0,-5))); checker->MultiCastVsAll(*rad, rad->GetRobot()->GetLinks(), multi_joints, collisions); ASSERT_EQ(collisions.size(), 1); Collision col = collisions[0]; Vector robot_normal = (float)(col.linkA == boxbot->GetLinks()[0].get() ? 1. : -1.) * col.normalB2A; EXPECT_VECTOR_NEAR(robot_normal, Vector(1/sqrtf(2), -1/sqrtf(2), 0), 1e-4); } }