virtual bool Initialize()
{
drawCommanded = 0;
drawDesired = 1;
drawPath = 0;
drawUI = 1;
drawContacts = 1;
robotInterface = new DefaultMotionQueueInterface(GetMotionQueue(sim.robotControllers[0]));
CopyWorld(*world,planningWorld);
Robot* robot = planningWorld.robots[0];
for(size_t i=0;i<robot->geometry.size();i++)
robot->geometry[i]->margin += collisionMargin;
plannerSettings.InitializeDefault(planningWorld);
uis.resize(0);
#ifndef MULTITHREADED
uis.push_back(new IKPlannerCommandInterface);
uis.push_back(new RRTCommandInterface);
#else
uis.push_back(new MTIKPlannerCommandInterface);
uis.push_back(new MTRRTCommandInterface);
#endif //WIN32
for(size_t i=0;i<uis.size();i++) {
uis[i]->world = world;
uis[i]->robotInterface = robotInterface;
uis[i]->viewport = &viewport;
uis[i]->planningWorld = &planningWorld;
uis[i]->settings = &plannerSettings;
dynamic_cast<InputProcessingInterface*>((RobotUserInterface*)uis[i])->SetProcessor(serialInputProcessor);
}
currentUI = oldUI = 0;
//activate current UI
string res=uis[currentUI]->ActivateEvent(true);
if(!WorldViewProgram::Initialize()) return false;
glui = GLUI_Master.create_glui_subwindow(main_window,GLUI_SUBWINDOW_RIGHT);
glui->set_main_gfx_window(main_window);
glui->add_button("Simulate",SIMULATE_BUTTON_ID,ControlFunc);
glui->add_button("Reset",RESET_BUTTON_ID,ControlFunc);
ui_listbox = glui->add_listbox("UI",¤tUI,UI_LISTBOX_ID,ControlFunc);
for(size_t i=0;i<uis.size();i++) {
char buf[256];
strcpy(buf,uis[i]->Description().c_str());
ui_listbox->add_item(i,buf);
}
glui->add_checkbox("Draw commanded",&drawCommanded);
glui->add_checkbox("Draw desired",&drawDesired);
glui->add_checkbox("Draw UI",&drawUI);
glui->add_checkbox("Draw path",&drawPath);
glui->add_checkbox("Draw contacts",&drawContacts);
printf("Done initializing...\n");
return true;
}
void Start()
{
Assert(sim.robotControllers[0] != NULL);
Assert(sim.robotControllers[0]->command != NULL);
Assert(sim.robotControllers[0]->sensors != NULL);
//choose and set the collision avoidance margin
collisionMargin = 0.0;
CopyWorld(*world,planningWorld);
Robot* robot = planningWorld.robots[0].robot;
for(size_t i=0;i<robot->geometry.size();i++) {
robot->geometry[i].margin += collisionMargin;
}
settings.InitializeDefault(planningWorld);
drawCommanded = 0;
drawDesired = 1;
drawPath = 0;
drawUI = 1;
drawContacts = 1;
MapButtonToggle("draw_commanded",&drawCommanded);
MapButtonToggle("draw_desired",&drawDesired);
MapButtonToggle("draw_path",&drawPath);
MapButtonToggle("draw_ui",&drawUI);
MapButtonToggle("draw_contacts",&drawContacts);
///Hack to initialize motion queue before the planner tries to get a hold of it
sim.robotControllers[0]->Update(0);
//set up user interface
robotInterface = new DefaultMotionQueueInterface(GetMotionQueue(sim.robotControllers[0]));
#ifdef MULTITHREADED
printf("Constructing multi-threaded RRT user interface...\n");
ui = new MTRRTCommandInterface;
#else
printf("Constructing single-threaded RRT user interface...\n");
ui = new RRTCommandInterface;
#endif //WIN32
ui->world = world;
ui->settings = &settings;
ui->planningWorld = &planningWorld;
ui->robotInterface = robotInterface;
ui->viewport = &viewport;
inputProcessor = new MyInputProcessor;
ui->SetProcessor(inputProcessor);
//activate current UI
string res=ui->ActivateEvent(true);
//enable simulation (this flag is in SimulationGUI)
simulate=1;
SimGUIBackend::Start();
}
/** @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();
}