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/multilevel/planners/qrrt/QRRT.h>
45#include <boost/math/constants/constants.hpp>
52const double pi = boost::math::constants::pi<double>();
56bool isInCollision(
double *val)
58 const double &x = val[0] - 0.5;
59 const double &y = val[1] - 0.5;
60 const double &z = val[2] - 0.5;
61 double d = sqrt(x * x + y * y + z * z);
65bool isStateValid_SE3(
const State *state)
67 static auto SO3(std::make_shared<SO3StateSpace>());
68 static SO3State SO3id(SO3);
76 double d = SO3->distance(SO3state, SO3stateIdentity);
77 return isInCollision(R3state->values) && (d < pi / 4.0);
80bool isStateValid_R3(
const State *state)
83 return isInCollision(R3->values);
92 auto SE3(std::make_shared<SE3StateSpace>());
96 SE3->setBounds(bounds);
98 si_SE3->setStateValidityChecker(isStateValid_SE3);
101 auto R3(std::make_shared<RealVectorStateSpace>(3));
104 si_R3->setStateValidityChecker(isStateValid_R3);
107 std::vector<SpaceInformationPtr> si_vec;
108 si_vec.push_back(si_R3);
109 si_vec.push_back(si_SE3);
112 SE3State start_SE3(SE3);
113 SE3State goal_SE3(SE3);
114 start_SE3->setXYZ(0, 0, 0);
115 start_SE3->rotation().setIdentity();
116 goal_SE3->setXYZ(1, 1, 1);
117 goal_SE3->rotation().setIdentity();
120 pdef->setStartAndGoalStates(start_SE3, goal_SE3);
126 auto planner = std::make_shared<ompl::multilevel::QRRT>(si_vec);
129 planner->setProblemDefinition(pdef);
136 std::cout << std::string(80,
'-') << std::endl;
137 std::cout <<
"Bundle-Space Path (SE3):" << std::endl;
138 std::cout << std::string(80,
'-') << std::endl;
139 pdef->getSolutionPath()->print(std::cout);
141 std::cout << std::string(80,
'-') << std::endl;
142 std::cout <<
"Base-Space Path (R3) :" << std::endl;
143 std::cout << std::string(80,
'-') << std::endl;
145 pdefR3->getSolutionPath()->print(std::cout);
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
A shared pointer wrapper for ompl::base::ProblemDefinition.
The lower and upper bounds for an Rn space.
The definition of a state in Rn
A state in SE(3): position = (x, y, z), quaternion = (x, y, z, w)
The definition of a state in SO(3) represented as a unit quaternion.
Definition of a scoped state.
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...
A class to store the exit status of Planner::solve()