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