// Parse a single particle def
void ParticlesManager::parseParticleDef(parser::DefTokeniser& tok) {
// Standard DEF, starts with "particle <name> {"
tok.assertNextToken("particle");
std::string name = tok.nextToken();
tok.assertNextToken("{");
ParticleDef pdef(name);
// Any global keywords will come first, after which we get a series of
// brace-delimited stages.
std::string token = tok.nextToken();
while (token != "}") {
if (token == "depthHack") {
tok.skipTokens(1); // we don't care about depthHack
}
else if (token == "{") {
// Parse stage
ParticleStage stage(parseParticleStage(tok));
// Append to the ParticleDef
pdef.appendStage(stage);
}
// Get next token
token = tok.nextToken();
}
// Add the ParticleDef to the map
_particleDefs.insert(ParticleDefMap::value_type(name, pdef));
}
static void set_object_argument(app_PluginRef &plugin, const std::string &struct_name) {
app_PluginObjectInputRef pdef(plugin.get_grt());
pdef->objectStructName(struct_name);
pdef->owner(plugin);
plugin->inputValues().insert(pdef);
}
bool start_planning()
{
//std::vector<double> start_Config;
//std::vector<double> goal_Config;
ob::PathPtr path;
//Construct the robot state space
ob::StateSpacePtr r_space(new ob::RealVectorStateSpace(motoman_arm_DOF)) ;
ob::RealVectorBounds Joint_bounds(motoman_arm_DOF) ;
//Joint bounds
setStateSpaceLimits(Joint_bounds) ;
r_space->as<ob::RealVectorStateSpace>()->setBounds(Joint_bounds);
//Setup sample space
ob::SpaceInformationPtr sample_si(new ob::SpaceInformation(r_space)) ;
//Setup Validity Checker
ValidityChecker checker(sample_si) ;
ompl::base::StateValidityCheckerPtr validity_checker(&checker, dealocate_StateValidityChecker_fn);
sample_si->setStateValidityChecker(validity_checker);
sample_si->setStateValidityCheckingResolution(0.03); // 3%
sample_si->setup();
//Set start and end state
ob::ScopedState<> start_state = SetStateConfig(start_Config,sample_si) ;
start_state.print(std::cout) ;
ob::ScopedState<> goal_state= SetStateConfig(goal_Config,sample_si) ;
goal_state.print(std::cout) ;
//Create Problem Definition
ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(sample_si)) ;
pdef->setStartAndGoalStates(start_state,goal_state) ;
//Make the planner
ob::PlannerPtr planner(new og::RRTstar(sample_si)) ;
planner->setProblemDefinition(pdef) ;
planner->setup() ;
//Planning
ob::PlannerStatus solved = planner->solve(planning_time) ;
if(solved)
{
//Generate the path
path = pdef->getSolutionPath() ;
std::cout<< "Path Found" << std::endl ;
path->print(std::cout) ;
return true ;
}
else
{
std::cout << "Can't find solution" << std::endl ;
return false ;
}
}
void RRTstar_planning(){
ROS_INFO("RRT star planning");
ob::StateSpacePtr space(new ob::RealVectorStateSpace(2));
space->as<ob::RealVectorStateSpace>()->setBounds(minX(local_map->info), maxX(local_map->info));
space->setLongestValidSegmentFraction(0.01/(maxX(local_map->info)-minX(local_map->info)));
ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
si->setup();
ob::ScopedState<> start(space);
start->as<ob::RealVectorStateSpace::StateType>()->values[0] = startState[0];
start->as<ob::RealVectorStateSpace::StateType>()->values[1] = startState[1];
ob::ScopedState<> goal(space);
goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = goalState[0];
goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = goalState[1];
ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
pdef->setStartAndGoalStates(start, goal);
pdef->setOptimizationObjective(ob::OptimizationObjectivePtr(new ClearanceObjective(si)));
og::RRTstar *plan_pt=new og::RRTstar(si);
plan_pt->setGoalBias(0.05);
plan_pt->setRange(1.0);
ob::PlannerPtr optimizingPlanner(plan_pt);
optimizingPlanner->setProblemDefinition(pdef);
optimizingPlanner->setup();
ob::PlannerStatus solved;
int it=0;
while(solved!=ompl::base::PlannerStatus::StatusType::EXACT_SOLUTION && it<100){
it++;
solved = optimizingPlanner->solve(1.0);
}
if(solved==ompl::base::PlannerStatus::StatusType::EXACT_SOLUTION){
std::vector< ob::State * > sol = boost::static_pointer_cast<og::PathGeometric>(pdef->getSolutionPath())->getStates();
current_path_raw.resize(sol.size());
std::transform(sol.begin(), sol.end(), current_path_raw.begin(), &obstateState);
/*std::cout << "Path length: " << pdef->getSolutionPath()->length() << std::endl;
std::cout << "Path cost: " << pdef->getSolutionPath()->cost(pdef->getOptimizationObjective()) << std::endl;
std::cout << "Path :" << std::endl;
for(std::deque<State<2>>::iterator it=current_path_raw.begin(),end=current_path_raw.end();it!=end;++it){
std::cout << (*it) << " -- ";
}
std::cout << std::endl;*/
planned=true;
}else{
ROS_INFO("Planning failed");
planned=false;
}
}
/**
* @brief Initializer for 3D points (translation only)
*
* @param sampler ...
* @param limits ...
* @return void
*/
void PlannerOMPL::initialize(Sampler *sampler)
{
QList<QPair<float,float> > limits = sampler->getLimits();
qDebug() << __FUNCTION__ << limits;
assert(limits.size() == 3);
//Create state space as R3
ob::RealVectorStateSpace *space = new ob::RealVectorStateSpace();
space->addDimension("X", limits[0].first, limits[0].second);
space->addDimension("Y", limits[1].first, limits[1].second);
space->addDimension("Z", limits[2].first, limits[2].second);
// qDebug() << space->getBounds().low[0] << space->getBounds().high[0] ;
// qDebug() << space->getBounds().low[1] << space->getBounds().high[1] ;
// qDebug() << space->getBounds().low[2] << space->getBounds().high[2] ;
//Setup class
simpleSetUp.reset(new og::SimpleSetup(ob::StateSpacePtr(space)));
//set Sampler
simpleSetUp->getSpaceInformation()->setValidStateSamplerAllocator(allocOBValidStateSampler);
simpleSetUp->setStateValidityChecker(boost::bind(&Sampler::isStateValid, sampler, _1));
ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(simpleSetUp->getSpaceInformation()));
pdef->setOptimizationObjective(getPathLengthObjective(simpleSetUp->getSpaceInformation()));
space->setup();
simpleSetUp->getSpaceInformation()->setStateValidityCheckingResolution(0.1);
//simpleSetUp->getSpaceInformation()->setStateValidityCheckingResolution(100 / space->getMaximumExtent());
//simpleSetUp->setPlanner(ob::PlannerPtr(new og::RRTConnect(simpleSetUp->getSpaceInformation())));
simpleSetUp->setPlanner(ob::PlannerPtr(new og::RRTstar(simpleSetUp->getSpaceInformation())));
simpleSetUp->getPlanner()->as<og::RRTstar>()->setProblemDefinition(pdef);
//simpleSetUp->getPlanner()->as<og::RRTConnect>()->setRange(2000);
simpleSetUp->getPlanner()->as<og::RRTConnect>()->setRange(0.5);
}
void plan(double runTime, optimalPlanner plannerType, planningObjective objectiveType, std::string outputFile)
{
// Construct the robot state space in which we're planning. We're
// planning in [0,1]x[0,1], a subset of R^2.
ob::StateSpacePtr space(new ob::RealVectorStateSpace(2));
// Set the bounds of space to be in [0,1].
space->as<ob::RealVectorStateSpace>()->setBounds(0.0, 1.0);
// Construct a space information instance for this state space
ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
// Set the object used to check which states in the space are valid
si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
si->setup();
// Set our robot's starting state to be the bottom-left corner of
// the environment, or (0,0).
ob::ScopedState<> start(space);
start->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
start->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.0;
// Set our robot's goal state to be the top-right corner of the
// environment, or (1,1).
ob::ScopedState<> goal(space);
goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = 1.0;
goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = 1.0;
// Create a problem instance
ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
// Set the start and goal states
pdef->setStartAndGoalStates(start, goal);
// Create the optimization objective specified by our command-line argument.
// This helper function is simply a switch statement.
pdef->setOptimizationObjective(allocateObjective(si, objectiveType));
// Construct the optimal planner specified by our command line argument.
// This helper function is simply a switch statement.
ob::PlannerPtr optimizingPlanner = allocatePlanner(si, plannerType);
// Set the problem instance for our planner to solve
optimizingPlanner->setProblemDefinition(pdef);
optimizingPlanner->setup();
// attempt to solve the planning problem in the given runtime
ob::PlannerStatus solved = optimizingPlanner->solve(runTime);
if (solved)
{
// Output the length of the path found
std::cout
<< optimizingPlanner->getName()
<< " found a solution of length "
<< pdef->getSolutionPath()->length()
<< " with an optimization objective value of "
<< pdef->getSolutionPath()->cost(pdef->getOptimizationObjective()) << std::endl;
// If a filename was specified, output the path as a matrix to
// that file for visualization
if (!outputFile.empty())
{
std::ofstream outFile(outputFile.c_str());
std::static_pointer_cast<og::PathGeometric>(pdef->getSolutionPath())->
printAsMatrix(outFile);
outFile.close();
}
}
else
std::cout << "No solution found." << std::endl;
}
void plan(void)
{
// construct the state space we are planning in
ob::StateSpacePtr space(new ob::SE2StateSpace());
// set the bounds for the R^2 part of SE(2)
ob::RealVectorBounds bounds(2);
bounds.setLow(0);
bounds.setHigh(2);
space->as<ob::SE2StateSpace>()->setBounds(bounds);
// create triangulation that ignores obstacle and respects propositions
MyDecomposition* ptd = new MyDecomposition(bounds);
// helper method that adds an obstacle, as well as three propositions p0,p1,p2
addObstaclesAndPropositions(ptd);
ptd->setup();
oc::PropositionalDecompositionPtr pd(ptd);
// create a control space
oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 2));
// set the bounds for the control space
ob::RealVectorBounds cbounds(2);
cbounds.setLow(-.5);
cbounds.setHigh(.5);
cspace->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
oc::SpaceInformationPtr si(new oc::SpaceInformation(space, cspace));
si->setStateValidityChecker(std::bind(&isStateValid, si.get(), ptd, std::placeholders::_1));
si->setStatePropagator(std::bind(&propagate, std::placeholders::_1,
std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
si->setPropagationStepSize(0.025);
//LTL co-safety sequencing formula: visit p2,p0 in that order
std::vector<unsigned int> sequence(2);
sequence[0] = 2;
sequence[1] = 0;
oc::AutomatonPtr cosafety = oc::Automaton::SequenceAutomaton(3, sequence);
//LTL safety avoidance formula: never visit p1
std::vector<unsigned int> toAvoid(1);
toAvoid[0] = 1;
oc::AutomatonPtr safety = oc::Automaton::AvoidanceAutomaton(3, toAvoid);
//construct product graph (propDecomp x A_{cosafety} x A_{safety})
oc::ProductGraphPtr product(new oc::ProductGraph(pd, cosafety, safety));
// LTLSpaceInformation creates a hybrid space of robot state space x product graph.
// It takes the validity checker from SpaceInformation and expands it to one that also
// rejects any hybrid state containing rejecting automaton states.
// It takes the state propagator from SpaceInformation and expands it to one that
// follows continuous propagation with setting the next decomposition region
// and automaton states accordingly.
//
// The robot state space, given by SpaceInformation, is referred to as the "lower space".
oc::LTLSpaceInformationPtr ltlsi(new oc::LTLSpaceInformation(si, product));
// LTLProblemDefinition creates a goal in hybrid space, corresponding to any
// state in which both automata are accepting
oc::LTLProblemDefinitionPtr pdef(new oc::LTLProblemDefinition(ltlsi));
// create a start state
ob::ScopedState<ob::SE2StateSpace> start(space);
start->setX(0.2);
start->setY(0.2);
start->setYaw(0.0);
// addLowerStartState accepts a state in lower space, expands it to its
// corresponding hybrid state (decomposition region containing the state, and
// starting states in both automata), and adds that as an official start state.
pdef->addLowerStartState(start.get());
//LTL planner (input: LTL space information, product automaton)
oc::LTLPlanner* ltlPlanner = new oc::LTLPlanner(ltlsi, product);
ltlPlanner->setProblemDefinition(pdef);
// attempt to solve the problem within thirty seconds of planning time
// considering the above cosafety/safety automata, a solution path is any
// path that visits p2 followed by p0 while avoiding obstacles and avoiding p1.
ob::PlannerStatus solved = ltlPlanner->as<ob::Planner>()->solve(30.0);
if (solved)
{
std::cout << "Found solution:" << std::endl;
// The path returned by LTLProblemDefinition is through hybrid space.
// getLowerSolutionPath() projects it down into the original robot state space
// that we handed to LTLSpaceInformation.
pdef->getLowerSolutionPath()->print(std::cout);
}
else
std::cout << "No solution found" << std::endl;
delete ltlPlanner;
}
void plan(void)
{
// construct the state space we are planning in
ob::StateSpacePtr space(new ob::SE3StateSpace());
// set the bounds for the R^3 part of SE(3)
ob::RealVectorBounds bounds(3);
bounds.setLow(-1);
bounds.setHigh(1);
space->as<ob::SE3StateSpace>()->setBounds(bounds);
// construct an instance of space information from this state space
ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
// set state validity checking for this space
si->setStateValidityChecker(std::bind(&isStateValid, std::placeholders::_1));
// create a random start state
ob::ScopedState<> start(space);
start.random();
// create a random goal state
ob::ScopedState<> goal(space);
goal.random();
// create a problem instance
ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
// set the start and goal states
pdef->setStartAndGoalStates(start, goal);
// create a planner for the defined space
ob::PlannerPtr planner(new og::RRTConnect(si));
// set the problem we are trying to solve for the planner
planner->setProblemDefinition(pdef);
// perform setup steps for the planner
planner->setup();
// print the settings for this space
si->printSettings(std::cout);
// print the problem settings
pdef->print(std::cout);
// attempt to solve the problem within one second of planning time
ob::PlannerStatus solved = planner->solve(1.0);
if (solved)
{
// get the goal representation from the problem definition (not the same as the goal state)
// and inquire about the found path
ob::PathPtr path = pdef->getSolutionPath();
std::cout << "Found solution:" << std::endl;
// print the path to screen
path->print(std::cout);
}
else
std::cout << "No solution found" << std::endl;
}
void plan(int argc, char** argv)
{
// Construct the robot state space in which we're planning. We're
// planning in [0,1]x[0,1], a subset of R^2.
ob::StateSpacePtr space(new ob::RealVectorStateSpace(2));
// Set the bounds of space to be in [0,1].
space->as<ob::RealVectorStateSpace>()->setBounds(0.0, 1.0);
// Construct a space information instance for this state space
ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
// Set the object used to check which states in the space are valid
si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
si->setup();
// Set our robot's starting state to be the bottom-left corner of
// the environment, or (0,0).
ob::ScopedState<> start(space);
start->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
start->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.0;
// Set our robot's goal state to be the top-right corner of the
// environment, or (1,1).
ob::ScopedState<> goal(space);
goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = 1.0;
goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = 1.0;
// Create a problem instance
ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
// Set the start and goal states
pdef->setStartAndGoalStates(start, goal);
// Since we want to find an optimal plan, we need to define what
// is optimal with an OptimizationObjective structure. Un-comment
// exactly one of the following 6 lines to see some examples of
// optimization objectives.
pdef->setOptimizationObjective(getPathLengthObjective(si));
// pdef->setOptimizationObjective(getThresholdPathLengthObj(si));
// pdef->setOptimizationObjective(getClearanceObjective(si));
// pdef->setOptimizationObjective(getBalancedObjective1(si));
// pdef->setOptimizationObjective(getBalancedObjective2(si));
// pdef->setOptimizationObjective(getPathLengthObjWithCostToGo(si));
// Construct our optimal planner using the RRTstarAR algorithm.
ob::PlannerPtr optimizingPlanner(new og::RRTstarAR(si));
// Set the problem instance for our planner to solve
optimizingPlanner->setProblemDefinition(pdef);
optimizingPlanner->setup();
// attempt to solve the planning problem within one second of
// planning time
ob::PlannerStatus solved = optimizingPlanner->solve(1.0);
if (solved)
{
// Output the length of the path found
std::cout
<< "Found solution of path length "
<< pdef->getSolutionPath()->length()
<< " with an optimization objective value of "
<< pdef->getSolutionPath()->cost(pdef->getOptimizationObjective()) << std::endl;
// If a filename was specified, output the path as a matrix to
// that file for visualization
if (argc > 1)
{
std::ofstream outFile(argv[1]);
boost::static_pointer_cast<og::PathGeometric>(pdef->getSolutionPath())->
printAsMatrix(outFile);
outFile.close();
}
}
else
std::cout << "No solution found." << std::endl;
}
