// 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; }