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();
}
std::string getPlanJSONString()
{
return factory.SaveJSON();
}
void setPlanJSONString(const char* string)
{
if(!factory.LoadJSON(string))
throw PyException("Invalid JSON string");
}
