virtual bool isValid(const ob::State *state) const { if (si_->satisfiesBounds(state) == false) return false; RobotBasePtr probot; probot = si_->getStateSpace()->as<SuperBotStateSpace>()->_probot; const ob::CompoundStateSpace::StateType *s = state->as<ob::CompoundStateSpace::StateType>(); const double* q = s->as<ob::RealVectorStateSpace::StateType>(0)->values; const double* qd = s->as<ob::RealVectorStateSpace::StateType>(1)->values; std::vector<double> jointvalues; for (int i = 0; i < int(probot->GetActiveDOF()); i++) jointvalues.push_back(q[i]); for (int i = int(probot->GetActiveDOF()); i < int(probot->GetDOF()); i++) jointvalues.push_back(0.0); { EnvironmentMutex::scoped_lock lock(probot->GetEnv()->GetMutex()); probot->SetDOFValues(jointvalues, CLA_NOTHING); if (probot->CheckSelfCollision()) return false; if (probot->GetEnv()->CheckCollision(probot)) return false; else 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; }
DensoControlSpace::DensoControlSpace(RobotBasePtr probot) : oc::RealVectorControlSpace(ob::StateSpacePtr(new DensoStateSpace(probot)), probot->GetActiveDOF()), _probot(probot) { unsigned int ndof = _probot->GetActiveDOF(); if (_controlViaAcceleration) { std::vector<double> qddBounds; _probot->GetDOFAccelerationLimits(qddBounds); for (unsigned int i = 0; i < ndof; i++) { bounds_.setLow(i, -qddBounds[i]); bounds_.setHigh(i, qddBounds[i]); } } else { std::vector<double> qdBounds; _probot->GetDOFVelocityLimits(qdBounds); for (unsigned int i = 0; i < ndof; i++) { bounds_.setLow(i, -qdBounds[i]); bounds_.setHigh(i, qdBounds[i]); } } }
SuperBotSetup::SuperBotSetup(RobotBasePtr probot) : oc::SimpleSetup(oc::ControlSpacePtr(new SuperBotControlSpace(probot))) { std::cout << "Planning for the first " << probot->GetActiveDOF() << " DOFs\n"; initialize(probot); }