TSRRT.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2020, 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 "ompl/geometric/planners/rrt/TSRRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include <limits>
41 
42 ompl::geometric::TSRRT::TSRRT(const base::SpaceInformationPtr &si, const TaskSpaceConfigPtr &task_space)
43  : base::Planner(si, "TSRRT"), task_space_(task_space)
44 {
46  specs_.directed = true;
47 
48  Planner::declareParam<double>("range", this, &TSRRT::setRange, &TSRRT::getRange, "0.:1.:10000.");
49  Planner::declareParam<double>("goal_bias", this, &TSRRT::setGoalBias, &TSRRT::getGoalBias, "0.:.05:1.");
50 }
51 
52 ompl::geometric::TSRRT::~TSRRT()
53 {
54  freeMemory();
55 }
56 
58 {
59  Planner::clear();
60  freeMemory();
61  if (nn_)
62  nn_->clear();
63  lastGoalMotion_ = nullptr;
64 }
65 
67 {
68  Planner::setup();
69  tools::SelfConfig sc(si_, getName());
70  sc.configurePlannerRange(maxDistance_);
71 
72  if (!nn_)
73  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
74  nn_->setDistanceFunction([this](const Motion *a, const Motion *b) { return distanceFunction(a, b); });
75 }
76 
78 {
79  if (nn_)
80  {
81  std::vector<Motion *> motions;
82  nn_->list(motions);
83  for (auto &motion : motions)
84  {
85  if (motion->state)
86  si_->freeState(motion->state);
87  delete motion;
88  }
89  }
90 }
91 
93 {
94  checkValidity();
95  base::Goal *goal = pdef_->getGoal().get();
96  auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
97 
98  if (!task_space_)
99  throw ompl::Exception("Task Space info is not set. Cannot solve");
100 
101  while (const base::State *st = pis_.nextStart())
102  {
103  auto *motion = new Motion(si_);
104  si_->copyState(motion->state, st);
105  motion->proj.resize(task_space_->getDimension());
106  task_space_->project(motion->state, motion->proj);
107  nn_->add(motion);
108  }
109 
110  if (nn_->size() == 0)
111  {
112  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
114  }
115 
116  OMPL_INFORM("%s: Starting with %u states already in datastructure. %d dimensional projection", getName().c_str(),
117  nn_->size(), task_space_->getDimension());
118 
119  Motion *solution = nullptr;
120  Motion *approxsol = nullptr;
121  double approxdif = std::numeric_limits<double>::infinity();
122  auto *rmotion = new Motion(si_);
123  base::State *rstate = rmotion->state;
124  base::State *xstate = si_->allocState();
125 
126  auto &r_proj = rmotion->proj;
127  r_proj.resize(task_space_->getDimension());
128 
129  while (ptc == false)
130  {
131  // Nearest state in the tree to the configuration we're about to sample.
132  Motion *nmotion = nullptr;
133 
134  // Sample state (with goal biasing)
135  if (goal_s != nullptr && rng_.uniform01() < goalBias_ && goal_s->canSample())
136  {
137  // Sample the goal in configuration space, then project to the task-space.
138  goal_s->sampleGoal(rstate);
139  task_space_->project(rstate, r_proj);
140  nmotion = nn_->nearest(rmotion);
141  }
142  else
143  {
144  // Sample the state in task-space, then lift this to the configuration space.
145  task_space_->sample(r_proj);
146  nmotion = nn_->nearest(rmotion);
147  if (!task_space_->lift(r_proj, nmotion->state, rstate))
148  continue;
149  }
150  base::State *dstate = rstate;
151 
152  // Truncate the maximum extension if necessary.
153  double d = si_->distance(nmotion->state, rstate);
154  if (d > maxDistance_)
155  {
156  si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
157  dstate = xstate;
158  }
159 
160  // Check for validity along the motion.
161  if (si_->checkMotion(nmotion->state, dstate))
162  {
163  /* create a motion */
164  Motion *motion = new Motion(si_);
165  si_->copyState(motion->state, dstate);
166  motion->parent = nmotion;
167  motion->proj.resize(task_space_->getDimension());
168  // Project state into task-space. Store the projection.
169  task_space_->project(motion->state, motion->proj);
170  nn_->add(motion);
171  double dist = 0.0;
172  bool sat = goal->isSatisfied(motion->state, &dist);
173  if (sat)
174  {
175  approxdif = dist;
176  solution = motion;
177  break;
178  }
179  if (dist < approxdif)
180  {
181  approxdif = dist;
182  approxsol = motion;
183  }
184  }
185  }
186 
187  bool solved = false;
188  bool approximate = false;
189  if (solution == nullptr)
190  {
191  solution = approxsol;
192  approximate = true;
193  }
194 
195  if (solution != nullptr)
196  {
197  lastGoalMotion_ = solution;
198 
199  /* construct the solution path */
200  std::vector<Motion *> mpath;
201  while (solution != nullptr)
202  {
203  mpath.push_back(solution);
204  solution = solution->parent;
205  }
206 
207  /* set the solution path */
208  auto path(std::make_shared<PathGeometric>(si_));
209  for (int i = mpath.size() - 1; i >= 0; --i)
210  path->append(mpath[i]->state);
211  pdef_->addSolutionPath(path, approximate, approxdif, getName());
212  solved = true;
213  }
214 
215  si_->freeState(xstate);
216  if (rmotion->state)
217  si_->freeState(rmotion->state);
218  delete rmotion;
219 
220  OMPL_INFORM("%s: Created %u states", getName().c_str(), nn_->size());
221 
222  return {solved, approximate};
223 }
224 
226 {
227  Planner::getPlannerData(data);
228 
229  std::vector<Motion *> motions;
230  if (nn_)
231  nn_->list(motions);
232 
233  if (lastGoalMotion_)
234  data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
235 
236  for (auto &motion : motions)
237  {
238  if (motion->parent == nullptr)
239  data.addStartVertex(base::PlannerDataVertex(motion->state));
240  else
241  data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state));
242  }
243 }
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: TSRRT.h:190
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: TSRRT.cpp:57
void freeMemory()
Free the memory allocated by this planner.
Definition: TSRRT.cpp:77
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
Definition of an abstract state.
Definition: State.h:113
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:123
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:486
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:269
A class to store the exit status of Planner::solve()
base::State * state
The state contained by the motion.
Definition: TSRRT.h:226
TSRRT(const base::SpaceInformationPtr &si, const TaskSpaceConfigPtr &task_space)
Constructor.
Definition: TSRRT.cpp:42
Abstract definition of goals.
Definition: Goal.h:126
void setGoalBias(double goalBias)
Set the goal bias.
Definition: TSRRT.h:174
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:259
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: TSRRT.cpp:66
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Abstract definition of a goal region that can be sampled.
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: TSRRT.cpp:92
Motion * parent
The parent motion in the exploration tree.
Definition: TSRRT.h:229
The exception type for ompl.
Definition: Exception.h:78
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: TSRRT.cpp:225
double getGoalBias() const
Get the goal bias the planner is using.
Definition: TSRRT.h:180
double getRange() const
Get the range the planner is using.
Definition: TSRRT.h:196
Representation of a motion.
Definition: TSRRT.h:215
@ INVALID_START
Invalid start state or no start state specified.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122