bool GetIKSolutions(EnvironmentBasePtr _penv, Transform Pose, std::vector< std::vector< dReal > > &vsolution){ ModuleBasePtr _pikfast = RaveCreateModule(_penv,"ikfast"); RobotBasePtr _probot = _penv->GetRobot(robotname); _probot->SetActiveManipulator("1"); RobotBase::ManipulatorPtr _pmanip = _probot->GetActiveManipulator(); _penv->Add(_pikfast,true,""); std::vector< std::vector< dReal > > solns; solns.resize(6); stringstream ssin,ssout; string iktype = "Transform6D"; ssin << "LoadIKFastSolver " << _probot->GetName() << " " << iktype; if( !_pikfast->SendCommand(ssout,ssin) ) { RAVELOG_ERROR("failed to load iksolver\n"); _penv->Destroy(); return false; } if(_pmanip->FindIKSolutions(IkParameterization(Pose),vsolution,IKFO_CheckEnvCollisions) ) { stringstream ss; ss << "solution is: "; for(size_t i = 0; i < vsolution.size(); ++i) { for(size_t j = 0; j < vsolution[i].size(); ++j){ ss << vsolution[i][j] << " " ; } ss << endl; } ss << endl; ////RAVELOG_INFO(ss.str()); } else { // could fail due to collisions, etc return false; } return true ; }
void GetTaskTime(EnvironmentBasePtr _penv, std::vector<dReal> &vsolutionA, std::vector<dReal> &vsolutionB, TrajectoryBasePtr ptraj){ //thread::id get_id(); std::cout << "this thread ::" << boost::this_thread::get_id() << std::endl; PlannerBasePtr planner = RaveCreatePlanner(_penv,"birrt"); //std::vector<dReal> vinitialconfig,vgoalconfig; ptraj = RaveCreateTrajectory(_penv,""); PlannerBase::PlannerParametersPtr params(new PlannerBase::PlannerParameters()); RobotBasePtr probot = _penv->GetRobot(robotname); params->_nMaxIterations = 4000; // max iterations before failure GraphHandlePtr pgraph; { EnvironmentMutex::scoped_lock lock(_penv->GetMutex()); // lock environment params->SetRobotActiveJoints(probot); // set planning configuration space to current active dofs //initial config probot->SetActiveDOFValues(vsolutionA); params->vinitialconfig.resize(probot->GetActiveDOF()); probot->GetActiveDOFValues(params->vinitialconfig); //goal config probot->SetActiveDOFValues(vsolutionB); params->vgoalconfig.resize(probot->GetActiveDOF()); probot->GetActiveDOFValues(params->vgoalconfig); //setting limits vector<dReal> vlower,vupper; probot->GetActiveDOFLimits(vlower,vupper); //RAVELOG_INFO("starting to plan\n"); if( !planner->InitPlan(probot,params) ) { //return ptraj; } // create a new output trajectory if( !planner->PlanPath(ptraj) ) { RAVELOG_WARN("plan failed \n"); //return NULL; } } ////RAVELOG_INFO(ss.str()); if (!! ptraj){ //std::cout << ptraj->GetDuration() << std::endl; writeTrajectory(ptraj); } std::cout << "this thread ::" << boost::this_thread::get_id() << " has completed its work" << std::endl; mtx__.lock(); completed_thread = boost::this_thread::get_id(); mtx__.unlock(); //return ptraj; }
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() { 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(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); } }