39 #include "boost/program_options.hpp"
40 #include "PolyWorld.h"
41 #include "PlanarManipulatorPolyWorld.h"
42 #include "PlanarManipulator.h"
43 #include "PlanarManipulatorStateSpace.h"
44 #include "PlanarManipulatorStateValidityChecker.h"
45 #include "PlanarManipulatorIKGoal.h"
46 #include "PlanarManipulatorTSRRTConfig.h"
47 #include "PlanarManipulatorXXLDecomposition.h"
49 #include <ompl/geometric/SimpleSetup.h>
52 #include <ompl/geometric/planners/xxl/XXL.h>
53 #include <ompl/geometric/planners/rrt/RRT.h>
54 #include <ompl/geometric/planners/kpiece/KPIECE1.h>
55 #include <ompl/geometric/planners/stride/STRIDE.h>
56 #include <ompl/geometric/planners/rrt/TSRRT.h>
57 #include <ompl/geometric/planners/rlrt/RLRT.h>
60 #include <ompl/geometric/planners/rrt/RRTConnect.h>
61 #include <ompl/geometric/planners/kpiece/BKPIECE1.h>
62 #include <ompl/geometric/planners/rlrt/BiRLRT.h>
64 #include <ompl/tools/benchmark/Benchmark.h>
79 Problem(
int links,
const std::string& problemName,
PolyWorld &&world,
const Eigen::Affine2d &baseFrame,
const Eigen::Affine2d &goalFrame)
80 : name(problemName), manipulator(links, 1.0 / links), world(std::move(world)), goalFrame(goalFrame)
82 manipulator.setBaseFrame(baseFrame);
88 Eigen::Affine2d goalFrame;
92 Problem CreateProblem(
const Arguments &args)
94 if (args.problem ==
"corridor")
96 Eigen::Affine2d baseFrame;
97 Eigen::Affine2d goalFrame;
98 PolyWorld world = createCorridorProblem(args.numLinks, baseFrame, goalFrame);
99 return Problem(args.numLinks, args.problem, std::move(world), baseFrame, goalFrame);
101 else if (args.problem ==
"constricted")
103 Eigen::Affine2d baseFrame;
104 Eigen::Affine2d goalFrame;
105 PolyWorld world = createConstrictedProblem(args.numLinks, baseFrame, goalFrame);
106 return Problem(args.numLinks, args.problem, std::move(world), baseFrame, goalFrame);
117 const unsigned int numLinks = problem.manipulator.getNumLinks();
121 bounds.setLow(-M_PI);
122 bounds.setHigh(M_PI);
126 problem.manipulator.setBounds(bounds.low, bounds.high);
131 setup->setStateValidityChecker(std::make_shared<PlanarManipulatorCollisionChecker>(
132 setup->getSpaceInformation(), problem.manipulator, problem.world));
135 setup->getSpaceInformation()->setStateValidityCheckingResolution(0.001);
140 for (
size_t i = 0; i < numLinks; ++i)
141 start_angles[i] = 1e-7;
142 setup->getProblemDefinition()->addStartState(start);
143 setup->getStateSpace()->freeState(start);
146 &problem.manipulator,
149 setup->setGoal(goal);
157 const auto xBounds = problem.world.xBounds();
158 const auto yBounds = problem.world.yBounds();
161 double xMin = std::max(origin.first - chainLength, xBounds.first);
162 double xMax = std::min(origin.first + chainLength, xBounds.second);
163 double yMin = std::max(origin.second - chainLength, yBounds.first);
164 double yMax = std::min(origin.second + chainLength, yBounds.second);
166 reachable_bounds.setLow(0, xMin);
167 reachable_bounds.setHigh(0, xMax);
168 reachable_bounds.setLow(1, yMin);
169 reachable_bounds.setHigh(1, yMax);
171 return reachable_bounds;
176 ompl::geometric::TaskSpaceConfigPtr getTaskSpaceConfig(
const Problem &problem)
179 unsigned int numLinks = manip.getNumLinks();
180 double linkLength = 1.0 / numLinks;
181 double chainLength = numLinks * linkLength;
183 const Eigen::Affine2d &baseFrame = manip.getBaseFrame();
184 Point origin = {baseFrame.translation()(0), baseFrame.translation()(1)};
189 return task_space_ptr;
200 unsigned int numLinks = manip.getNumLinks();
201 double linkLength = 1.0 / numLinks;
202 double chainLength = numLinks * linkLength;
205 const Eigen::Affine2d &baseFrame = manip.getBaseFrame();
206 Point origin = {baseFrame.translation()(0), baseFrame.translation()(1)};
210 std::vector<int> xySlices(2, numXYSlices);
211 const int thetaSlices = 1;
214 std::vector<int> projLinks;
218 projLinks.push_back((numLinks / 2) - 1);
221 projLinks.push_back(numLinks - 1);
224 si, &manip, reachable_bounds, xySlices, thetaSlices, projLinks,
true));
233 if (!planner->getProblemDefinition()->hasSolution())
236 double cartesianDist = 0.0;
238 *(planner->getProblemDefinition()->getSolutionPath().get()));
239 for (
size_t i = 0; i < path.getStateCount() - 1; ++i)
241 std::vector<Eigen::Affine2d> startFrames, endFrames;
245 for (
size_t j = 1; j < endFrames.size(); ++j)
246 cartesianDist += (endFrames[j].translation() - startFrames[j].translation()).norm();
249 run[
"Cartesian Distance REAL"] = boost::lexical_cast<std::string>(cartesianDist);
264 const int numLinks = problem.manipulator.getNumLinks();
265 const int xySlices = std::max(2, numLinks / 3);
267 getXXLDecomp(setup->getSpaceInformation(), problem, xySlices)));
269 setup->getSpaceInformation(), getXXLDecomp(setup->getSpaceInformation(), problem, 1)));
270 xxl1->setName(
"XXL1");
272 std::string name =
"PlanarManipulator - " + problem.name;
275 benchmark.addPlanner(rrt);
276 benchmark.addPlanner(rrtc);
277 benchmark.addPlanner(rlrt);
278 benchmark.addPlanner(birlrt);
279 benchmark.addPlanner(stride);
280 benchmark.addPlanner(kpiece);
281 benchmark.addPlanner(bkpiece);
282 benchmark.addPlanner(xxl);
283 benchmark.addPlanner(xxl1);
284 benchmark.addPlanner(tsrrt);
287 postRunEvent(planner, run, &problem.manipulator);
289 benchmark.addExperimentParameter(
"num_links",
"INTEGER", boost::lexical_cast<std::string>(numLinks));
290 benchmark.addExperimentParameter(
"cells",
"INTEGER", boost::lexical_cast<std::string>(xySlices));
292 double memoryLimit = 8192.0;
294 benchmark.benchmark(request);
296 benchmark.saveResultsToFile();
301 const int numLinks = problem.manipulator.getNumLinks();
303 const char *world_file =
"world.yaml";
305 problem.world.writeWorld(world_file);
307 const double linkLength = 1.0 / numLinks;
308 const Eigen::Affine2d &basePose = problem.manipulator.getBaseFrame();
310 const char *path_file =
"manipulator_path.txt";
315 fout.open(path_file);
317 fout << numLinks <<
" " << linkLength <<
" " << basePose.translation()(0) <<
" " << basePose.translation()(1) <<
" "
318 << xySlices << std::endl;
320 for (
size_t i = 0; i < path.getStateCount(); ++i)
323 for (
size_t j = 0; j < problem.manipulator.getNumLinks(); ++j)
324 fout << angles[j] <<
" ";
333 const int numLinks = problem.manipulator.getNumLinks();
335 const int xySlices = std::max(2, numLinks / 3);
337 getXXLDecomp(setup->getSpaceInformation(), problem, xySlices)));
338 setup->setPlanner(xxl);
352 WriteVisualization(problem, pgeo, xySlices);
361 void PlanarManipulatorPlanning(
const Arguments &args)
365 Problem problem = CreateProblem(args);
368 if (args.numRuns == 1)
369 SolveProblem(setup, problem, args.timeout, args.viz);
371 BenchmarkProblem(setup, problem, args.numRuns, args.timeout);
374 int main(
int argc,
char **argv)
379 namespace po = boost::program_options;
380 po::options_description desc(
"Allowed options");
383 (
"help,?",
"Show this message")
384 (
"links,l", po::value<int>(&args.numLinks)->default_value(10),
385 "Set the number of links in the chain")
386 (
"runs,r", po::value<int>(&args.numRuns)->default_value(1),
387 "The number of times to execute the query. >1 implies benchmarking")
388 (
"timeout,t", po::value<double>(&args.timeout)->default_value(60.0),
389 "The maximum time (seconds) before failure is declared")
390 (
"problem,p", po::value<std::string>(&args.problem)->default_value(
"corridor"),
391 "The name of the problem [corridor,constricted] to solve")
392 (
"viz,v", po::bool_switch(&args.viz)->default_value(
false),
393 "Write visualization output to disk. Only works when runs = 1");
396 po::variables_map vm;
397 po::store(po::parse_command_line(argc, argv, desc), vm);
400 if (vm.count(
"help"))
402 std::cout << desc << std::endl;
405 PlanarManipulatorPlanning(args);
The exception type for ompl.
A shared pointer wrapper for ompl::base::Goal.
A shared pointer wrapper for ompl::base::Planner.
The lower and upper bounds for an Rn space.
The definition of a state in Rn
A shared pointer wrapper for ompl::base::StateSpace.
Definition of an abstract state.
const T * as() const
Cast this instance to a desired type.
Bi-directional KPIECE with one level of discretization.
Bi-directional Range-Limited Random Tree (Ryan Luna's Random Tree)
Kinematic Planning by Interior-Exterior Cell Exploration.
Definition of a geometric path.
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
void interpolate(unsigned int count)
Insert a number of states in a path so that the path is made up of exactly count states....
Range-Limited Random Tree (Ryan Luna's Random Tree)
Rapidly-exploring Random Trees.
Search Tree with Resolution Independent Density Estimation.
A shared pointer wrapper for ompl::geometric::SimpleSetup.
Create the set of classes typically needed to solve a geometric problem.
Task-space Rapidly-exploring Random Trees.
A shared pointer wrapper for ompl::geometric::XXLDecomposition.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
void setLogLevel(LogLevel level)
Set the minimum level of logging data to output. Messages with lower logging levels will not be recor...
A class to store the exit status of Planner::solve()
@ EXACT_SOLUTION
The planner found an exact solution.
@ APPROXIMATE_SOLUTION
The planner found an approximate solution.