37#include "ConstrainedPlanningCommon.h"
42 SphereConstraint() : ob::Constraint(3, 1)
46 void function(
const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out)
const override
48 out[0] = x.norm() - 1;
51 void jacobian(
const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out)
const override
53 out = x.transpose().normalized();
60 SphereProjection(
const ob::StateSpacePtr &space) : ob::ProjectionEvaluator(space)
64 unsigned int getDimension()
const override
69 void defaultCellSizes()
override
76 void project(
const ob::State *state, Eigen::Ref<Eigen::VectorXd> projection)
const override
79 projection(0) = atan2(x[1], x[0]);
80 projection(1) = acos(x[2]);
88 if (-0.80 < x[2] && x[2] < -0.6)
90 if (-0.05 < x[1] && x[1] < 0.05)
94 else if (-0.1 < x[2] && x[2] < 0.1)
96 if (-0.05 < x[0] && x[0] < 0.05)
100 else if (0.6 < x[2] && x[2] < 0.80)
102 if (-0.05 < x[1] && x[1] < 0.05)
110bool spherePlanningOnce(
ConstrainedProblem &cp,
enum PLANNER_TYPE planner,
bool output)
112 cp.setPlanner(planner,
"sphere");
119 OMPL_INFORM(
"Dumping problem information to `sphere_info.txt`.");
120 std::ofstream infofile(
"sphere_info.txt");
121 infofile << cp.type << std::endl;
128 cp.dumpGraph(
"sphere");
133bool spherePlanningBench(
ConstrainedProblem &cp, std::vector<enum PLANNER_TYPE> &planners)
135 cp.setupBenchmark(planners,
"sphere");
140bool spherePlanning(
bool output,
enum SPACE_TYPE space, std::vector<enum PLANNER_TYPE> &planners,
144 auto rvss = std::make_shared<ob::RealVectorStateSpace>(3);
150 rvss->setBounds(bounds);
153 auto constraint = std::make_shared<SphereConstraint>();
156 cp.setConstrainedOptions(c_opt);
157 cp.setAtlasOptions(a_opt);
159 cp.css->registerProjection(
"sphere", std::make_shared<SphereProjection>(cp.css));
161 Eigen::VectorXd start(3), goal(3);
165 cp.setStartAndGoalStates(start, goal);
166 cp.ss->setStateValidityChecker(obstacles);
169 return spherePlanningOnce(cp, planners[0], output);
171 return spherePlanningBench(cp, planners);
174auto help_msg =
"Shows this help message.";
175auto output_msg =
"Dump found solution path (if one exists) in plain text and planning graph in GraphML to "
176 "`sphere_path.txt` and `sphere_graph.graphml` respectively.";
177auto bench_msg =
"Do benchmarking on provided planner list.";
179int main(
int argc,
char **argv)
182 enum SPACE_TYPE space = PJ;
183 std::vector<enum PLANNER_TYPE> planners = {RRT};
188 po::options_description desc(
"Options");
189 desc.add_options()(
"help,h", help_msg);
190 desc.add_options()(
"output,o", po::bool_switch(&output)->default_value(
false), output_msg);
191 desc.add_options()(
"bench", po::bool_switch(&bench)->default_value(
false), bench_msg);
193 addSpaceOption(desc, &space);
194 addPlannerOption(desc, &planners);
195 addConstrainedOptions(desc, &c_opt);
196 addAtlasOptions(desc, &a_opt);
198 po::variables_map vm;
199 po::store(po::parse_command_line(argc, argv, desc), vm);
202 if (vm.count(
"help") != 0u)
204 std::cout << desc << std::endl;
208 return spherePlanning(output, space, planners, c_opt, a_opt, bench);
A State in a ConstrainedStateSpace, represented as a dense real vector of values. For convenience and...
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
std::vector< double > cellSizes_
The size of a cell, in every dimension of the projected space, in the implicitly defined integer grid...
The lower and upper bounds for an Rn space.
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.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
A class to store the exit status of Planner::solve()