Loading...
Searching...
No Matches
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
51namespace 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
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
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
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
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
411
412 void ThunderRetrieveRepair::getRepairPlannerDatas(std::vector<base::PlannerDataPtr> &data) const
413 {
414 data = repairPlannerDatas_;
415 }
416
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,...
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.
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.
STL namespace.
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()
@ ABORT
The planner did not find a solution for some other reason.
@ TIMEOUT
The planner failed to find a solution.
@ UNKNOWN
Uninitialized status.
Struct for passing around partially solved solutions.
Definition SPARSdb.h:234