distance_field.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
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 Willow Garage 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: Mrinal Kalakrishnan, Ken Anderson, E. Gil Jones */
36 
40 #include <tf2_eigen/tf2_eigen.h>
41 #include <ros/console.h>
42 #include <octomap/octomap.h>
43 #include <octomap/OcTree.h>
44 
45 namespace distance_field
46 {
47 DistanceField::DistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x,
48  double origin_y, double origin_z)
49  : size_x_(size_x)
50  , size_y_(size_y)
51  , size_z_(size_z)
52  , origin_x_(origin_x)
53  , origin_y_(origin_y)
54  , origin_z_(origin_z)
55  , resolution_(resolution)
56  , inv_twice_resolution_(1.0 / (2.0 * resolution_))
57 {
58 }
59 
60 DistanceField::~DistanceField() = default;
61 
62 double DistanceField::getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y,
63  double& gradient_z, bool& in_bounds) const
64 {
65  int gx, gy, gz;
66 
67  worldToGrid(x, y, z, gx, gy, gz);
68 
69  // if out of bounds, return max_distance, and 0 gradient
70  // we need extra padding of 1 to get gradients
71  if (gx < 1 || gy < 1 || gz < 1 || gx >= getXNumCells() - 1 || gy >= getYNumCells() - 1 || gz >= getZNumCells() - 1)
72  {
73  gradient_x = 0.0;
74  gradient_y = 0.0;
75  gradient_z = 0.0;
76  in_bounds = false;
77  return getUninitializedDistance();
78  }
79 
80  gradient_x = (getDistance(gx + 1, gy, gz) - getDistance(gx - 1, gy, gz)) * inv_twice_resolution_;
81  gradient_y = (getDistance(gx, gy + 1, gz) - getDistance(gx, gy - 1, gz)) * inv_twice_resolution_;
82  gradient_z = (getDistance(gx, gy, gz + 1) - getDistance(gx, gy, gz - 1)) * inv_twice_resolution_;
83 
84  in_bounds = true;
85  return getDistance(gx, gy, gz);
86 }
87 
88 void DistanceField::getIsoSurfaceMarkers(double min_distance, double max_distance, const std::string& frame_id,
89  const ros::Time stamp, visualization_msgs::Marker& inf_marker) const
90 {
91  inf_marker.points.clear();
92  inf_marker.header.frame_id = frame_id;
93  inf_marker.header.stamp = stamp;
94  inf_marker.ns = "distance_field";
95  inf_marker.id = 1;
96  inf_marker.type = visualization_msgs::Marker::CUBE_LIST;
97  inf_marker.action = visualization_msgs::Marker::MODIFY;
98  inf_marker.scale.x = resolution_;
99  inf_marker.scale.y = resolution_;
100  inf_marker.scale.z = resolution_;
101  inf_marker.color.r = 1.0;
102  inf_marker.color.g = 0.0;
103  inf_marker.color.b = 0.0;
104  inf_marker.color.a = 0.1;
105  // inf_marker.lifetime = ros::Duration(30.0);
106 
107  inf_marker.points.reserve(100000);
108  for (int x = 0; x < getXNumCells(); ++x)
109  {
110  for (int y = 0; y < getYNumCells(); ++y)
111  {
112  for (int z = 0; z < getZNumCells(); ++z)
113  {
114  double dist = getDistance(x, y, z);
115 
116  if (dist >= min_distance && dist <= max_distance)
117  {
118  int last = inf_marker.points.size();
119  inf_marker.points.resize(last + 1);
120  double nx, ny, nz;
121  gridToWorld(x, y, z, nx, ny, nz);
122  Eigen::Translation3d vec(nx, ny, nz);
123  inf_marker.points[last].x = vec.x();
124  inf_marker.points[last].y = vec.y();
125  inf_marker.points[last].z = vec.z();
126  }
127  }
128  }
129  }
130 }
131 
132 void DistanceField::getGradientMarkers(double min_distance, double max_distance, const std::string& frame_id,
133  const ros::Time& stamp, visualization_msgs::MarkerArray& marker_array) const
134 {
135  Eigen::Vector3d unit_x(1, 0, 0);
136  Eigen::Vector3d unit_y(0, 1, 0);
137  Eigen::Vector3d unit_z(0, 0, 1);
138 
139  int id = 0;
140 
141  for (int x = 0; x < getXNumCells(); ++x)
142  {
143  for (int y = 0; y < getYNumCells(); ++y)
144  {
145  for (int z = 0; z < getZNumCells(); ++z)
146  {
147  double world_x, world_y, world_z;
148  gridToWorld(x, y, z, world_x, world_y, world_z);
149 
150  double gradient_x, gradient_y, gradient_z;
151  bool in_bounds;
152  double distance = getDistanceGradient(world_x, world_y, world_z, gradient_x, gradient_y, gradient_z, in_bounds);
153  Eigen::Vector3d gradient(gradient_x, gradient_y, gradient_z);
154 
155  if (in_bounds && distance >= min_distance && distance <= max_distance && gradient.norm() > 0)
156  {
157  visualization_msgs::Marker marker;
158 
159  marker.header.frame_id = frame_id;
160  marker.header.stamp = stamp;
161 
162  marker.ns = "distance_field_gradient";
163  marker.id = id++;
164  marker.type = visualization_msgs::Marker::ARROW;
165  marker.action = visualization_msgs::Marker::ADD;
166 
167  marker.pose.position.x = world_x;
168  marker.pose.position.y = world_y;
169  marker.pose.position.z = world_z;
170 
171  // Eigen::Vector3d axis = gradient.cross(unitX).norm() > 0 ? gradient.cross(unitX) : unitY;
172  // double angle = -gradient.angle(unitX);
173  // Eigen::AngleAxisd rotation(angle, axis);
174 
175  // marker.pose.orientation.x = rotation.linear().x();
176  // marker.pose.orientation.y = rotation.linear().y();
177  // marker.pose.orientation.z = rotation.linear().z();
178  // marker.pose.orientation.w = rotation.linear().w();
179 
180  marker.scale.x = getResolution();
181  marker.scale.y = getResolution();
182  marker.scale.z = getResolution();
183 
184  marker.color.r = 0.0;
185  marker.color.g = 0.0;
186  marker.color.b = 1.0;
187  marker.color.a = 1.0;
188 
189  marker_array.markers.push_back(marker);
190  }
191  }
192  }
193  }
194 }
195 
196 bool DistanceField::getShapePoints(const shapes::Shape* shape, const Eigen::Isometry3d& pose,
197  EigenSTL::vector_Vector3d* points)
198 {
199  if (shape->type == shapes::OCTREE)
200  {
201  const shapes::OcTree* oc = dynamic_cast<const shapes::OcTree*>(shape);
202  if (!oc)
203  {
204  ROS_ERROR_NAMED("distance_field", "Problem dynamic casting shape that claims to be OcTree");
205  return false;
206  }
207  getOcTreePoints(oc->octree.get(), points);
208  }
209  else
210  {
212  body->setDimensionsDirty(shape);
213  body->setPoseDirty(pose);
214  body->updateInternalData();
215  findInternalPointsConvex(*body, resolution_, *points);
216  delete body;
217  }
218  return true;
219 }
220 
221 void DistanceField::addShapeToField(const shapes::Shape* shape, const Eigen::Isometry3d& pose)
222 {
223  EigenSTL::vector_Vector3d point_vec;
224  getShapePoints(shape, pose, &point_vec);
225  addPointsToField(point_vec);
226 }
227 
228 // DEPRECATED
229 void DistanceField::addShapeToField(const shapes::Shape* shape, const geometry_msgs::Pose& pose)
230 {
231  Eigen::Isometry3d pose_e;
232  tf2::fromMsg(pose, pose_e);
233  addShapeToField(shape, pose_e);
234 }
235 
236 void DistanceField::getOcTreePoints(const octomap::OcTree* octree, EigenSTL::vector_Vector3d* points)
237 {
238  // lower extent
239  double min_x, min_y, min_z;
240  gridToWorld(0, 0, 0, min_x, min_y, min_z);
241 
242  octomap::point3d bbx_min(min_x, min_y, min_z);
243 
244  int num_x = getXNumCells();
245  int num_y = getYNumCells();
246  int num_z = getZNumCells();
247 
248  // upper extent
249  double max_x, max_y, max_z;
250  gridToWorld(num_x, num_y, num_z, max_x, max_y, max_z);
251 
252  octomap::point3d bbx_max(max_x, max_y, max_z);
253 
254  for (octomap::OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(bbx_min, bbx_max), end = octree->end_leafs_bbx();
255  it != end; ++it)
256  {
257  if (octree->isNodeOccupied(*it))
258  {
259  if (it.getSize() <= resolution_)
260  {
261  Eigen::Vector3d point(it.getX(), it.getY(), it.getZ());
262  points->push_back(point);
263  }
264  else
265  {
266  double ceil_val = ceil(it.getSize() / resolution_) * resolution_ / 2.0;
267  for (double x = it.getX() - ceil_val; x <= it.getX() + ceil_val; x += resolution_)
268  {
269  for (double y = it.getY() - ceil_val; y <= it.getY() + ceil_val; y += resolution_)
270  {
271  for (double z = it.getZ() - ceil_val; z <= it.getZ() + ceil_val; z += resolution_)
272  {
273  points->push_back(Eigen::Vector3d(x, y, z));
274  }
275  }
276  }
277  }
278  }
279  }
280 }
281 
282 void DistanceField::addOcTreeToField(const octomap::OcTree* octree)
283 {
284  EigenSTL::vector_Vector3d points;
285  getOcTreePoints(octree, &points);
286  addPointsToField(points);
287 }
288 
289 void DistanceField::moveShapeInField(const shapes::Shape* shape, const Eigen::Isometry3d& old_pose,
290  const Eigen::Isometry3d& new_pose)
291 {
292  if (shape->type == shapes::OCTREE)
293  {
294  ROS_WARN_NAMED("distance_field", "Move shape not supported for Octree");
295  return;
296  }
298  body->setDimensionsDirty(shape);
299  body->setPoseDirty(old_pose);
300  body->updateInternalData();
301  EigenSTL::vector_Vector3d old_point_vec;
302  findInternalPointsConvex(*body, resolution_, old_point_vec);
303  body->setPose(new_pose);
304  EigenSTL::vector_Vector3d new_point_vec;
305  findInternalPointsConvex(*body, resolution_, new_point_vec);
306  delete body;
307  updatePointsInField(old_point_vec, new_point_vec);
308 }
309 
310 // DEPRECATED
311 void DistanceField::moveShapeInField(const shapes::Shape* shape, const geometry_msgs::Pose& old_pose,
312  const geometry_msgs::Pose& new_pose)
313 {
314  Eigen::Isometry3d old_pose_e, new_pose_e;
315  tf2::fromMsg(old_pose, old_pose_e);
316  tf2::fromMsg(new_pose, new_pose_e);
317  moveShapeInField(shape, old_pose_e, new_pose_e);
318 }
319 
320 void DistanceField::removeShapeFromField(const shapes::Shape* shape, const Eigen::Isometry3d& pose)
321 {
323  body->setDimensionsDirty(shape);
324  body->setPoseDirty(pose);
325  body->updateInternalData();
326  EigenSTL::vector_Vector3d point_vec;
327  findInternalPointsConvex(*body, resolution_, point_vec);
328  delete body;
329  removePointsFromField(point_vec);
330 }
331 
332 // DEPRECATED
333 void DistanceField::removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::Pose& pose)
334 {
335  Eigen::Isometry3d pose_e;
336  tf2::fromMsg(pose, pose_e);
337  removeShapeFromField(shape, pose_e);
338 }
339 
340 void DistanceField::getPlaneMarkers(PlaneVisualizationType type, double length, double width, double height,
341  const Eigen::Vector3d& origin, const std::string& frame_id, const ros::Time stamp,
342  visualization_msgs::Marker& plane_marker) const
343 {
344  plane_marker.header.frame_id = frame_id;
345  plane_marker.header.stamp = stamp;
346  plane_marker.ns = "distance_field_plane";
347  plane_marker.id = 1;
348  plane_marker.type = visualization_msgs::Marker::CUBE_LIST;
349  plane_marker.action = visualization_msgs::Marker::ADD;
350  plane_marker.scale.x = resolution_;
351  plane_marker.scale.y = resolution_;
352  plane_marker.scale.z = resolution_;
353  // plane_marker.lifetime = ros::Duration(30.0);
354 
355  plane_marker.points.reserve(100000);
356  plane_marker.colors.reserve(100000);
357 
358  double min_x = 0;
359  double max_x = 0;
360  double min_y = 0;
361  double max_y = 0;
362  double min_z = 0;
363  double max_z = 0;
364 
365  switch (type)
366  {
367  case XY_PLANE:
368  min_z = height;
369  max_z = height;
370 
371  min_x = -length / 2.0;
372  max_x = length / 2.0;
373  min_y = -width / 2.0;
374  max_y = width / 2.0;
375  break;
376  case XZ_PLANE:
377  min_y = height;
378  max_y = height;
379 
380  min_x = -length / 2.0;
381  max_x = length / 2.0;
382  min_z = -width / 2.0;
383  max_z = width / 2.0;
384  break;
385  case YZ_PLANE:
386  min_x = height;
387  max_x = height;
388 
389  min_y = -length / 2.0;
390  max_y = length / 2.0;
391  min_z = -width / 2.0;
392  max_z = width / 2.0;
393  break;
394  }
395 
396  min_x += origin.x();
397  min_y += origin.y();
398  min_z += origin.z();
399 
400  max_x += origin.x();
401  max_y += origin.y();
402  max_z += origin.z();
403 
404  int min_x_cell = 0;
405  int max_x_cell = 0;
406  int min_y_cell = 0;
407  int max_y_cell = 0;
408  int min_z_cell = 0;
409  int max_z_cell = 0;
410 
411  worldToGrid(min_x, min_y, min_z, min_x_cell, min_y_cell, min_z_cell);
412  worldToGrid(max_x, max_y, max_z, max_x_cell, max_y_cell, max_z_cell);
413  plane_marker.color.a = 1.0;
414  for (int x = min_x_cell; x <= max_x_cell; ++x)
415  {
416  for (int y = min_y_cell; y <= max_y_cell; ++y)
417  {
418  for (int z = min_z_cell; z <= max_z_cell; ++z)
419  {
420  if (!isCellValid(x, y, z))
421  {
422  continue;
423  }
424  double dist = getDistance(x, y, z);
425  int last = plane_marker.points.size();
426  plane_marker.points.resize(last + 1);
427  plane_marker.colors.resize(last + 1);
428  double nx, ny, nz;
429  gridToWorld(x, y, z, nx, ny, nz);
430  Eigen::Vector3d vec(nx, ny, nz);
431  plane_marker.points[last].x = vec.x();
432  plane_marker.points[last].y = vec.y();
433  plane_marker.points[last].z = vec.z();
434  if (dist < 0.0)
435  {
436  plane_marker.colors[last].r = fmax(fmin(0.1 / fabs(dist), 1.0), 0.0);
437  plane_marker.colors[last].g = fmax(fmin(0.05 / fabs(dist), 1.0), 0.0);
438  plane_marker.colors[last].b = fmax(fmin(0.01 / fabs(dist), 1.0), 0.0);
439  }
440  else
441  {
442  plane_marker.colors[last].b = fmax(fmin(0.1 / (dist + 0.001), 1.0), 0.0);
443  plane_marker.colors[last].g = fmax(fmin(0.05 / (dist + 0.001), 1.0), 0.0);
444  plane_marker.colors[last].r = fmax(fmin(0.01 / (dist + 0.001), 1.0), 0.0);
445  }
446  }
447  }
448  }
449 }
450 
451 void DistanceField::setPoint(int xCell, int yCell, int zCell, double dist, geometry_msgs::Point& point,
452  std_msgs::ColorRGBA& color, double max_distance) const
453 {
454  double wx, wy, wz;
455  gridToWorld(xCell, yCell, zCell, wx, wy, wz);
456 
457  point.x = wx;
458  point.y = wy;
459  point.z = wz;
460 
461  color.r = 1.0;
462  color.g = dist / max_distance; // dist/max_distance * 0.5;
463  color.b = dist / max_distance; // dist/max_distance * 0.1;
464 }
465 
466 void DistanceField::getProjectionPlanes(const std::string& frame_id, const ros::Time& stamp, double max_dist,
467  visualization_msgs::Marker& marker) const
468 {
469  int max_x_cell = getXNumCells();
470  int max_y_cell = getYNumCells();
471  int max_z_cell = getZNumCells();
472 
473  double* x_projection = new double[max_y_cell * max_z_cell];
474  double* y_projection = new double[max_z_cell * max_x_cell];
475  double* z_projection = new double[max_x_cell * max_y_cell];
476  double initial_val = sqrt(INT_MAX);
477 
478  // Initialize
479  for (int y = 0; y < max_y_cell; y++)
480  for (int x = 0; x < max_x_cell; x++)
481  z_projection[x + y * max_x_cell] = initial_val;
482 
483  for (int z = 0; z < max_z_cell; z++)
484  for (int y = 0; y < max_y_cell; y++)
485  x_projection[y + z * max_y_cell] = initial_val;
486 
487  for (int z = 0; z < max_z_cell; z++)
488  for (int x = 0; x < max_x_cell; x++)
489  y_projection[x + z * max_x_cell] = initial_val;
490 
491  // Calculate projections
492  for (int z = 0; z < max_z_cell; z++)
493  {
494  for (int y = 0; y < max_y_cell; y++)
495  {
496  for (int x = 0; x < max_x_cell; x++)
497  {
498  double dist = getDistance(x, y, z);
499  z_projection[x + y * max_x_cell] = std::min(dist, z_projection[x + y * max_x_cell]);
500  x_projection[y + z * max_y_cell] = std::min(dist, x_projection[y + z * max_y_cell]);
501  y_projection[x + z * max_x_cell] = std::min(dist, y_projection[x + z * max_x_cell]);
502  }
503  }
504  }
505 
506  // Make markers
507  marker.points.clear();
508  marker.header.frame_id = frame_id;
509  marker.header.stamp = stamp;
510  marker.ns = "distance_field_projection_plane";
511  marker.id = 1;
512  marker.type = visualization_msgs::Marker::CUBE_LIST;
513  marker.action = visualization_msgs::Marker::MODIFY;
514  marker.scale.x = getResolution();
515  marker.scale.y = getResolution();
516  marker.scale.z = getResolution();
517  marker.color.a = 1.0;
518  // marker.lifetime = ros::Duration(30.0);
519 
520  int x, y, z;
521  int index = 0;
522  marker.points.resize(max_x_cell * max_y_cell + max_y_cell * max_z_cell + max_z_cell * max_x_cell);
523  marker.colors.resize(max_x_cell * max_y_cell + max_y_cell * max_z_cell + max_z_cell * max_x_cell);
524 
525  z = 0;
526  for (y = 0; y < max_y_cell; y++)
527  {
528  for (x = 0; x < max_x_cell; x++)
529  {
530  double dist = z_projection[x + y * max_x_cell];
531  setPoint(x, y, z, dist, marker.points[index], marker.colors[index], max_dist);
532  index++;
533  }
534  }
535 
536  x = 0;
537  for (z = 0; z < max_z_cell; z++)
538  {
539  for (y = 0; y < max_y_cell; y++)
540  {
541  double dist = x_projection[y + z * max_y_cell];
542  setPoint(x, y, z, dist, marker.points[index], marker.colors[index], max_dist);
543  index++;
544  }
545  }
546 
547  y = 0;
548  for (z = 0; z < max_z_cell; z++)
549  {
550  for (x = 0; x < max_x_cell; x++)
551  {
552  double dist = y_projection[x + z * max_x_cell];
553  setPoint(x, y, z, dist, marker.points[index], marker.colors[index], max_dist);
554  index++;
555  }
556  }
557 
558  if (x_projection)
559  delete[] x_projection;
560  if (y_projection)
561  delete[] y_projection;
562  if (z_projection)
563  delete[] z_projection;
564 }
565 
566 } // end of namespace distance_field
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::end_leafs_bbx
const leaf_bbx_iterator end_leafs_bbx() const
shapes::OcTree::octree
std::shared_ptr< const octomap::OcTree > octree
bodies::Body::setPoseDirty
void setPoseDirty(const Eigen::Isometry3d &pose)
tf2_eigen.h
tf2::fromMsg
void fromMsg(const A &, B &b)
bodies::Body::setDimensionsDirty
void setDimensionsDirty(const shapes::Shape *shape)
distance_field::XZ_PLANE
@ XZ_PLANE
Definition: distance_field.h:70
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
shapes::Shape
bodies::Body
distance_field::XY_PLANE
@ XY_PLANE
Definition: distance_field.h:69
getDistance
S getDistance(const Vector3< S > &p)
OcTree.h
octomath::Vector3
distance_field::PlaneVisualizationType
PlaneVisualizationType
The plane to visualize.
Definition: distance_field.h:67
octomap::OcTree
console.h
y
double y
bodies::createEmptyBodyFromShapeType
Body * createEmptyBodyFromShapeType(const shapes::ShapeType &shapeType)
distance_field::findInternalPointsConvex
void findInternalPointsConvex(const bodies::Body &body, double resolution, EigenSTL::vector_Vector3d &points)
Find all points on a regular grid that are internal to the body, assuming the body is a convex shape....
Definition: find_internal_points.cpp:39
shapes::Shape::type
ShapeType type
body_operations.h
bodies::Body::updateInternalData
virtual void updateInternalData()=0
find_internal_points.h
shapes::OcTree
distance_field::YZ_PLANE
@ YZ_PLANE
Definition: distance_field.h:71
shapes::OCTREE
OCTREE
distance_field.h
distance_field
Namespace for holding classes that generate distance fields.
Definition: distance_field.h:64
octomap::AbstractOccupancyOcTree::isNodeOccupied
bool isNodeOccupied(const OcTreeNode &occupancyNode) const
point
std::chrono::system_clock::time_point point
ros::Time
distance_field::DistanceField::DistanceField
DistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, double origin_z)
Constructor, where units are arbitrary but are assumed to be meters.
Definition: distance_field.cpp:79
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
octomap.h
index
unsigned int index
bodies::Body::setPose
void setPose(const Eigen::Isometry3d &pose)
x
double x
length
TF2SIMD_FORCE_INLINE tf2Scalar length(const Quaternion &q)
pr2_arm_kinematics::distance
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:86
z
double z
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::begin_leafs_bbx
leaf_bbx_iterator begin_leafs_bbx(const OcTreeKey &min, const OcTreeKey &max, unsigned char maxDepth=0) const
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Oct 12 2021 02:24:44