Loading...
Searching...
No Matches
TSRRT.h
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#ifndef OMPL_GEOMETRIC_PLANNERS_RRT_TSRRT_
38#define OMPL_GEOMETRIC_PLANNERS_RRT_TSRRT_
39
40#include "ompl/base/ProjectionEvaluator.h"
41#include "ompl/geometric/planners/PlannerIncludes.h"
42#include "ompl/datastructures/NearestNeighbors.h"
43
44namespace ompl
45{
46 namespace geometric
47 {
48 OMPL_CLASS_FORWARD(TaskSpaceConfig);
49
51 {
52 public:
53 virtual ~TaskSpaceConfig()
54 {
55 }
56 // Returns the dimension of the task space.
57 virtual int getDimension() const = 0;
58 // Project the c-space state into the task-space.
59 virtual void project(const base::State *state, Eigen::Ref<Eigen::VectorXd> ts_proj) const = 0;
60
61 // Sample point uniformly in task-space.
62 virtual void sample(Eigen::Ref<Eigen::VectorXd> ts_proj) const = 0;
63
64 // Given a point in task-space, generate a configuraton space state
65 // that projects to this point. seed is the nearest task-space neighbor.
66 virtual bool lift(const Eigen::Ref<Eigen::VectorXd> &ts_proj, const base::State *seed,
67 base::State *state) const = 0;
68 };
69
87 class TSRRT : public base::Planner
88 {
89 public:
91 TSRRT(const base::SpaceInformationPtr &si, const TaskSpaceConfigPtr &task_space);
92
93 virtual ~TSRRT();
94
95 virtual void getPlannerData(base::PlannerData &data) const;
96
98
99 virtual void clear();
100
110 void setGoalBias(double goalBias)
111 {
112 goalBias_ = goalBias;
113 }
114
116 double getGoalBias() const
117 {
118 return goalBias_;
119 }
120
126 void setRange(double distance)
127 {
128 maxDistance_ = distance;
129 }
130
132 double getRange() const
133 {
134 return maxDistance_;
135 }
136
138 template <template <typename T> class NN>
140 {
141 nn_.reset(new NN<Motion *>());
142 }
143
144 virtual void setup();
145
146 protected:
151 class Motion
152 {
153 public:
155 Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(nullptr)
156 {
157 }
158
159 ~Motion() = default;
160
163
165 Motion *parent{nullptr};
166
167 // Projection of the state into the task space.
168 Eigen::VectorXd proj;
169 };
170
172 void freeMemory();
173
175 double distanceFunction(const Motion *a, const Motion *b) const
176 {
177 double sqr_dist = 0.0;
178 for (int ix = 0; ix < a->proj.size(); ++ix)
179 {
180 double sqr_val = (*b).proj[ix] - (*a).proj[ix];
181 sqr_val *= sqr_val;
182
183 sqr_dist += sqr_val;
184 }
185 return sqr_dist;
186 }
187
189 std::shared_ptr<NearestNeighbors<Motion *>> nn_;
190
193 double goalBias_{0.05};
194
196 double maxDistance_{0.};
197
200
203
204 // Mapping to/from task space.
205 TaskSpaceConfigPtr task_space_;
206 };
207
208 } // namespace geometric
209} // namespace ompl
210
211#endif
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:216
Definition of an abstract state.
Definition State.h:50
Representation of a motion.
Definition TSRRT.h:152
Motion * parent
The parent motion in the exploration tree.
Definition TSRRT.h:165
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Definition TSRRT.h:155
base::State * state
The state contained by the motion.
Definition TSRRT.h:162
Task-space Rapidly-exploring Random Trees.
Definition TSRRT.h:88
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)
Definition TSRRT.h:193
void freeMemory()
Free the memory allocated by this planner.
Definition TSRRT.cpp:77
void setRange(double distance)
Set the range the planner is supposed to use.
Definition TSRRT.h:126
RNG rng_
The random number generator.
Definition TSRRT.h:199
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition TSRRT.h:196
double getGoalBias() const
Get the goal bias the planner is using.
Definition TSRRT.h:116
double getRange() const
Get the range the planner is using.
Definition TSRRT.h:132
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition TSRRT.h:175
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
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition TSRRT.h:189
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
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition TSRRT.cpp:57
void setGoalBias(double goalBias)
Set the goal bias.
Definition TSRRT.h:110
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition TSRRT.h:139
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition TSRRT.cpp:66
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition TSRRT.h:202
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()