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)
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...
virtual void defaultCellSizes()
Set the default cell dimensions for this projection. The default implementation of this function is e...
virtual unsigned int getDimension() const =0
Return the dimension of the projection defined by this evaluator.
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()