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 {
62 specs_.approximateSolutions = true;
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...
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:416
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition Planner.h:422
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition Planner.h:413
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:410
bool setup_
Flag indicating whether setup() has been called.
Definition Planner.h:433
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
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains code that is specific to planning under geometric constraints.
This namespace includes magic constants used in various places in OMPL.
Definition Constraint.h:52
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.
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:233