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