SE3RigidBodyPlanningWithOptimization.cpp
1 /*********************************************************************
2 * Rice University Software Distribution License
3 *
4 * Copyright (c) 2010, Rice University
5 * All Rights Reserved.
6 *
7 * For a full description see the file named LICENSE.
8 *
9 *********************************************************************/
10 
11 /* Author: Ioan Sucan */
12 
13 #include <omplapp/apps/SE3RigidBodyPlanning.h>
14 #include <ompl/geometric/planners/rrt/RRTstar.h>
15 #include <ompl/tools/multiplan/OptimizePlan.h>
16 #include <ompl/geometric/planners/rrt/RRTConnect.h>
17 #include <ompl/base/objectives/PathLengthOptimizationObjective.h>
18 #include <omplapp/config.h>
19 
20 using namespace ompl;
21 
22 int main()
23 {
24  // plan in SE3
26 
27  // load the robot and the environment
28  std::string robot_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/3D/cubicles_robot.dae";
29  std::string env_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/3D/cubicles_env.dae";
30  setup.setRobotMesh(robot_fname.c_str());
31  setup.setEnvironmentMesh(env_fname.c_str());
32 
33  // define start state
35  start->setX(-4.96);
36  start->setY(-40.62);
37  start->setZ(70.57);
38  start->rotation().setIdentity();
39 
40  // define goal state
42  goal->setX(200.49);
43  goal->setY(-40.62);
44  goal->setZ(70.57);
45  goal->rotation().setIdentity();
46 
47  // set the start & goal states
48  setup.setStartAndGoalStates(start, goal);
49 
50  // setting collision checking resolution to 1% of the space extent
51  setup.getSpaceInformation()->setStateValidityCheckingResolution(0.01);
52 
53  // make sure the planners run until the time limit, and get the best possible solution
54  setup.getProblemDefinition()->setOptimizationObjective(
55  std::make_shared<base::PathLengthOptimizationObjective>(setup.getSpaceInformation()));
56 
57  setup.setup();
58 
59  std::stringstream res;
60 
61  // run with RRT*
62  setup.setPlanner(std::make_shared<geometric::RRTstar>(setup.getSpaceInformation()));
63  res << "RRT*" << std::endl;
64  for (double time = 1.0 ; time < 10.1 ; time = time + 1.0)
65  {
66  setup.clear();
67  double length = -1.0;
68  // try to solve the problem
69  if (setup.solve(time) && setup.haveExactSolutionPath())
70  length = setup.getSolutionPath().length();
71  res << "time = " << setup.getLastPlanComputationTime() << " \t length = " << length << std::endl;
72  }
73 
75  res << "RRTConnect with path hybridization (one thread)" << std::endl;
76  for (double time = 1.0 ; time < 10.1 ; time = time + 1.0)
77  {
78  setup.clear();
79  op.clearPlanners();
80 
81  // add one planer only, so there is only one planning thread in use
82  op.addPlanner(std::make_shared<geometric::RRTConnect>(setup.getSpaceInformation()));
83 
84  double length = -1.0;
85  double duration = 0.0;
86 
88  // try to solve the problem
89  if (op.solve(time, 30, 1) && setup.haveExactSolutionPath())
90  {
91  duration = ompl::time::seconds(ompl::time::now() - start);
92  length = setup.getSolutionPath().length();
93  }
94  else
95  duration = ompl::time::seconds(ompl::time::now() - start);
96 
97  res << "time = " << duration << "s \t length = " << length << std::endl;
98  }
99 
100  res << "RRTConnect with path hybridization (four threads)" << std::endl;
101  for (double time = 1.0 ; time < 10.1 ; time = time + 1.0)
102  {
103  setup.clear();
104  op.clearPlanners();
105 
106  // add one planer only, so there is only one planning thread in use
107  op.addPlanner(std::make_shared<geometric::RRTConnect>(setup.getSpaceInformation()));
108  op.addPlanner(std::make_shared<geometric::RRTConnect>(setup.getSpaceInformation()));
109  op.addPlanner(std::make_shared<geometric::RRTConnect>(setup.getSpaceInformation()));
110  op.addPlanner(std::make_shared<geometric::RRTConnect>(setup.getSpaceInformation()));
111 
112  double length = -1.0;
113  double duration = 0.0;
114 
116  // try to solve the problem
117  if (op.solve(time, 30, 4) && setup.haveExactSolutionPath())
118  {
119  duration = ompl::time::seconds(ompl::time::now() - start);
120  length = setup.getSolutionPath().length();
121  }
122  else
123  duration = ompl::time::seconds(ompl::time::now() - start);
124 
125  res << "time = " << duration << "s \t length = " << length << std::endl;
126  }
127 
128  std::cout << res.str();
129 
130  return 0;
131 }
bool haveExactSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
void setStartAndGoalStates(const base::ScopedState<> &start, const base::ScopedState<> &goal, const double threshold=std::numeric_limits< double >::epsilon())
Set the start and goal states to use.
Definition: SimpleSetup.cpp:90
double length() const override
Compute the length of a geometric path (sum of lengths of segments that make up the path) ...
PathGeometric & getSolutionPath() const
Get the solution path. Throw an exception if no solution is available.
Definition of a scoped state.
Definition: ScopedState.h:56
Wrapper for ompl::app::RigidBodyPlanning that plans for rigid bodies in SE3.
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: AppBase.h:98
void setPlanner(const base::PlannerPtr &planner)
Set the planner to use. If the planner is not set, an attempt is made to use the planner allocator...
Definition: SimpleSetup.h:208
const base::SpaceInformationPtr & getSpaceInformation() const
Get the current instance of the space information.
Definition: SimpleSetup.h:75
virtual bool setEnvironmentMesh(const std::string &env)
This function specifies the name of the CAD file representing the environment (env). Returns 1 on success, 0 on failure.
double getLastPlanComputationTime() const
Get the amount of time (in seconds) spent during the last planning step.
Definition: SimpleSetup.h:239
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:76
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
std::chrono::system_clock::duration duration
Representation of a time duration.
Definition: Time.h:67
Run one or more motion planners repeatedly (using a specified number of threads), and hybridize solut...
Definition: OptimizePlan.h:49
const base::ProblemDefinitionPtr & getProblemDefinition() const
Get the current instance of the problem definition.
Definition: SimpleSetup.h:81
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for up to a specified amount of time (default is 1 second)
point now()
Get the current time point.
Definition: Time.h:70
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:64
virtual bool setRobotMesh(const std::string &robot)
This function specifies the name of the CAD file representing the robot (robot). Returns 1 on success...
virtual void clear()
Clear all planning data. This only includes data generated by motion plan computation. Planner settings, start & goal states are not affected.
Definition: SimpleSetup.cpp:82