int makeNewPlan(int cspace) { if(cspace < 0 || cspace >= (int)spaces.size() || spaces[cspace]==NULL) throw PyException("Invalid cspace index"); if(plansDeleteList.empty()) { plans.push_back(factory.Create(spaces[cspace])); return (int)plans.size()-1; } else { int index = plansDeleteList.front(); plansDeleteList.erase(plansDeleteList.begin()); plans[index] = factory.Create(spaces[cspace]); return index; } }
bool PlannerInterface::setEndpoints(PyObject* start,PyObject* goal) { if(index < 0 || index >= (int)plans.size() || plans[index]==NULL) throw PyException("Invalid plan index"); Config qstart,qgoal; bool res=PyListToConfig(start,qstart); if(!res) throw PyException("Invalid start endpoint"); if(!spaces[spaceIndex]->IsFeasible(qstart)) { throw PyException("Start configuration is infeasible"); } int istart=plans[index]->AddMilestone(qstart); if(istart < 0) { throw PyException("Start configuration is infeasible"); } if(istart != 0) { throw PyException("Plan already initialized?"); } res=PyListToConfig(goal,qgoal); if(res) { if(!spaces[spaceIndex]->IsFeasible(qgoal)) { throw PyException("Goal configuration is infeasible"); } int igoal=plans[index]->AddMilestone(qgoal); if(igoal < 0) { throw PyException("Goal configuration is infeasible"); } } else { //test if it's a goal test if(PyCallable_Check(goal)) { goalSets.resize(plans.size()); goalSets[index] = new PyGoalSet(spaces[spaceIndex],goal); plans[index]=factory.Create(spaces[spaceIndex],qstart,goalSets[index]); } else { throw PyException("Invalid goal endpoint"); } } return true; }
/** @brief Performs path planning in collision-free space for the * given robot at the stance s between start configuration qstart and * destination qgoal. * Stability is tested at each configuration. * * The output is given in path. cond controls the number of iterations/time * for planning. * * The constraint specifications are given in WorldPlannerSettings. If you * have custom requirements, you will need to set them up. */ bool StancePlan(RobotWorld& world,int robot,Config& qstart,Config& qgoal,const Stance& s,MilestonePath& path, const HaltingCondition& cond,const string& plannerSettings="") { WorldPlannerSettings settings; settings.InitializeDefault(world); //do more constraint setup here (modifying the settings object) if desired, //e.g., set collision margins, edge collision checking resolution, etc. StanceCSpace cspace(world,robot,&settings); cspace.SetStance(s); cspace.CalculateSP(); if(gSupportPolygonMargin != 0) cspace.SetSPMargin(gSupportPolygonMargin); //sanity checking if(!cspace.CheckContact(qstart)) { printf("Start configuration does not meet contact constraint, error %g\n",cspace.ContactDistance(qstart)); if(!cspace.SolveContact()) { printf(" Solving for contact failed.\n"); return false; } else { printf(" IK problem was solved, using new start configuration\n"); qstart = cspace.GetRobot()->q; } } if(!cspace.CheckContact(qgoal)) { printf("Goal configuration does not meet contact constraint, error %g\n",cspace.ContactDistance(qgoal)); if(!cspace.SolveContact()) { printf(" Solving for contact failed.\n"); return false; } else { printf(" IK problem was solved, using new goal configuration\n"); qgoal = cspace.GetRobot()->q; } } if(!cspace.IsFeasible(qstart)) { cout<<"Start configuration is infeasible, violated constraints:"<<endl; vector<bool> infeasible; cspace.CheckObstacles(qstart,infeasible); for(size_t i=0;i<infeasible.size();i++) if(infeasible[i]) cout<<" "<<cspace.ObstacleName(i)<<endl; return false; } if(!cspace.IsFeasible(qgoal)) { cout<<"Goal configuration is infeasible, violated constraints:"<<endl; vector<bool> infeasible; cspace.CheckObstacles(qgoal,infeasible); for(size_t i=0;i<infeasible.size();i++) if(infeasible[i]) cout<<" "<<cspace.ObstacleName(i)<<endl; return false; } MotionPlannerFactory factory; if(!plannerSettings.empty()) factory.LoadJSON(plannerSettings); //do more planner setup here if desired, e.g., change planner type, //perturbation size, connection radius, etc //make the planner and do the planning MotionPlannerInterface* planner = factory.Create(&cspace,qstart,qgoal); string res = planner->Plan(path,cond); cout<<"Planner terminated with condition "<<res<<endl; printf(" Stats: SolveContact %d, %gs. IsFeasible %d, %gs\n",cspace.numSolveContact,cspace.solveContactTime,cspace.numIsFeasible,cspace.isFeasibleTime); delete planner; return !path.edges.empty(); }