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  {
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 
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 base::PlannerStatus(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 == true)
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
std::vector< base::PlannerDataPtr > repairPlannerDatas_
Debug the repair planner by saving its planner data each time it is used.
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:212
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
tools::ThunderDBPtr experienceDB_
The database of motions to search through.
void freeMemory()
Free the memory allocated by this planner.
The planner failed to find a solution.
Definition: PlannerStatus.h:62
base::ProblemDefinitionPtr repairProblemDef_
A secondary problem definition for the repair planner to use.
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available. Because sampling of goal states may also produce invalid goals, this function takes an argument that specifies whether a termination condition has been reached. If the termination condition evaluates to true the function terminates even if no valid goal has been found.
Definition: Planner.cpp:269
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
void getRepairPlannerDatas(std::vector< base::PlannerDataPtr > &data) const
Get information about the exploration data structure the repair motion planner used each call...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
STL namespace.
Struct for passing around partially solved solutions.
Definition: SPARSdb.h:240
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:418
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:222
base::State * getState(unsigned int index)
Get the state located at index along the path.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
const PathGeometric & getChosenRecallPath() const
Get the chosen path used from database for repair.
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:438
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:76
bool repairPath(const base::PlannerTerminationCondition &ptc, PathGeometric &path)
Repairs a path to be valid in the current planning environment.
void setExperienceDB(const tools::ThunderDBPtr &experienceDB)
Pass a pointer of the database from the thunder framework.
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::size_t nearestPathsChosenID_
the ID within nearestPaths_ of the path that was chosen for repair
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
A shared pointer wrapper for ompl::base::Planner.
std::size_t getLastRecalledNearestPathChosen() const
Get debug information about the top recalled paths that were chosen for further filtering.
The planner did not find a solution for some other reason.
Definition: PlannerStatus.h:70
base::PlannerPtr repairPlanner_
A secondary planner for replanning.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
bool smoothingEnabled_
Optionally smooth retrieved and repaired paths from database.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
int nearestK_
Number of &#39;k&#39; close solutions to choose from database for further filtering.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
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...
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:49
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:421
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:427
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:230
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
The exception type for ompl.
Definition: Exception.h:46
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
const std::vector< PathGeometric > & getLastRecalledNearestPaths() const
Get debug information about the top recalled paths that were chosen for further filtering.
std::vector< PathGeometric > nearestPaths_
Recall the nearest paths and store this in planner data for introspection later.
void setRepairPlanner(const base::PlannerPtr &planner)
Set the planner that will be used for repairing invalid paths recalled from experience.
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
point now()
Get the current time point.
Definition: Time.h:70
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...
PathSimplifierPtr path_simplifier_
The instance of the path simplifier.
ThunderRetrieveRepair(const base::SpaceInformationPtr &si, tools::ThunderDBPtr experienceDB)
Constructor.
void restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again...
Definition: Planner.cpp:171
Definition of a geometric path.
Definition: PathGeometric.h:60
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:64
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
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...
void getPlannerData(base::PlannerData &data) const override
Get information about the exploration data structure the planning from scratch motion planner used...
A shared pointer wrapper for ompl::base::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...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68