ob::PlannerStatus KrisLibraryOMPLPlanner::solve (const ob::PlannerTerminationCondition &ptc)
{
if(!planner) {
//may have had a previous clear() call
//fprintf(stderr,"KrisLibraryOMPLPlanner::solve(): Warning, setup() not called yet\n");
setup();
if(!planner)
return ob::PlannerStatus(ob::PlannerStatus::UNKNOWN);
}
ob::ProblemDefinitionPtr pdef = this->getProblemDefinition();
//how much to plan?
int increment = (StartsWith(factory.type.c_str(),"fmm") ? 1 : 50);
Real oldBest = Inf;
bool optimizing = planner->IsOptimizing();
double desiredCost = 0;
if(optimizing) {
if(pdef->getOptimizationObjective() != NULL)
desiredCost = pdef->getOptimizationObjective()->getCostThreshold().value();
else
OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed planning time.", getName().c_str());
}
while(!ptc()) {
if(planner->IsSolved()) {
//convert solution to OMPL solution
MilestonePath path;
planner->GetSolution(path);
if(optimizing) {
//optimizing
Real cost = path.Length();
if(cost < oldBest) {
oldBest = cost;
if(cost < desiredCost) {
ob::PathPtr pptr(ToOMPL(si_,path));
this->getProblemDefinition()->addSolutionPath(pptr);
return ob::PlannerStatus(true,false);
}
}
}
else {
//non-optimizing
ob::PathPtr pptr(ToOMPL(si_,path));
this->getProblemDefinition()->addSolutionPath(pptr);
return ob::PlannerStatus(true,false);
}
}
planner->PlanMore(increment);
}
if(planner->IsSolved()) {
//convert solution to OMPL solution
MilestonePath path;
planner->GetSolution(path);
ob::PathPtr pptr(ToOMPL(si_,path));
this->getProblemDefinition()->addSolutionPath(pptr);
return ob::PlannerStatus(true,false);
}
return ob::PlannerStatus(false,false);
}
void SBLPRT::CreatePath(int i,int j,MilestonePath& path)
{
Assert(i >= 0 && i < (int)roadmap.nodes.size());
Assert(j >= 0 && j < (int)roadmap.nodes.size());
ConnectedSeedCallback callback(this,j);
callback.node = j;
roadmap.NewTraversal();
roadmap._BFS(i,callback);
if(!callback.found) {
cerr<<"SBLPRT::CreatePath: Warning, a path doesn't exist between nodes "<<i<<" and "<<j<<endl;
return;
}
list<PathInfo> subpaths;
int n=j;
while(n != i) {
int p=callback.parents[n];
Assert(p >= 0);
PathInfo temp;
temp.tstart = p;
temp.tgoal = n;
temp.path = roadmap.FindEdge(n,p);
Assert(temp.path != NULL);
Assert(!temp.path->edges.empty());
subpaths.push_front(temp);
n=p;
}
for(list<PathInfo>::iterator it=subpaths.begin();it!=subpaths.end();it++) {
int s=it->tstart;
int g=it->tgoal;
Assert(s >= 0 && s <(int)roadmap.nodes.size());
Assert(g >= 0 && g <(int)roadmap.nodes.size());
MilestonePath& subpath=*it->path;
Assert(!subpath.edges.empty());
//forward path or backward path?
if(subpath.edges.front()->Start() == *roadmap.nodes[s]->root) {
Assert(subpath.edges.back()->Goal() == *roadmap.nodes[g]->root);
//forward path
path.Concat(subpath);
if(!path.IsValid()) fprintf(stderr,"SBLPRT::CreatePath: Path invalidated on %d %d\n",s,g);
}
else {
Assert(subpath.edges.front()->Start() == *roadmap.nodes[g]->root);
Assert(subpath.edges.back()->Goal() == *roadmap.nodes[s]->root);
//backward path
for(int k=(int)subpath.edges.size()-1;k>=0;k--) {
path.edges.push_back(subpath.edges[k]->ReverseCopy());
}
if(!path.IsValid()) fprintf(stderr,"SBLPRT::CreatePath: Path invalidated on backwards %d %d\n",s,g);
}
}
Assert(path.IsValid());
}
PyObject* PlannerInterface::getPath(int milestone1,int milestone2)
{
if(index < 0 || index >= (int)plans.size() || plans[index]==NULL)
throw PyException("Invalid plan index");
if(!plans[index]->IsConnected(milestone1,milestone2)) {
Py_RETURN_NONE;
}
MilestonePath path;
plans[index]->GetPath(milestone1,milestone2,path);
PyObject* pypath = PyList_New(path.NumMilestones());
for(int i=0;i<path.NumMilestones();i++)
PyList_SetItem(pypath,(Py_ssize_t)i,PyListFromConfig(path.GetMilestone(i)));
return pypath;
}
void PointToSetMotionPlanner::GetSolution(MilestonePath& path)
{
path.edges.clear();
for(size_t i=0;i<goalNodes.size();i++) {
MilestonePath temp;
mp->GetPath(0,goalNodes[i],temp);
if(!temp.edges.empty()) {
if(path.edges.empty()) path = temp;
else {
if(temp.Length() < path.Length())
path = temp;
}
}
}
}
bool VisibilityGraphPlanner::Plan(const Vector2& a,const Vector2& b,vector<Vector2>& path)
{
Config qa(2),qb(2);
qa.copy(a);
qb.copy(b);
MilestonePath mpath;
if(!Plan(qa,qb,mpath)) {
path.resize(0);
return false;
}
path.resize(mpath.NumMilestones());
for(size_t i=0;i<path.size();i++)
path[i].set(mpath.GetMilestone(i));
return true;
}
std::string MotionPlannerInterface::Plan(MilestonePath& path,const HaltingCondition& cond)
{
path.edges.clear();
bool foundPath = false;
Real lastCheckTime = 0, lastCheckValue = 0;
Timer timer;
for(int iters=0;iters<cond.maxIters;iters++) {
Real t=timer.ElapsedTime();
if(t > cond.timeLimit) {
if(foundPath) {
//get the final path
GetSolution(path);
}
return "timeLimit";
}
//check for cost improvements
if(foundPath && t > lastCheckTime + cond.costImprovementPeriod) {
GetSolution(path);
Real len = path.Length();
if(len < cond.costThreshold)
return "costThreshold";
if(lastCheckValue - len < cond.costImprovementThreshold)
return "costImprovementThreshold";
lastCheckTime = t;
lastCheckValue = len;
}
//do planning, check if a path is found
PlanMore();
if(!foundPath) {
if(IsSolved()) {
foundPath = true;
GetSolution(path);
if(cond.foundSolution) {
return "foundSolution";
}
lastCheckTime = t;
lastCheckValue = path.Length();
}
}
}
if(foundPath) {
//get the final path
GetSolution(path);
}
return "maxIters";
}
void ReversePath(MilestonePath& path)
{
for(size_t k=0;k<path.edges.size()/2;k++) {
SmartPointer<EdgePlanner> e1 = path.edges[k];
SmartPointer<EdgePlanner> e2 = path.edges[path.edges.size()-k];
path.edges[k] = e2->ReverseCopy();
path.edges[path.edges.size()-k] = e1->ReverseCopy();
}
if(path.edges.size()%2 == 1)
path.edges[path.edges.size()/2] = path.edges[path.edges.size()/2]->ReverseCopy();
if(!path.IsValid()) fprintf(stderr,"ReversePath : Path invalidated ?!?!\n");
}
void RoadmapPlanner::CreatePath(int i,int j,MilestonePath& path)
{
Assert(ccs.SameComponent(i,j));
//Graph::PathIntCallback callback(roadmap.nodes.size(),j);
//roadmap._BFS(i,callback);
EdgeDistance distanceWeightFunc;
Graph::ShortestPathProblem<Config,SmartPointer<EdgePlanner> > spp(roadmap);
spp.InitializeSource(i);
spp.FindPath_Undirected(j,distanceWeightFunc);
if(IsInf(spp.d[j])) {
FatalError("RoadmapPlanner::CreatePath: SameComponent is true, but no shortest path?");
return;
}
list<int> nodes;
//Graph::GetAncestorPath(callback.parents,j,i,nodes);
bool res=Graph::GetAncestorPath(spp.p,j,i,nodes);
if(!res) {
FatalError("RoadmapPlanner::CreatePath: GetAncestorPath returned false");
return;
}
if(nodes.front() != i || nodes.back() != j) {
FatalError("RoadmapPlanner::CreatePath: GetAncestorPath didn't return correct path? %d to %d vs %d to %d",nodes.front(),nodes.back(),i,j);
}
Assert(nodes.front()==i);
Assert(nodes.back()==j);
path.edges.clear();
path.edges.reserve(nodes.size());
for(list<int>::const_iterator p=nodes.begin();p!=--nodes.end();++p) {
list<int>::const_iterator n=p; ++n;
SmartPointer<EdgePlanner>* e=roadmap.FindEdge(*p,*n);
Assert(e);
if(*e == NULL) {
//edge data not stored
path.edges.push_back(space->LocalPlanner(roadmap.nodes[*p],roadmap.nodes[*n]));
}
else {
//edge data stored
if((*e)->Start() == roadmap.nodes[*p]) {
//path.edges.push_back((*e)->Copy());
path.edges.push_back(*e);
}
else {
Assert((*e)->Goal() == roadmap.nodes[*p]);
path.edges.push_back((*e)->ReverseCopy());
}
}
}
Assert(path.IsValid());
}
int main(int argc,const char** argv)
{
if(argc <= 3) {
printf("USAGE: ContactPlan [options] world_file configs stance\n");
printf("OPTIONS:\n");
printf("-o filename: the output linear path or multipath (default contactplan.xml)\n");
printf("-p settings: set the planner configuration file\n");
printf("-opt: do optimal planning (do not terminate on the first found solution)\n");
printf("-n iters: set the default number of iterations per step (default 1000)\n");
printf("-t time: set the planning time limit (default infinity)\n");
printf("-m margin: set support polygon margin (default 0)\n");
printf("-r robotindex: set the robot index (default 0)\n");
return 0;
}
Srand(time(NULL));
int robot = 0;
const char* outputfile = "contactplan.xml";
HaltingCondition termCond;
string plannerSettings;
int i;
//parse command line arguments
for(i=1;i<argc;i++) {
if(argv[i][0]=='-') {
if(0==strcmp(argv[i],"-n")) {
termCond.maxIters = atoi(argv[i+1]);
i++;
}
else if(0==strcmp(argv[i],"-t")) {
termCond.timeLimit = atof(argv[i+1]);
i++;
}
else if(0==strcmp(argv[i],"-opt")) {
termCond.foundSolution = false;
}
else if(0==strcmp(argv[i],"-p")) {
if(!GetFileContents(argv[i+1],plannerSettings)) {
printf("Unable to load planner settings file %s\n",argv[i+1]);
return 1;
}
i++;
}
else if(0==strcmp(argv[i],"-r")) {
robot = atoi(argv[i+1]);
i++;
}
else if(0==strcmp(argv[i],"-m")) {
gSupportPolygonMargin = atof(argv[i+1]);
i++;
}
else if(0==strcmp(argv[i],"-o")) {
outputfile = argv[i+1];
i++;
}
else {
printf("Invalid option %s\n",argv[i]);
return 1;
}
}
else break;
}
if(i+3 < argc) {
printf("USAGE: ContactPlan [options] world_file configs stance\n");
return 1;
}
if(i+3 > argc) {
printf("Warning: extra arguments provided\n");
}
const char* worldfile = argv[i];
const char* configsfile = argv[i+1];
const char* stancefile = argv[i+2];
//Read in the world file
XmlWorld xmlWorld;
RobotWorld world;
if(!xmlWorld.Load(worldfile)) {
printf("Error loading world XML file %s\n",worldfile);
return 1;
}
if(!xmlWorld.GetWorld(world)) {
printf("Error loading world file %s\n",worldfile);
return 1;
}
vector<Config> configs;
{
//Read in the configurations specified in configsfile
ifstream in(configsfile);
if(!in) {
printf("Error opening configs file %s\n",configsfile);
return false;
}
while(in) {
Config temp;
in >> temp;
if(in) configs.push_back(temp);
}
if(configs.size() < 2) {
printf("Configs file does not contain 2 or more configs\n");
return 1;
}
in.close();
}
Stance stance;
{
//read in the stance specified by stancefile
ifstream in(stancefile,ios::in);
in >> stance;
if(!in) {
printf("Error loading stance file %s\n",stancefile);
return 1;
}
in.close();
}
//If the stance has no contacts, use ContactPlan. Otherwise, use StancePlan
bool ignoreContactForces = false;
if(NumContactPoints(stance)==0) {
printf("No contact points in stance, planning without stability constraints\n");
ignoreContactForces = true;
}
//set up the command line, store it into the MultiPath settings
string cmdline;
cmdline = argv[0];
for(int i=1;i<argc;i++) {
cmdline += " ";
cmdline += argv[i];
}
MultiPath path;
path.settings["robot"] = world.robots[robot].name;
path.settings["command"] = cmdline;
//begin planning
bool feasible = true;
Config qstart = world.robots[robot].robot->q;
for(size_t i=0;i+1<configs.size();i++) {
MilestonePath mpath;
bool res = false;
if(ignoreContactForces)
res = ContactPlan(world,robot,configs[i],configs[i+1],stance,mpath,termCond,plannerSettings);
else
res = StancePlan(world,robot,configs[i],configs[i+1],stance,mpath,termCond,plannerSettings);
if(!res) {
printf("Planning from stance %d to %d failed\n",i,i+1);
path.sections.resize(path.sections.size()+1);
path.SetStance(stance,path.sections.size()-1);
path.sections.back().milestones[0] = configs[i];
path.sections.back().milestones[1] = configs[i+1];
break;
}
else {
path.sections.resize(path.sections.size()+1);
path.sections.back().milestones.resize(mpath.NumMilestones());
path.SetStance(stance,path.sections.size()-1);
for(int j=0;j<mpath.NumMilestones();j++)
path.sections.back().milestones[j] = mpath.GetMilestone(j);
qstart = path.sections.back().milestones.back();
}
}
if(feasible)
printf("Path planning success! Saving to %s\n",outputfile);
else
printf("Path planning failure. Saving placeholder path to %s\n",outputfile);
const char* ext = FileExtension(outputfile);
if(ext && 0==strcmp(ext,"path")) {
printf("Converted to linear path format\n");
LinearPath lpath;
Convert(path,lpath);
ofstream f(outputfile,ios::out);
lpath.Save(f);
f.close();
}
else
path.Save(outputfile);
return 0;
}
