MRPT  1.9.9
PlannerRRT_SE2_TPS.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "nav-precomp.h" // Precomp header
11 
14 #include <mrpt/random.h>
15 #include <mrpt/system/CTicTac.h>
16 #include <mrpt/system/filesystem.h>
17 
18 using namespace mrpt::nav;
19 using namespace mrpt::math;
20 using namespace mrpt::system;
21 using namespace mrpt::poses;
22 using namespace std;
23 
24 MRPT_TODO("Optimize getNearestNode() with KD-tree!")
25 
27 /** Load all params from a config file source */
28 void PlannerRRT_SE2_TPS::loadConfig(
29  const mrpt::config::CConfigFileBase& ini, const std::string& sSect)
30 {
32 }
33 
34 /** Must be called after setting all params (see `loadConfig()`) and before
35  * calling `solve()` */
37 {
39 
40  m_initialized = true;
41 }
42 
43 /** The main API entry point: tries to find a planned path from 'goal' to
44  * 'target' */
48 {
49  mrpt::system::CTimeLoggerEntry tleg(m_timelogger, "PT_RRT::solve");
50 
51  // Sanity checks:
52  ASSERTMSG_(m_initialized, "initialize() must be called before!");
53 
54  // Calc maximum vehicle shape radius:
55  double max_veh_radius = 0.;
56  for (const auto& ptg : m_PTGs)
57  mrpt::keep_max(max_veh_radius, ptg->getMaxRobotRadius());
58 
59  // [Algo `tp_space_rrt`: Line 1]: Init tree adding the initial pose
60  if (result.move_tree.getAllNodes().empty())
61  {
62  result.move_tree.root = 0;
63  result.move_tree.insertNode(
64  result.move_tree.root, TNodeSE2_TP(pi.start_pose));
65  }
66 
67  mrpt::system::CTicTac working_time;
68  working_time.Tic();
69  size_t rrt_iter_counter = 0;
70 
71  size_t SAVE_3D_TREE_LOG_DECIMATION_CNT = 0;
72  static size_t SAVE_LOG_SOLVE_COUNT = 0;
73  SAVE_LOG_SOLVE_COUNT++;
74 
75  // Keep track of the best solution so far:
76  // By reusing the contents of "result" we make the algorithm re-callable
77  // ("any-time" algorithm) to refine results
78 
79  // [Algo `tp_space_rrt`: Line 2]: Iterate
80  // ------------------------------------------
81  for (;;)
82  {
83  // Check end conditions:
84  const double elap_tim = working_time.Tac();
85  if ((end_criteria.maxComputationTime > 0 &&
86  elap_tim > end_criteria.maxComputationTime) // Max comp time
87  ||
88  (result.goal_distance < end_criteria.acceptedDistToTarget &&
89  elap_tim >= end_criteria.minComputationTime) // Reach closer than
90  // this to target
91  )
92  {
93  break;
94  }
95 
96  // [Algo `tp_space_rrt`: Line 3]: sample random state (with goal
97  // biasing)
98  // -----------------------------------------
99  node_pose_t x_rand;
100  // bool rand_is_target=false;
101  if (mrpt::random::getRandomGenerator().drawUniform(0.0, 1.0) <
102  params.goalBias)
103  {
104  x_rand = pi.goal_pose;
105  // rand_is_target=true;
106  }
107  else
108  {
109  // Sample uniform:
110  for (int i = 0; i < node_pose_t::static_size; i++)
112  pi.world_bbox_min[i], pi.world_bbox_max[i]);
113  }
114  const CPose2D x_rand_pose(x_rand);
115 
116  // [Algo `tp_space_rrt`: Line 4]: Init empty solution set
117  // -----------------------------------------
118  using sorted_solution_list_t = std::map<double, TMoveEdgeSE2_TP>;
119  sorted_solution_list_t candidate_new_nodes; // Map: cost -> info. Pick
120  // begin() to select the
121  // lowest-cose one.
122 
124  distance_evaluator_se2; // Plain distances in SE(2), not along PTGs
125  bool is_new_best_solution = false; // Just for logging purposes
126 
127  //#define DO_LOG_TXTS
128  std::string sLogTxt;
129 
130  // [Algo `tp_space_rrt`: Line 5]: For each PTG
131  // -----------------------------------------
132  const size_t nPTGs = m_PTGs.size();
133  for (size_t idxPTG = 0; idxPTG < nPTGs; ++idxPTG)
134  {
135  rrt_iter_counter++;
136 
137  // [Algo `tp_space_rrt`: Line 5]: Search nearest neig. to x_rand
138  // -----------------------------------------------
139  const PoseDistanceMetric<TNodeSE2_TP> distance_evaluator(
140  *m_PTGs[idxPTG]);
141 
142  const TNodeSE2_TP query_node(x_rand);
143 
144  m_timelogger.enter("TMoveTree::getNearestNode");
145  mrpt::graphs::TNodeID x_nearest_id =
146  result.move_tree.getNearestNode(query_node, distance_evaluator);
147  m_timelogger.leave("TMoveTree::getNearestNode");
148 
149  if (x_nearest_id == INVALID_NODEID)
150  {
151  // We can't find any close node, at least with this PTG's paths:
152  // skip
153 
154  // Before that, save log:
155  if (params.save_3d_log_freq > 0 &&
156  (++SAVE_3D_TREE_LOG_DECIMATION_CNT >=
157  params.save_3d_log_freq))
158  {
159  SAVE_3D_TREE_LOG_DECIMATION_CNT =
160  0; // Reset decimation counter
161  TRenderPlannedPathOptions render_options;
162  render_options.highlight_path_to_node_id =
163  result.best_goal_node_id;
164  render_options.highlight_last_added_edge = false;
165  render_options.x_rand_pose = &x_rand_pose;
166  render_options.log_msg = "SKIP: Can't find any close node";
167  render_options.log_msg_position = mrpt::math::TPoint3D(
168  pi.world_bbox_min.x, pi.world_bbox_min.y, 0);
169  render_options.ground_xy_grid_frequency = 1.0;
170 
172  renderMoveTree(scene, pi, result, render_options);
173  mrpt::system::createDirectory("./rrt_log_trees");
174  scene.saveToFile(mrpt::format(
175  "./rrt_log_trees/rrt_log_%03u_%06u.3Dscene",
176  static_cast<unsigned int>(SAVE_LOG_SOLVE_COUNT),
177  static_cast<unsigned int>(rrt_iter_counter)));
178  }
179 
180  continue; // Skip
181  }
182 
183  const TNodeSE2_TP& x_nearest_node =
184  result.move_tree.getAllNodes().find(x_nearest_id)->second;
185 
186  // [Algo `tp_space_rrt`: Line 6]: Relative target
187  // -----------------------------------------------
188  const CPose2D x_nearest_pose(x_nearest_node.state);
189  const CPose2D x_rand_rel = x_rand_pose - x_nearest_pose;
190 
191  // [Algo `tp_space_rrt`: Line 7]: Relative target in TP-Space
192  // ------------------------------------------------------------
193  const double D_max =
194  std::min(params.maxLength, m_PTGs[idxPTG]->getRefDistance());
195 
196  double d_rand; // Coordinates in TP-space
197  int k_rand; // k_rand is the index of target_alpha in PTGs
198  // corresponding to a specific d_rand
199  // bool tp_point_is_exact =
200  m_PTGs[idxPTG]->inverseMap_WS2TP(
201  x_rand_rel.x(), x_rand_rel.y(), k_rand, d_rand);
202  d_rand *=
203  m_PTGs[idxPTG]
204  ->getRefDistance(); // distance to target, in "real meters"
205 
206  float d_free;
207  // bool local_obs_ok = false; // Just for 3D log files: indicates
208  // whether obstacle points have been recomputed
209 
210  // [Algo `tp_space_rrt`: Line 8]: TP-Obstacles
211  // ------------------------------------------------------------
212  // Transform obstacles as seen from x_nearest_node -> TP_obstacles
213  double TP_Obstacles_k_rand = .0; // vector<double> TP_Obstacles;
214  const double MAX_DIST_FOR_OBSTACLES =
215  1.5 * m_PTGs[idxPTG]->getRefDistance(); // Maximum Euclidean
216  // distance (radius)
217  // for considering
218  // obstacles around the
219  // current robot pose
220 
222  m_PTGs[idxPTG]->getRefDistance(),
223  1.1 * max_veh_radius); // Make sure the PTG covers at least a
224  // bit more than the vehicle shape!!
225  // (should be much, much higher)
226 
227  {
228  CTimeLoggerEntry tle(
229  m_timelogger, "PT_RRT::solve.changeCoordinatesReference");
230  transformPointcloudWithSquareClipping(
231  pi.obstacles_points, m_local_obs,
232  CPose2D(x_nearest_node.state), MAX_DIST_FOR_OBSTACLES);
233  // local_obs_ok=true;
234  }
235  {
236  CTimeLoggerEntry tle(
237  m_timelogger, "PT_RRT::solve.SpaceTransformer");
238  spaceTransformerOneDirectionOnly(
239  k_rand, m_local_obs, m_PTGs[idxPTG].get(),
240  MAX_DIST_FOR_OBSTACLES, TP_Obstacles_k_rand);
241  }
242 
243  // directions k_rand in TP_obstacles[k_rand] = d_free
244  // this is the collision free distance to the TP_target
245  d_free = TP_Obstacles_k_rand; // TP_Obstacles[k_rand];
246 
247  // [Algo `tp_space_rrt`: Line 10]: d_new
248  // ------------------------------------------------------------
249  double d_new = std::min(
250  D_max,
251  d_rand); // distance of the new candidate state in TP-space
252 
253  // mrpt::poses::CPose2D *log_new_state_ptr=NULL; // For graphical
254  // logs only
255 
256 #ifdef DO_LOG_TXTS
257  sLogTxt += mrpt::format(
258  "tp_idx=%u tp_exact=%c\n d_free: %f d_rand=%f d_new=%f\n",
259  static_cast<unsigned int>(idxPTG),
260  tp_point_is_exact ? 'Y' : 'N', d_free, d_rand, d_new);
261  sLogTxt += mrpt::format(
262  " nearest:%s\n", x_nearest_pose.asString().c_str());
263 #endif
264 
265  // [Algo `tp_space_rrt`: Line 13]: Do we have free space?
266  // ------------------------------------------------------------
267  if (d_free >= d_new)
268  {
269  // [Algo `tp_space_rrt`: Line 14]: PTG function
270  // ------------------------------------------------------------
271  // given d_rand and k_rand provides x,y,phi of the point in
272  // c-space
273  uint32_t nStep;
274  m_PTGs[idxPTG]->getPathStepForDist(k_rand, d_new, nStep);
275 
276  mrpt::math::TPose2D rel_pose;
277  m_PTGs[idxPTG]->getPathPose(k_rand, nStep, rel_pose);
278 
279  mrpt::math::wrapToPiInPlace(rel_pose.phi); // wrap to [-pi,pi]
280  // -->avoid out of
281  // bounds errors
282 
283  // [Algo `tp_space_rrt`: Line 15]: pose composition
284  // ------------------------------------------------------------
285  const mrpt::poses::CPose2D new_state_rel(rel_pose);
286  mrpt::poses::CPose2D new_state =
287  x_nearest_pose + new_state_rel; // compose the new_motion
288  // as the last nmotion and
289  // the new state
290  // log_new_state_ptr = &new_state;
291 
292  // Check whether there's already a too-close node around:
293  // --------------------------------------------------------
294  bool accept_this_node = true;
295 
296  // Is this a potential solution
297  const double goal_dist =
298  new_state.distance2DTo(pi.goal_pose.x, pi.goal_pose.y);
299  const double goal_ang = std::abs(
300  mrpt::math::angDistance(new_state.phi(), pi.goal_pose.phi));
301  const bool is_acceptable_goal =
302  (goal_dist < end_criteria.acceptedDistToTarget) &&
303  (goal_ang < end_criteria.acceptedAngToTarget);
304 
305  auto new_nearest_id = INVALID_NODEID;
306  if (!is_acceptable_goal) // Only check for nearby nodes if this
307  // is not a solution!
308  {
309  double new_nearest_dist;
310  const TNodeSE2 new_state_node(new_state.asTPose());
311 
312  m_timelogger.enter("TMoveTree::getNearestNode");
313  new_nearest_id = result.move_tree.getNearestNode(
314  new_state_node, distance_evaluator_se2,
315  &new_nearest_dist, &result.acceptable_goal_node_ids);
316  m_timelogger.leave("TMoveTree::getNearestNode");
317 
318  if (new_nearest_id != INVALID_NODEID)
319  {
320  // Also check angular distance:
321  const double new_nearest_ang =
322  std::abs(mrpt::math::angDistance(
323  new_state.phi(), result.move_tree.getAllNodes()
324  .find(new_nearest_id)
325  ->second.state.phi));
326  accept_this_node =
327  (new_nearest_dist >=
328  params.minDistanceBetweenNewNodes ||
329  new_nearest_ang >= params.minAngBetweenNewNodes);
330  }
331  }
332 
333  if (!accept_this_node)
334  {
335 #ifdef DO_LOG_TXTS
336  if (new_nearest_id != INVALID_NODEID)
337  {
338  sLogTxt += mrpt::format(
339  " -> new node NOT accepted for closeness to: %s\n",
340  result.move_tree.getAllNodes()
341  .find(new_nearest_id)
342  ->second.state.asString()
343  .c_str());
344  }
345 #endif
346  continue; // Too close node, skip!
347  }
348 
349  // [Algo `tp_space_rrt`: Line 16]: Add to candidate solution set
350  // ------------------------------------------------------------
351  // Create "movement" (tree edge) object:
352  TMoveEdgeSE2_TP new_edge(x_nearest_id, new_state.asTPose());
353 
354  new_edge.cost = d_new;
355  new_edge.ptg_index = idxPTG;
356  new_edge.ptg_K = k_rand;
357  new_edge.ptg_dist = d_new;
358 
359  candidate_new_nodes[new_edge.cost] = new_edge;
360 
361  } // end if the path is obstacle free
362  else
363  {
364 #ifdef DO_LOG_TXTS
365  sLogTxt += mrpt::format(" -> d_free NOT < d_rand\n");
366 #endif
367  }
368 
369  } // end for idxPTG
370 
371  // [Algo `tp_space_rrt`: Line 19]: Any solution found?
372  // ------------------------------------------------------------
373  if (!candidate_new_nodes.empty())
374  {
375  const TMoveEdgeSE2_TP& best_edge =
376  candidate_new_nodes.begin()->second;
377  const TNodeSE2_TP new_state_node(best_edge.end_state);
378 
379  // Insert into the tree:
380  const mrpt::graphs::TNodeID new_child_id =
381  result.move_tree.getNextFreeNodeID();
383  best_edge.parent_id, new_child_id, new_state_node, best_edge);
384 
385  // Distance to goal:
386  const double goal_dist =
388  .distance2DTo(pi.goal_pose.x, pi.goal_pose.y);
389  const double goal_ang = std::abs(mrpt::math::angDistance(
390  best_edge.end_state.phi, pi.goal_pose.phi));
391 
392  const bool is_acceptable_goal =
393  (goal_dist < end_criteria.acceptedDistToTarget) &&
394  (goal_ang < end_criteria.acceptedAngToTarget);
395 
396  if (is_acceptable_goal)
397  result.acceptable_goal_node_ids.insert(new_child_id);
398 
399  // Total path length:
400  double this_path_cost = std::numeric_limits<double>::max();
401  if (is_acceptable_goal) // Don't waste time computing path length
402  // if it doesn't matter anyway
403  {
404  TMoveTreeSE2_TP::path_t candidate_solution_path;
405  result.move_tree.backtrackPath(
406  new_child_id, candidate_solution_path);
407  this_path_cost = 0;
408  for (auto it = candidate_solution_path.begin();
409  it != candidate_solution_path.end(); ++it)
410  if (it->edge_to_parent)
411  this_path_cost += it->edge_to_parent->cost;
412  }
413 
414  // Check if this should be the new optimal path:
415  if (is_acceptable_goal && this_path_cost < result.path_cost)
416  {
417  result.goal_distance = goal_dist;
418  result.path_cost = this_path_cost;
419 
420  result.best_goal_node_id = new_child_id;
421  is_new_best_solution = true;
422  }
423  } // end if any candidate found
424 
425  // Graphical logging, if enabled:
426  // ------------------------------------------------------
427  if (params.save_3d_log_freq > 0 &&
428  (++SAVE_3D_TREE_LOG_DECIMATION_CNT >= params.save_3d_log_freq ||
429  is_new_best_solution))
430  {
431  CTimeLoggerEntry tle(
432  m_timelogger, "PT_RRT::solve.generate_log_files");
433  SAVE_3D_TREE_LOG_DECIMATION_CNT = 0; // Reset decimation counter
434 
435  // Render & save to file:
436  TRenderPlannedPathOptions render_options;
437  render_options.highlight_path_to_node_id = result.best_goal_node_id;
438  render_options.x_rand_pose = &x_rand_pose;
439  // render_options.x_nearest_pose = &x_nearest_pose;
440  // if (local_obs_ok) render_options.local_obs_from_nearest_pose =
441  // &m_local_obs;
442  // render_options.new_state = log_new_state_ptr;
443  render_options.highlight_last_added_edge = true;
444  render_options.ground_xy_grid_frequency = 1.0;
445 
446  render_options.log_msg = sLogTxt;
447  render_options.log_msg_position = mrpt::math::TPoint3D(
448  pi.world_bbox_min.x, pi.world_bbox_min.y, 0);
449 
451  renderMoveTree(scene, pi, result, render_options);
452 
453  mrpt::system::createDirectory("./rrt_log_trees");
454  scene.saveToFile(mrpt::format(
455  "./rrt_log_trees/rrt_log_%03u_%06u.3Dscene",
456  static_cast<unsigned int>(SAVE_LOG_SOLVE_COUNT),
457  static_cast<unsigned int>(rrt_iter_counter)));
458  }
459 
460  } // end loop until end conditions
461 
462  // [Algo `tp_space_rrt`: Line 17]: Tree back trace
463  // ------------------------------------------------------------
464  result.success = (result.goal_distance < end_criteria.acceptedDistToTarget);
465  result.computation_time = working_time.Tac();
466 
467 } // end solve()
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:161
mrpt::math::TPose2D state
state in SE2 as 2D pose (x, y, phi)
Definition: TMoveTree.h:245
mrpt::graphs::TNodeID getNextFreeNodeID() const
Definition: TMoveTree.h:139
void internal_loadConfig_PTG(const mrpt::config::CConfigFileBase &cfgSource, const std::string &sSectionName=std::string("PTG_CONFIG"))
Load all PTG params from a config file source.
std::set< mrpt::graphs::TNodeID > acceptable_goal_node_ids
The set of target nodes within an acceptable distance to target (including best_goal_node_id and othe...
mrpt::graphs::TNodeID best_goal_node_id
The ID of the best target node in the tree.
A safe way to call enter() and leave() of a mrpt::system::CTimeLogger upon construction and destructi...
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::graphs::TNodeID parent_id
The ID of the parent node in the tree.
Definition: TMoveTree.h:185
Pose metric for SE(2) limited to a given PTG manifold.
Definition: TMoveTree.h:253
A high-performance stopwatch, with typical resolution of nanoseconds.
T angDistance(T from, T to)
Computes the shortest angular increment (or distance) between two planar orientations, such that it is constrained to [-pi,pi] and is correct for any combination of angles (e.g.
Definition: wrap2pi.h:95
world_limits_t world_bbox_min
Bounding box of the world, used to draw uniform random pose samples.
STL namespace.
void insertNodeAndEdge(const mrpt::graphs::TNodeID parent_id, const mrpt::graphs::TNodeID new_child_id, const NODE_TYPE_DATA &new_child_node_data, const EDGE_TYPE &new_edge_data)
Definition: TMoveTree.h:114
const node_map_t & getAllNodes() const
Definition: TMoveTree.h:140
std::list< node_t > path_t
A topological path up-tree.
Definition: TMoveTree.h:80
void initialize()
Must be called after setting all params (see loadConfig()) and before calling solve() ...
double cost
cost associated to each motion, this should be defined by the user according to a spefic cost functio...
Definition: TMoveTree.h:191
mrpt::math::TPose2D asTPose() const
Definition: CPose2D.cpp:468
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:61
This base provides a set of functions for maths stuff.
bool success
Whether the target was reached or not.
mrpt::config::CConfigFileBase CConfigFileBase
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
TNodeID root
The root of the tree.
Definition: CDirectedTree.h:79
GLsizei const GLchar ** string
Definition: glext.h:4116
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double ground_xy_grid_frequency
(Default=10 meters) Set to 0 to disable
bool saveToFile(const std::string &fil) const
Saves the scene to a 3Dscene file, loadable by the application SceneViewer3D.
double path_cost
Total cost of the best found path (cost ~~ Euclidean distance)
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
void internal_initialize_PTG()
Must be called after setting all params (see internal_loadConfig_PTG()) and before calling solve() ...
void solve(const TPlannerInput &pi, TPlannerResult &result)
The main API entry point: tries to find a planned path from &#39;goal&#39; to &#39;target&#39;.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::maps::CSimplePointsMap obstacles_points
World obstacles, as a point cloud.
double goal_distance
Distance from best found path to goal.
double computation_time
Time spent (in secs)
#define MRPT_TODO(x)
Definition: common.h:129
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
double distance2DTo(double ax, double ay) const
Returns the 2D distance from this pose/point to a 2D point (ignores Z, if it exists).
Definition: CPoseOrPoint.h:237
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
void insertNode(const mrpt::graphs::TNodeID node_id, const NODE_TYPE_DATA &node_data)
Insert a node without edges (should be used only for a tree root node)
Definition: TMoveTree.h:133
Lightweight 2D pose.
Definition: TPose2D.h:22
mrpt::graphs::TNodeID getNearestNode(const NODE_TYPE_FOR_METRIC &query_pt, const PoseDistanceMetric< NODE_TYPE_FOR_METRIC > &distanceMetricEvaluator, double *out_distance=nullptr, const std::set< mrpt::graphs::TNodeID > *ignored_nodes=nullptr) const
Finds the nearest node to a given pose, using the given metric.
Definition: TMoveTree.h:84
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:58
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:16
An edge for the move tree used for planning in SE2 and TP-space.
Definition: TMoveTree.h:182
#define INVALID_NODEID
Definition: TNodeID.h:19
mrpt::math::TPose2D end_state
state in SE2 as 2D pose (x, y, phi) -
Definition: TMoveTree.h:188
mrpt::graphs::TNodeID highlight_path_to_node_id
Highlight the path from root towards this node (usually, the target)
void backtrackPath(const mrpt::graphs::TNodeID target_node, path_t &out_path) const
Builds the path (sequence of nodes, with info about next edge) up-tree from a target_node towards the...
Definition: TMoveTree.h:145
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
Lightweight 3D point.
Definition: TPoint3D.h:90
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
GLenum const GLfloat * params
Definition: glext.h:3538
TP Space-based RRT path planning for SE(2) (planar) robots.
double phi
Orientation (rads)
Definition: TPose2D.h:32
tree_t move_tree
The generated motion tree that explores free space starting at "start".



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019