ThunderRetrieveRepair.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, University of Colorado, Boulder
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 the Univ of CO, Boulder 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: Dave Coleman */
36 
37 #include <ompl/geometric/planners/experience/ThunderRetrieveRepair.h>
38 #include <ompl/geometric/planners/rrt/RRTConnect.h>
39 #include <ompl/base/goals/GoalState.h>
40 #include <ompl/base/goals/GoalSampleableRegion.h>
41 #include <ompl/tools/config/SelfConfig.h>
42 #include <ompl/util/Console.h>
43 #include <ompl/tools/thunder/ThunderDB.h>
44 #include "ompl/tools/config/MagicConstants.h"
45 
46 #include <thread>
47 
48 #include <limits>
49 #include <utility>
50 
51 namespace ompl
52 {
53  namespace geometric
54  {
55  ThunderRetrieveRepair::ThunderRetrieveRepair(const base::SpaceInformationPtr &si,
56  tools::ThunderDBPtr experienceDB)
57  : base::Planner(si, "Thunder_Retrieve_Repair")
58  , experienceDB_(std::move(experienceDB))
59  , nearestK_(ompl::magic::NEAREST_K_RECALL_SOLUTIONS) // default value
60  , smoothingEnabled_(false) // makes understanding recalled paths more difficult if enabled
61  {
63  specs_.directed = true;
64 
65  // Repair Planner Specific:
66  repairProblemDef_ = std::make_shared<base::ProblemDefinition>(si_);
67 
68  path_simplifier_ = std::make_shared<PathSimplifier>(si_);
69  }
70 
71  ThunderRetrieveRepair::~ThunderRetrieveRepair()
72  {
73  freeMemory();
74  }
75 
77  {
78  Planner::clear();
79  freeMemory();
80 
81  // Clear the inner planner
82  if (repairPlanner_)
83  repairPlanner_->clear();
84  }
85 
86  void ThunderRetrieveRepair::setExperienceDB(const tools::ThunderDBPtr &experienceDB)
87  {
88  experienceDB_ = experienceDB;
89  }
90 
91  void ThunderRetrieveRepair::setRepairPlanner(const base::PlannerPtr &planner)
92  {
93  if (planner && planner->getSpaceInformation().get() != si_.get())
94  throw Exception("Repair planner instance does not match space information");
95  repairPlanner_ = planner;
96  setup_ = false;
97  }
98 
100  {
101  Planner::setup();
102 
103  // Setup repair planner (for use by the rrPlanner)
104  // Note: does not use the same pdef as the main planner in this class
105  if (!repairPlanner_)
106  {
107  // Set the repair planner
108  auto repair_planner(std::make_shared<RRTConnect>(si_));
109 
110  OMPL_DEBUG("No repairing planner specified. Using default: %s", repair_planner->getName().c_str());
111  repairPlanner_ = repair_planner; // Planner( repair_planer );
112  }
113  // Setup the problem definition for the repair planner
114  repairProblemDef_->setOptimizationObjective(pdef_->getOptimizationObjective()); // copy primary problem def
115 
116  // Setup repair planner
117  repairPlanner_->setProblemDefinition(repairProblemDef_);
118  if (!repairPlanner_->isSetup())
119  repairPlanner_->setup();
120  }
121 
123  {
124  }
125 
127  {
128  bool solved = false;
129  double approxdif = std::numeric_limits<double>::infinity();
130  nearestPaths_.clear();
131 
132  // Check if the database is empty
133  if (experienceDB_->isEmpty())
134  {
135  OMPL_INFORM("Experience database is empty so unable to run ThunderRetrieveRepair algorithm.");
136 
138  }
139 
140  // Restart the Planner Input States so that the first start and goal state can be fetched
141  pis_.restart();
142 
143  // Get a single start and goal state TODO: more than one
144  const base::State *startState = pis_.nextStart();
145  const base::State *goalState = pis_.nextGoal(ptc);
146 
147  // Create solution path struct
148  SPARSdb::CandidateSolution candidateSolution;
149 
150  // Search for previous solution in database
151  // TODO make this more than 1 path
152  if (!experienceDB_->findNearestStartGoal(nearestK_, startState, goalState, candidateSolution, ptc))
153  {
154  OMPL_INFORM("RetrieveRepair::solve() No nearest start or goal found");
155  return base::PlannerStatus::TIMEOUT; // The planner failed to find a solution
156  }
157 
158  // Save this for future debugging
159  nearestPaths_.push_back(candidateSolution.getGeometricPath());
160  nearestPathsChosenID_ = 0; // TODO not hardcode
161 
162  // All save trajectories should be at least 2 states long, then we append the start and goal states, for min
163  // of 4
164  assert(candidateSolution.getStateCount() >= 4);
165 
166  // Smooth the result
167  if (smoothingEnabled_)
168  {
169  OMPL_INFORM("ThunderRetrieveRepair solve: Simplifying solution (smoothing)...");
170  time::point simplifyStart = time::now();
171  std::size_t numStates = candidateSolution.getGeometricPath().getStateCount();
172  // ompl::geometric::PathGeometric pg = candidateSolution.getGeometricPath(); // TODO do not copy to new
173  // type
174  path_simplifier_->simplify(candidateSolution.getGeometricPath(), ptc);
175  double simplifyTime = time::seconds(time::now() - simplifyStart);
176  OMPL_INFORM("ThunderRetrieveRepair: Path simplification took %f seconds and removed %d states",
177  simplifyTime, numStates - candidateSolution.getGeometricPath().getStateCount());
178  }
179 
180  // Finished
181  approxdif = 0;
182  bool approximate = candidateSolution.isApproximate_;
183 
184  pdef_->addSolutionPath(candidateSolution.path_, approximate, approxdif, getName());
185  solved = true;
186  return {solved, approximate};
187  }
188 
190  {
191  // \todo: we could reuse our collision checking from the previous step to make this faster
192  // but that complicates everything and I'm not suppose to be spending too much time
193  // on this prototype - DTC
194 
195  OMPL_INFORM("Repairing path ----------------------------------");
196 
197  // Error check
198  if (primaryPath.getStateCount() < 2)
199  {
200  OMPL_ERROR("Cannot repair a path with less than 2 states");
201  return false;
202  }
203 
204  // Loop through every pair of states and make sure path is valid.
205  // If not, replan between those states
206  for (std::size_t toID = 1; toID < primaryPath.getStateCount(); ++toID)
207  {
208  std::size_t fromID = toID - 1; // this is our last known valid state
209  base::State *fromState = primaryPath.getState(fromID);
210  base::State *toState = primaryPath.getState(toID);
211 
212  // Check if our planner is out of time
213  if (ptc)
214  {
215  OMPL_DEBUG("Repair path function interrupted because termination condition is true.");
216  return false;
217  }
218 
219  // Check path between states
220  if (!si_->checkMotion(fromState, toState))
221  {
222  // Path between (from, to) states not valid, but perhaps to STATE is
223  // Search until next valid STATE is found in existing path
224  std::size_t subsearch_id = toID;
225  base::State *new_to;
226  OMPL_DEBUG("Searching for next valid state, because state %d to %d was not valid out %d total "
227  "states",
228  fromID, toID, primaryPath.getStateCount());
229  while (subsearch_id < primaryPath.getStateCount())
230  {
231  new_to = primaryPath.getState(subsearch_id);
232  if (si_->isValid(new_to))
233  {
234  OMPL_DEBUG("State %d was found to valid, we can now repair between states", subsearch_id);
235  // This future state is valid, we can stop searching
236  toID = subsearch_id;
237  toState = new_to;
238  break;
239  }
240  ++subsearch_id; // keep searching for a new state to plan to
241  }
242  // Check if we ever found a next state that is valid
243  if (subsearch_id >= primaryPath.getStateCount())
244  {
245  // We never found a valid state to plan to, instead we reached the goal state and it too wasn't
246  // valid. This is bad.
247  // I think this is a bug.
248  OMPL_ERROR("No state was found valid in the remainder of the path. Invalid goal state. This "
249  "should not happen.");
250  return false;
251  }
252 
253  // Plan between our two valid states
254  PathGeometric newPathSegment(si_);
255 
256  // Not valid motion, replan
257  OMPL_DEBUG("Planning from %d to %d", fromID, toID);
258 
259  if (!replan(fromState, toState, newPathSegment, ptc))
260  {
261  OMPL_WARN("Unable to repair path between state %d and %d", fromID, toID);
262  return false;
263  }
264 
265  // TODO make sure not approximate solution
266 
267  // Reference to the path
268  std::vector<base::State *> &primaryPathStates = primaryPath.getStates();
269 
270  // Remove all invalid states between (fromID, toID) - not including those states themselves
271  while (fromID != toID - 1)
272  {
273  OMPL_INFORM("Deleting state %d", fromID + 1);
274  primaryPathStates.erase(primaryPathStates.begin() + fromID + 1);
275  --toID; // because vector has shrunk
276  }
277 
278  // Insert new path segment into current path
279  OMPL_DEBUG("Inserting new %d states into old path. Previous length: %d",
280  newPathSegment.getStateCount() - 2, primaryPathStates.size());
281 
282  // Note: skip first and last states because they should be same as our start and goal state, same as
283  // `fromID` and `toID`
284  for (std::size_t i = 1; i < newPathSegment.getStateCount() - 1; ++i)
285  {
286  std::size_t insertLocation = toID + i - 1;
287  OMPL_DEBUG("Inserting newPathSegment state %d into old path at position %d", i, insertLocation);
288  primaryPathStates.insert(primaryPathStates.begin() + insertLocation,
289  si_->cloneState(newPathSegment.getStates()[i]));
290  }
291  // primaryPathStates.insert( primaryPathStates.begin() + toID, newPathSegment.getStates().begin(),
292  // newPathSegment.getStates().end() );
293  OMPL_DEBUG("Inserted new states into old path. New length: %d", primaryPathStates.size());
294 
295  // Set the toID to jump over the newly inserted states to the next unchecked state. Subtract 2
296  // because we ignore start and goal
297  toID = toID + newPathSegment.getStateCount() - 2;
298  OMPL_DEBUG("Continuing searching at state %d", toID);
299  }
300  }
301 
302  OMPL_INFORM("Done repairing ---------------------------------");
303 
304  return true;
305  }
306 
307  bool ThunderRetrieveRepair::replan(const base::State *start, const base::State *goal,
308  PathGeometric &newPathSegment, const base::PlannerTerminationCondition &ptc)
309  {
310  // Reset problem definition
311  repairProblemDef_->clearSolutionPaths();
312  repairProblemDef_->clearStartStates();
313  repairProblemDef_->clearGoal();
314 
315  // Reset planner
316  repairPlanner_->clear();
317 
318  // Configure problem definition
319  repairProblemDef_->setStartAndGoalStates(start, goal);
320 
321  // Configure planner
322  repairPlanner_->setProblemDefinition(repairProblemDef_);
323 
324  // Solve
325  OMPL_INFORM("Preparing to repair path-----------------------------------------");
327  time::point startTime = time::now();
328 
329  // TODO: if we use replanner like RRT* the ptc will allow it to run too long and no time will be left for
330  // the rest of algorithm
331  lastStatus = repairPlanner_->solve(ptc);
332 
333  // Results
334  double planTime = time::seconds(time::now() - startTime);
335  if (!lastStatus)
336  {
337  OMPL_WARN("Replan Solve: No replan solution between disconnected states found after %f seconds",
338  planTime);
339  return false;
340  }
341 
342  // Check if approximate
343  if (repairProblemDef_->hasApproximateSolution() ||
344  repairProblemDef_->getSolutionDifference() > std::numeric_limits<double>::epsilon())
345  {
346  OMPL_INFORM("Replan Solve: Solution is approximate, not using");
347  return false;
348  }
349 
350  // Convert solution into a PathGeometric path
351  base::PathPtr p = repairProblemDef_->getSolutionPath();
352  if (!p)
353  {
354  OMPL_ERROR("Unable to get solution path from problem definition");
355  return false;
356  }
357 
358  newPathSegment = static_cast<PathGeometric &>(*p);
359 
360  // Smooth the result
361  OMPL_INFORM("Repair: Simplifying solution (smoothing)...");
362  time::point simplifyStart = time::now();
363  std::size_t numStates = newPathSegment.getStateCount();
364  path_simplifier_->simplify(newPathSegment, ptc);
365  double simplifyTime = time::seconds(time::now() - simplifyStart);
366  OMPL_INFORM("ThunderRetrieveRepair: Path simplification took %f seconds and removed %d states",
367  simplifyTime, numStates - newPathSegment.getStateCount());
368 
369  // Save the planner data for debugging purposes
370  repairPlannerDatas_.push_back(std::make_shared<base::PlannerData>(si_));
371  repairPlanner_->getPlannerData(*repairPlannerDatas_.back());
372  repairPlannerDatas_.back()->decoupleFromPlanner(); // copy states so that when planner unloads/clears we
373  // don't lose them
374 
375  // Return success
376  OMPL_INFORM("Replan Solve: solution found in %f seconds with %d states", planTime,
377  newPathSegment.getStateCount());
378 
379  return true;
380  }
381 
383  {
384  OMPL_INFORM("ThunderRetrieveRepair getPlannerData: including %d similar paths", nearestPaths_.size());
385 
386  // Visualize the n candidate paths that we recalled from the database
387  for (auto path : nearestPaths_)
388  {
389  for (std::size_t j = 1; j < path.getStateCount(); ++j)
390  {
391  data.addEdge(base::PlannerDataVertex(path.getState(j - 1)),
392  base::PlannerDataVertex(path.getState(j)));
393  }
394  }
395  }
396 
397  const std::vector<PathGeometric> &ThunderRetrieveRepair::getLastRecalledNearestPaths() const
398  {
399  return nearestPaths_; // list of candidate paths
400  }
401 
403  {
404  return nearestPathsChosenID_; // of the candidate paths list, the one we chose
405  }
406 
408  {
410  }
411 
412  void ThunderRetrieveRepair::getRepairPlannerDatas(std::vector<base::PlannerDataPtr> &data) const
413  {
414  data = repairPlannerDatas_;
415  }
416 
417  std::size_t ThunderRetrieveRepair::checkMotionScore(const base::State *s1, const base::State *s2) const
418  {
419  int segmentCount = si_->getStateSpace()->validSegmentCount(s1, s2);
420 
421  std::size_t invalidStatesScore = 0; // count number of interpolated states in collision
422 
423  // temporary storage for the checked state
424  base::State *test = si_->allocState();
425 
426  // Linerarly step through motion between state 0 to state 1
427  double iteration_step = 1.0 / double(segmentCount);
428  for (double location = 0.0; location <= 1.0; location += iteration_step)
429  {
430  si_->getStateSpace()->interpolate(s1, s2, location, test);
431 
432  if (!si_->isValid(test))
433  {
434  // OMPL_DEBUG("Found INVALID location between states at gradient %f", location);
435  invalidStatesScore++;
436  }
437  else
438  {
439  // OMPL_DEBUG("Found valid location between states at gradient %f", location);
440  }
441  }
442  si_->freeState(test);
443 
444  return invalidStatesScore;
445  }
446 
447  } // namespace geometric
448 } // namespace ompl
The exception type for ompl.
Definition: Exception.h:47
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
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...
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:237
void restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again.
Definition: Planner.cpp:180
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available....
Definition: Planner.cpp:274
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerInputStates pis_
Utility class to extract valid input states
Definition: Planner.h:423
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:429
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:420
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:417
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:440
Definition of an abstract state.
Definition: State.h:50
Definition of a geometric path.
Definition: PathGeometric.h:66
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
base::State * getState(unsigned int index)
Get the state located at index along the path.
std::vector< base::State * > & getStates()
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
void setExperienceDB(const tools::ThunderDBPtr &experienceDB)
Pass a pointer of the database from the thunder framework.
int nearestK_
Number of 'k' close solutions to choose from database for further filtering.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
const std::vector< PathGeometric > & getLastRecalledNearestPaths() const
Get debug information about the top recalled paths that were chosen for further filtering.
std::size_t getLastRecalledNearestPathChosen() const
Get debug information about the top recalled paths that were chosen for further filtering.
bool repairPath(const base::PlannerTerminationCondition &ptc, PathGeometric &primaryPath)
Repairs a path to be valid in the current planning environment.
bool replan(const base::State *start, const base::State *goal, PathGeometric &newPathSegment, const base::PlannerTerminationCondition &ptc)
Use our secondary planner to find a valid path between start and goal, and return that path.
std::size_t nearestPathsChosenID_
the ID within nearestPaths_ of the path that was chosen for repair
bool smoothingEnabled_
Optionally smooth retrieved and repaired paths from database.
base::PlannerPtr repairPlanner_
A secondary planner for replanning.
std::vector< PathGeometric > nearestPaths_
Recall the nearest paths and store this in planner data for introspection later.
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
std::size_t checkMotionScore(const base::State *s1, const base::State *s2) const
Count the number of states along the discretized path that are in collision Note: This is kind of an ...
std::vector< base::PlannerDataPtr > repairPlannerDatas_
Debug the repair planner by saving its planner data each time it is used.
void getPlannerData(base::PlannerData &data) const override
Get information about the exploration data structure the planning from scratch motion planner used.
void freeMemory()
Free the memory allocated by this planner.
ThunderRetrieveRepair(const base::SpaceInformationPtr &si, tools::ThunderDBPtr experienceDB)
Constructor.
base::ProblemDefinitionPtr repairProblemDef_
A secondary problem definition for the repair planner to use.
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
tools::ThunderDBPtr experienceDB_
The database of motions to search through.
void setRepairPlanner(const base::PlannerPtr &planner)
Set the planner that will be used for repairing invalid paths recalled from experience.
void getRepairPlannerDatas(std::vector< base::PlannerDataPtr > &data) const
Get information about the exploration data structure the repair motion planner used each call.
const PathGeometric & getChosenRecallPath() const
Get the chosen path used from database for repair.
PathSimplifierPtr path_simplifier_
The instance of the path simplifier.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:52
point now()
Get the current time point.
Definition: Time.h:58
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:64
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:212
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:202
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
@ ABORT
The planner did not find a solution for some other reason.
Definition: PlannerStatus.h:70
@ TIMEOUT
The planner failed to find a solution.
Definition: PlannerStatus.h:62
@ UNKNOWN
Uninitialized status.
Definition: PlannerStatus.h:54
Struct for passing around partially solved solutions.
Definition: SPARSdb.h:234