38 #include <ompl/base/spaces/SE3StateSpace.h>
39 #include <ompl/base/spaces/SO3StateSpace.h>
40 #include <ompl/base/spaces/RealVectorStateSpace.h>
41 #include <ompl/base/SpaceInformation.h>
42 #include <ompl/base/StateSpace.h>
43 #include <ompl/geometric/planners/quotientspace/QRRT.h>
45 #include <boost/math/constants/constants.hpp>
52 const double pi = boost::math::constants::pi<double>();
57 bool isInCollision(
double *val)
59 const double &x = val[0] - 0.5;
60 const double &y = val[1] - 0.5;
61 const double &z = val[2] - 0.5;
62 double d = sqrt(x * x + y * y + z * z);
66 bool isStateValid_SE3(
const ob::State *state)
68 static auto SO3(std::make_shared<ob::SO3StateSpace>());
77 double d = SO3->distance(SO3state, SO3stateIdentity);
78 return isInCollision(R3state->values) && (d < pi / 4.0);
81 bool isStateValid_R3(
const ob::State *state)
84 return isInCollision(R3->values);
93 auto SE3(std::make_shared<ob::SE3StateSpace>());
97 SE3->setBounds(bounds);
98 ob::SpaceInformationPtr si_SE3(std::make_shared<ob::SpaceInformation>(SE3));
99 si_SE3->setStateValidityChecker(isStateValid_SE3);
102 auto R3(std::make_shared<ob::RealVectorStateSpace>(3));
104 ob::SpaceInformationPtr si_R3(std::make_shared<ob::SpaceInformation>(R3));
105 si_R3->setStateValidityChecker(isStateValid_R3);
108 std::vector<ob::SpaceInformationPtr> si_vec;
109 si_vec.push_back(si_R3);
110 si_vec.push_back(si_SE3);
115 start_SE3->setXYZ(0, 0, 0);
116 start_SE3->rotation().setIdentity();
117 goal_SE3->setXYZ(1, 1, 1);
118 goal_SE3->rotation().setIdentity();
120 ob::ProblemDefinitionPtr pdef = std::make_shared<ob::ProblemDefinition>(si_SE3);
121 pdef->setStartAndGoalStates(start_SE3, goal_SE3);
127 auto planner = std::make_shared<og::QRRT>(si_vec);
130 planner->setProblemDefinition(pdef);
137 std::cout << std::string(80,
'-') << std::endl;
138 std::cout <<
"Configuration-Space Path (SE3):" << std::endl;
139 std::cout << std::string(80,
'-') << std::endl;
140 pdef->getSolutionPath()->print(std::cout);
142 std::cout << std::string(80,
'-') << std::endl;
143 std::cout <<
"Quotient-Space Path (R3):" << std::endl;
144 std::cout << std::string(80,
'-') << std::endl;
145 const ob::ProblemDefinitionPtr pdefR3 = planner->getProblemDefinition(0);
146 pdefR3->getSolutionPath()->print(std::cout);
148 std::vector<int> nodes = planner->getFeasibleNodes();
150 std::cout << std::string(80,
'-') << std::endl;
151 for (
unsigned int k = 0; k < nodes.size(); k++)
153 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.
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
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()