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.
67 struct 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.
77 struct 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.
92 Problem 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.
115 ompl::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.
155 ompl::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.
176 ompl::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.
196 ompl::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.
230 void 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 
252 void 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 
299 void 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 
330 void 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 
361 void 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 
374 int 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 }
A shared pointer wrapper for ompl::base::SpaceInformation.
@ APPROXIMATE_SOLUTION
The planner found an approximate solution.
A shared pointer wrapper for ompl::geometric::SimpleSetup.
Task-space Rapidly-exploring Random Trees.
Definition: TSRRT.h:152
Definition of an abstract state.
Definition: State.h:114
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:127
Bi-directional Range-Limited Random Tree (Ryan Luna's Random Tree)
Definition: BiRLRT.h:128
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
Definition of a geometric path.
Definition: PathGeometric.h:98
A shared pointer wrapper for ompl::base::Planner.
RRT-Connect (RRTConnect)
Definition: RRTConnect.h:126
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
Rapidly-exploring Random Trees.
Definition: RRT.h:130
A class to store the exit status of Planner::solve()
Representation of a benchmark request.
Definition: Benchmark.h:253
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
Bi-directional KPIECE with one level of discretization.
Definition: BKPIECE1.h:138
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:113
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
@ EXACT_SOLUTION
The planner found an exact solution.
A shared pointer wrapper for ompl::base::StateSpace.
Search Tree with Resolution Independent Density Estimation.
Definition: STRIDE.h:111
Range-Limited Random Tree (Ryan Luna's Random Tree)
Definition: RLRT.h:128
void interpolate(unsigned int count)
Insert a number of states in a path so that the path is made up of exactly count states....
The exception type for ompl.
Definition: Exception.h:79
A shared pointer wrapper for ompl::base::Goal.
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:175
Kinematic Planning by Interior-Exterior Cell Exploration.
Definition: KPIECE1.h:136
A shared pointer wrapper for ompl::geometric::XXLDecomposition.
The lower and upper bounds for an Rn space.