Loading...
Searching...
No Matches
PlanarManipulatorDemo.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2015, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author: Ryan Luna */
36
37#include <fstream>
38
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"
48
49#include <ompl/geometric/SimpleSetup.h>
50
51// planners
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>
58
59// bi-directional
60#include <ompl/geometric/planners/rrt/RRTConnect.h>
61#include <ompl/geometric/planners/kpiece/BKPIECE1.h>
62#include <ompl/geometric/planners/rlrt/BiRLRT.h>
63
64#include <ompl/tools/benchmark/Benchmark.h>
65
66// Input arguments to this binary.
67struct Arguments
68{
69 int numLinks;
70 int numRuns;
71 double timeout;
72 std::string problem;
73 bool viz;
74};
75
76// Minimal setup for a planar manipulation problem.
77struct Problem
78{
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)
81 {
82 manipulator.setBaseFrame(baseFrame);
83 }
84
85 std::string name;
86 PlanarManipulator manipulator;
87 PolyWorld world;
88 Eigen::Affine2d goalFrame;
89};
90
91// Creates a problem from the input arguments.
92Problem CreateProblem(const Arguments &args)
93{
94 if (args.problem == "corridor")
95 {
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);
100 }
101 else if (args.problem == "constricted")
102 {
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);
107 }
108 else
109 {
110 throw ompl::Exception("Unknown problem name: " + args.problem);
111 }
112}
113
114// Initialize OMPL for the given planar manipulator problem.
115ompl::geometric::SimpleSetupPtr setupOMPL(Problem &problem)
116{
117 const unsigned int numLinks = problem.manipulator.getNumLinks();
118 // Create the state space for the manipulator.
120 ompl::base::RealVectorBounds bounds(numLinks);
121 bounds.setLow(-M_PI);
122 bounds.setHigh(M_PI);
123
124 // Bound the joints of the manipulator between [-PI, PI]
125 space->as<PlanarManipulatorStateSpace>()->setBounds(bounds);
126 problem.manipulator.setBounds(bounds.low, bounds.high);
127
129
130 // Create the collision checker.
131 setup->setStateValidityChecker(std::make_shared<PlanarManipulatorCollisionChecker>(
132 setup->getSpaceInformation(), problem.manipulator, &problem.world));
133
134 // Increase motion validator resolution.
135 setup->getSpaceInformation()->setStateValidityCheckingResolution(0.001);
136
137 // Set the start and goal.
138 ompl::base::State *start = setup->getStateSpace()->allocState();
139 double *start_angles = start->as<PlanarManipulatorStateSpace::StateType>()->values;
140 for (size_t i = 0; i < numLinks; ++i)
141 start_angles[i] = 1e-7; // zero is dangerous for numerical reasons.
142 setup->getProblemDefinition()->addStartState(start);
143 setup->getStateSpace()->freeState(start);
144
145 ompl::base::GoalPtr goal(new PlanarManipulatorIKGoal(setup->getSpaceInformation(), problem.goalFrame,
146 &problem.manipulator,
147 false)); // orientation is not fixed
148 goal->as<PlanarManipulatorIKGoal>()->setThreshold(1e-3);
149 setup->setGoal(goal);
150
151 return setup;
152}
153
154// Returns the bounds on the reachable part of the world for the chain.
155ompl::base::RealVectorBounds GetReachableWorkspaceBounds(Point origin, double chainLength, const Problem &problem)
156{
157 const auto xBounds = problem.world.xBounds();
158 const auto yBounds = problem.world.yBounds();
159
160 // Clip the world bounds based on reachable workspace of the chain.
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);
165 ompl::base::RealVectorBounds reachable_bounds(2);
166 reachable_bounds.setLow(0, xMin);
167 reachable_bounds.setHigh(0, xMax);
168 reachable_bounds.setLow(1, yMin);
169 reachable_bounds.setHigh(1, yMax);
170
171 return reachable_bounds;
172}
173
174// Returns the task-space projection for the planar manipulator when planning
175// using TSRRT. Projects the end-effector position into the workspace.
176ompl::geometric::TaskSpaceConfigPtr getTaskSpaceConfig(const Problem &problem)
177{
178 const PlanarManipulator &manip = problem.manipulator;
179 unsigned int numLinks = manip.getNumLinks();
180 double linkLength = 1.0 / numLinks;
181 double chainLength = numLinks * linkLength;
182
183 const Eigen::Affine2d &baseFrame = manip.getBaseFrame();
184 Point origin = {baseFrame.translation()(0), baseFrame.translation()(1)};
185
186 const ompl::base::RealVectorBounds reachable_bounds = GetReachableWorkspaceBounds(origin, chainLength, problem);
187
188 ompl::geometric::TaskSpaceConfigPtr task_space_ptr(new PlanarManipTaskSpaceConfig(&manip, reachable_bounds));
189 return task_space_ptr;
190}
191
192// Returns the XXL decomposition for the planar manipulator. Splits the
193// 2D workspace into a grid with numXYSlices in the x and y dimensions.
194// Projects the end-effector position of the chain into the decomposition.
195// For chains with more than six links, the midpoint is also projected.
196ompl::geometric::XXLDecompositionPtr getXXLDecomp(const ompl::base::SpaceInformationPtr &si, const Problem &problem,
197 int numXYSlices)
198{
199 const PlanarManipulator &manip = problem.manipulator;
200 unsigned int numLinks = manip.getNumLinks();
201 double linkLength = 1.0 / numLinks; // TODO: pass this in
202 double chainLength = numLinks * linkLength;
203
204 // Creating decomposition for XXL
205 const Eigen::Affine2d &baseFrame = manip.getBaseFrame();
206 Point origin = {baseFrame.translation()(0), baseFrame.translation()(1)};
207
208 const ompl::base::RealVectorBounds reachable_bounds = GetReachableWorkspaceBounds(origin, chainLength, problem);
209
210 std::vector<int> xySlices(2, numXYSlices);
211 const int thetaSlices = 1;
212 // Select point(s) on the manipulator for projection.
213 // For short chains, we only pick the end-effector position.
214 std::vector<int> projLinks;
215 if (numLinks > 6)
216 {
217 // midpoint
218 projLinks.push_back((numLinks / 2) - 1);
219 }
220 // end-effector.
221 projLinks.push_back(numLinks - 1);
222
224 si, &manip, reachable_bounds, xySlices, thetaSlices, projLinks, true)); // diagonal edges
225 return decomp;
226}
227
228// Computes the Cartesian distance traveled by each joint in the chain
229// on the solution path that is computed.
230void postRunEvent(const ompl::base::PlannerPtr &planner, ompl::tools::Benchmark::RunProperties &run,
231 const PlanarManipulator *manip)
232{
233 if (!planner->getProblemDefinition()->hasSolution())
234 return;
235
236 double cartesianDist = 0.0;
237 const auto &path = static_cast<const ompl::geometric::PathGeometric &>(
238 *(planner->getProblemDefinition()->getSolutionPath().get()));
239 for (size_t i = 0; i < path.getStateCount() - 1; ++i)
240 {
241 std::vector<Eigen::Affine2d> startFrames, endFrames;
242 manip->FK(path.getState(i)->as<PlanarManipulatorStateSpace::StateType>()->values, startFrames);
243 manip->FK(path.getState(i + 1)->as<PlanarManipulatorStateSpace::StateType>()->values, endFrames);
244
245 for (size_t j = 1; j < endFrames.size(); ++j)
246 cartesianDist += (endFrames[j].translation() - startFrames[j].translation()).norm();
247 }
248
249 run["Cartesian Distance REAL"] = boost::lexical_cast<std::string>(cartesianDist);
250}
251
252void BenchmarkProblem(ompl::geometric::SimpleSetupPtr setup, const Problem &problem, int runs, double timeout)
253{
254 ompl::base::PlannerPtr kpiece(new ompl::geometric::KPIECE1(setup->getSpaceInformation()));
255 ompl::base::PlannerPtr rrt(new ompl::geometric::RRT(setup->getSpaceInformation()));
256 ompl::base::PlannerPtr rlrt(new ompl::geometric::RLRT(setup->getSpaceInformation()));
257 ompl::base::PlannerPtr stride(new ompl::geometric::STRIDE(setup->getSpaceInformation()));
258 ompl::base::PlannerPtr tsrrt(new ompl::geometric::TSRRT(setup->getSpaceInformation(), getTaskSpaceConfig(problem)));
259
260 ompl::base::PlannerPtr rrtc(new ompl::geometric::RRTConnect(setup->getSpaceInformation()));
261 ompl::base::PlannerPtr bkpiece(new ompl::geometric::BKPIECE1(setup->getSpaceInformation()));
262 ompl::base::PlannerPtr birlrt(new ompl::geometric::BiRLRT(setup->getSpaceInformation()));
263
264 const int numLinks = problem.manipulator.getNumLinks();
265 const int xySlices = std::max(2, numLinks / 3);
266 ompl::base::PlannerPtr xxl(new ompl::geometric::XXL(setup->getSpaceInformation(),
267 getXXLDecomp(setup->getSpaceInformation(), problem, xySlices)));
269 setup->getSpaceInformation(), getXXLDecomp(setup->getSpaceInformation(), problem, /*xySlices*/ 1)));
270 xxl1->setName("XXL1");
271
272 std::string name ="PlanarManipulator - " + problem.name;
273 ompl::tools::Benchmark benchmark(*setup, name);
274
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);
285
286 benchmark.setPostRunEvent([&](const ompl::base::PlannerPtr &planner, ompl::tools::Benchmark::RunProperties &run) {
287 postRunEvent(planner, run, &problem.manipulator);
288 });
289 benchmark.addExperimentParameter("num_links", "INTEGER", boost::lexical_cast<std::string>(numLinks));
290 benchmark.addExperimentParameter("cells", "INTEGER", boost::lexical_cast<std::string>(xySlices));
291
292 double memoryLimit = 8192.0; // MB. XXL requires an XXL amount of memory
293 ompl::tools::Benchmark::Request request(timeout, memoryLimit, runs);
294 benchmark.benchmark(request);
295
296 benchmark.saveResultsToFile();
297}
298
299void WriteVisualization(const Problem &problem, const ompl::geometric::PathGeometric &path, int xySlices)
300{
301 const int numLinks = problem.manipulator.getNumLinks();
302
303 const char *world_file = "world.yaml";
304 OMPL_INFORM("Writing world to %s", world_file);
305 problem.world.writeWorld(world_file);
306
307 const double linkLength = 1.0 / numLinks;
308 const Eigen::Affine2d &basePose = problem.manipulator.getBaseFrame();
309
310 const char *path_file = "manipulator_path.txt";
311 OMPL_INFORM("Writing path to %s", path_file);
312 std::ofstream fout;
313
314 // This is a proprietary format for the python visualization script.
315 fout.open(path_file);
316 // Preamble.
317 fout << numLinks << " " << linkLength << " " << basePose.translation()(0) << " " << basePose.translation()(1) << " "
318 << xySlices << std::endl;
319 // Write each state on the interpolated path.
320 for (size_t i = 0; i < path.getStateCount(); ++i)
321 {
322 const double *angles = path.getState(i)->as<PlanarManipulatorStateSpace::StateType>()->values;
323 for (size_t j = 0; j < problem.manipulator.getNumLinks(); ++j)
324 fout << angles[j] << " ";
325 fout << std::endl;
326 }
327 fout.close();
328}
329
330void SolveProblem(ompl::geometric::SimpleSetupPtr setup, const Problem &problem, double timeout, bool write_viz_out)
331{
332 // Solve the problem with XXL.
333 const int numLinks = problem.manipulator.getNumLinks();
334 // The number of grid cells in each dimension of the workspace decomposition.
335 const int xySlices = std::max(2, numLinks / 3);
336 ompl::base::PlannerPtr xxl(new ompl::geometric::XXL(setup->getSpaceInformation(),
337 getXXLDecomp(setup->getSpaceInformation(), problem, xySlices)));
338 setup->setPlanner(xxl);
339
340 // SOLVE!
341 ompl::base::PlannerStatus status = setup->solve(timeout);
342
345 {
346 ompl::geometric::PathGeometric &pgeo = setup->getSolutionPath();
347 OMPL_INFORM("Solution path has %d states", pgeo.getStateCount());
348
349 if (write_viz_out)
350 {
351 pgeo.interpolate(250);
352 WriteVisualization(problem, pgeo, xySlices);
353 }
354 }
355 else
356 {
357 OMPL_WARN("Planning failed");
358 }
359}
360
361void PlanarManipulatorPlanning(const Arguments &args)
362{
363 ompl::msg::setLogLevel(ompl::msg::LOG_INFO);
364
365 Problem problem = CreateProblem(args);
366 ompl::geometric::SimpleSetupPtr setup = setupOMPL(problem);
367
368 if (args.numRuns == 1)
369 SolveProblem(setup, problem, args.timeout, args.viz);
370 else
371 BenchmarkProblem(setup, problem, args.numRuns, args.timeout);
372}
373
374int main(int argc, char **argv)
375{
376 Arguments args;
377
378 // Read args
379 namespace po = boost::program_options;
380 po::options_description desc("Allowed options");
381 // clang-format off
382 desc.add_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");
394 // clang-format on
395
396 po::variables_map vm;
397 po::store(po::parse_command_line(argc, argv, desc), vm);
398 po::notify(vm);
399
400 if (vm.count("help"))
401 {
402 std::cout << desc << std::endl;
403 return 0;
404 }
405 PlanarManipulatorPlanning(args);
406}
The exception type for ompl.
Definition Exception.h:47
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.
A shared pointer wrapper for ompl::base::SpaceInformation.
A shared pointer wrapper for ompl::base::StateSpace.
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
Bi-directional KPIECE with one level of discretization.
Definition BKPIECE1.h:74
Bi-directional Range-Limited Random Tree (Ryan Luna's Random Tree)
Definition BiRLRT.h:64
Kinematic Planning by Interior-Exterior Cell Exploration.
Definition KPIECE1.h:72
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)
Definition RLRT.h:64
RRT-Connect (RRTConnect)
Definition RRTConnect.h:62
Rapidly-exploring Random Trees.
Definition RRT.h:66
Search Tree with Resolution Independent Density Estimation.
Definition STRIDE.h:79
A shared pointer wrapper for ompl::geometric::SimpleSetup.
Create the set of classes typically needed to solve a geometric problem.
Definition SimpleSetup.h:63
Task-space Rapidly-exploring Random Trees.
Definition TSRRT.h:88
A shared pointer wrapper for ompl::geometric::XXLDecomposition.
Benchmark a set of planners on a problem instance.
Definition Benchmark.h:49
std::map< std::string, std::string > RunProperties
The data collected from a run of a planner is stored as key-value pairs.
Definition Benchmark.h:79
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66
void setLogLevel(LogLevel level)
Set the minimum level of logging data to output. Messages with lower logging levels will not be recor...
Definition Console.cpp:136
STL namespace.
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.
Representation of a benchmark request.
Definition Benchmark.h:157