38 #include <ompl/base/spaces/SE2StateSpace.h>
39 #include <ompl/base/spaces/RealVectorStateSpace.h>
40 #include <ompl/base/SpaceInformation.h>
41 #include <ompl/base/StateSpace.h>
42 #include <ompl/geometric/planners/quotientspace/QRRT.h>
44 #include <boost/math/constants/constants.hpp>
52 bool boxConstraint(
const double values[])
54 const double &x = values[0] - 0.5;
55 const double &y = values[1] - 0.5;
56 double pos_cnstr = sqrt(x * x + y * y);
57 return (pos_cnstr > 0.2);
59 bool isStateValid_SE2(
const ob::State *state)
64 return boxConstraint(R2->values) && (SO2->value < boost::math::constants::pi<double>() / 2.0);
66 bool isStateValid_R2(
const ob::State *state)
69 return boxConstraint(R2->values);
75 auto SE2(std::make_shared<ob::SE2StateSpace>());
79 SE2->setBounds(bounds);
80 ob::SpaceInformationPtr si_SE2(std::make_shared<ob::SpaceInformation>(SE2));
81 si_SE2->setStateValidityChecker(isStateValid_SE2);
84 auto R2(std::make_shared<ob::RealVectorStateSpace>(2));
86 ob::SpaceInformationPtr si_R2(std::make_shared<ob::SpaceInformation>(R2));
87 si_R2->setStateValidityChecker(isStateValid_R2);
90 std::vector<ob::SpaceInformationPtr> si_vec;
91 si_vec.push_back(si_R2);
92 si_vec.push_back(si_SE2);
96 SE2State start_SE2(SE2);
97 SE2State goal_SE2(SE2);
98 start_SE2->setXY(0, 0);
100 goal_SE2->setXY(1, 1);
103 ob::ProblemDefinitionPtr pdef = std::make_shared<ob::ProblemDefinition>(si_SE2);
104 pdef->setStartAndGoalStates(start_SE2, goal_SE2);
107 auto planner = std::make_shared<og::QRRT>(si_vec);
110 planner->setProblemDefinition(pdef);
117 std::cout << std::string(80,
'-') << std::endl;
118 std::cout <<
"Configuration-Space Path (SE2):" << std::endl;
119 std::cout << std::string(80,
'-') << std::endl;
120 pdef->getSolutionPath()->print(std::cout);
122 std::cout << std::string(80,
'-') << std::endl;
123 std::cout <<
"Quotient-Space Path (R2):" << std::endl;
124 std::cout << std::string(80,
'-') << std::endl;
125 const ob::ProblemDefinitionPtr pdefR2 = planner->getProblemDefinition(0);
126 pdefR2->getSolutionPath()->print(std::cout);
128 std::vector<int> nodes = planner->getFeasibleNodes();
129 std::cout << std::string(80,
'-') << std::endl;
130 for (
unsigned int k = 0; k < nodes.size(); k++)
132 std::cout <<
"QuotientSpace" << k <<
" has " << nodes.at(k) <<
" nodes." << std::endl;
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
The lower and upper bounds for an Rn space.
Definition of a scoped state.
ompl::base::State StateType
Define the type of state allocated by this space.
Definition of an abstract state.
const T * as() const
Cast this instance to a desired type.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains code that is specific to planning under geometric constraints.
A class to store the exit status of Planner::solve()