collision_env_distance_field.h
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 the 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: E. Gil Jones */
36 
37 #pragma once
38 
44 #include <boost/thread/mutex.hpp>
45 
46 namespace collision_detection
47 {
48 static const double DEFAULT_SIZE_X = 3.0;
49 static const double DEFAULT_SIZE_Y = 3.0;
50 static const double DEFAULT_SIZE_Z = 4.0;
51 static const bool DEFAULT_USE_SIGNED_DISTANCE_FIELD = false;
52 static const double DEFAULT_RESOLUTION = .02;
53 static const double DEFAULT_COLLISION_TOLERANCE = 0.0;
54 static const double DEFAULT_MAX_PROPOGATION_DISTANCE = .25;
55 
56 MOVEIT_CLASS_FORWARD(CollisionEnvDistanceField); // Defines CollisionEnvDistanceFieldPtr, ConstPtr, WeakPtr... etc
57 
58 class CollisionEnvDistanceField : public CollisionEnv
59 {
60 public:
61  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
62 
63  CollisionEnvDistanceField(const moveit::core::RobotModelConstPtr& robot_model,
64  const std::map<std::string, std::vector<CollisionSphere>>& link_body_decompositions =
65  std::map<std::string, std::vector<CollisionSphere>>(),
66  double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y,
67  double size_z = DEFAULT_SIZE_Z, const Eigen::Vector3d& origin = Eigen::Vector3d(0, 0, 0),
68  bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD,
69  double resolution = DEFAULT_RESOLUTION,
70  double collision_tolerance = DEFAULT_COLLISION_TOLERANCE,
71  double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0,
72  double scale = 1.0);
73 
74  CollisionEnvDistanceField(const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world,
75  const std::map<std::string, std::vector<CollisionSphere>>& link_body_decompositions =
76  std::map<std::string, std::vector<CollisionSphere>>(),
77  double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y,
78  double size_z = DEFAULT_SIZE_Z, const Eigen::Vector3d& origin = Eigen::Vector3d(0, 0, 0),
79  bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD,
80  double resolution = DEFAULT_RESOLUTION,
81  double collision_tolerance = DEFAULT_COLLISION_TOLERANCE,
82  double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0,
83  double scale = 1.0);
84 
85  CollisionEnvDistanceField(const CollisionEnvDistanceField& other, const WorldPtr& world);
86 
87  void initialize(const std::map<std::string, std::vector<CollisionSphere>>& link_body_decompositions,
88  const Eigen::Vector3d& size, const Eigen::Vector3d& origin, bool use_signed_distance_field,
89  double resolution, double collision_tolerance, double max_propogation_distance);
90 
92  const moveit::core::RobotState& state) const override;
93 
95  const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const;
96 
98  const moveit::core::RobotState& state,
99  const collision_detection::AllowedCollisionMatrix& acm) const override;
100 
103  GroupStateRepresentationPtr& gsr) const;
104 
106  visualization_msgs::MarkerArray& model_markers) const;
107 
108  virtual double distanceSelf(const moveit::core::RobotState& /* state */) const
109  {
110  return 0.0;
111  }
112 
113  virtual double distanceSelf(const moveit::core::RobotState& /* state */,
114  const collision_detection::AllowedCollisionMatrix& /* acm */) const
115  {
116  return 0.0;
117  }
118 
119  void distanceSelf(const DistanceRequest& /* req */, DistanceResult& /* res */,
120  const moveit::core::RobotState& /* state */) const override
121  {
122  ROS_ERROR_NAMED("collision_distance_field", "Not implemented");
123  }
124 
125  DistanceFieldCacheEntryConstPtr getLastDistanceFieldEntry() const
126  {
128  }
129 
130  // void getSelfCollisionsGradients(const collision_detection::CollisionRequest
131  // &req,
132  // collision_detection::CollisionResult &res,
133  // const moveit::core::RobotState &state,
134  // const
135  // collision_detection::AllowedCollisionMatrix
136  // &acm) const;
137 
138  MOVEIT_STRUCT_FORWARD(DistanceFieldCacheEntryWorld);
139  struct DistanceFieldCacheEntryWorld
140  {
141  std::map<std::string, std::vector<PosedBodyPointDecompositionPtr>> posed_body_point_decompositions_;
142  distance_field::DistanceFieldPtr distance_field_;
143  };
144 
146 
147  void checkCollision(const CollisionRequest& req, CollisionResult& res,
148  const moveit::core::RobotState& state) const override;
149 
150  virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state,
151  GroupStateRepresentationPtr& gsr) const;
152 
153  void checkCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state,
154  const AllowedCollisionMatrix& acm) const override;
155 
156  virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state,
157  const AllowedCollisionMatrix& acm, GroupStateRepresentationPtr& gsr) const;
158 
160  const moveit::core::RobotState& state) const override;
161 
162  virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
163  const moveit::core::RobotState& state, GroupStateRepresentationPtr& gsr) const;
164 
166  const AllowedCollisionMatrix& acm) const override;
167 
168  virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
169  const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm,
170  GroupStateRepresentationPtr& gsr) const;
171 
173  const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override;
174 
176  const moveit::core::RobotState& state2) const override;
177 
178  virtual double distanceRobot(const moveit::core::RobotState& state, bool verbose = false) const
179  {
180  (void)state;
181  (void)verbose;
182  return 0.0;
183  }
184 
185  virtual double distanceRobot(const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm,
186  bool verbose = false) const
187  {
188  (void)state;
189  (void)acm;
190  (void)verbose;
191  return 0.0;
192  }
193 
194  void distanceRobot(const DistanceRequest& /* req */, DistanceResult& /* res */,
195  const moveit::core::RobotState& /* state */) const override
196  {
197  ROS_ERROR_NAMED("collision_distance_field", "Not implemented");
198  }
199 
200  void setWorld(const WorldPtr& world) override;
201 
202  distance_field::DistanceFieldConstPtr getDistanceField() const
203  {
204  return distance_field_cache_entry_->distance_field_;
205  }
206 
207  collision_detection::GroupStateRepresentationConstPtr getLastGroupStateRepresentation() const
208  {
209  return last_gsr_;
210  }
211 
213  const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const;
214 
216  const AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr) const;
217 
218 protected:
219  bool getSelfProximityGradients(GroupStateRepresentationPtr& gsr) const;
220 
221  bool getIntraGroupProximityGradients(GroupStateRepresentationPtr& gsr) const;
222 
224  GroupStateRepresentationPtr& gsr) const;
225 
227  collision_detection::CollisionResult& res, GroupStateRepresentationPtr& gsr) const;
228 
232  GroupStateRepresentationPtr& gsr) const;
233 
235  GroupStateRepresentationPtr& gsr) const;
236 
237  void generateCollisionCheckingStructures(const std::string& group_name, const moveit::core::RobotState& state,
239  GroupStateRepresentationPtr& gsr, bool generate_distance_field) const;
240 
241  DistanceFieldCacheEntryConstPtr
242  getDistanceFieldCacheEntry(const std::string& group_name, const moveit::core::RobotState& state,
244 
245  DistanceFieldCacheEntryPtr generateDistanceFieldCacheEntry(const std::string& group_name,
246  const moveit::core::RobotState& state,
248  bool generate_distance_field) const;
249 
250  void addLinkBodyDecompositions(double resolution);
251 
252  void addLinkBodyDecompositions(double resolution,
253  const std::map<std::string, std::vector<CollisionSphere>>& link_body_decompositions);
254 
255  PosedBodySphereDecompositionPtr getPosedLinkBodySphereDecomposition(const moveit::core::LinkModel* ls,
256  unsigned int ind) const;
257 
258  PosedBodyPointDecompositionPtr getPosedLinkBodyPointDecomposition(const moveit::core::LinkModel* ls) const;
259 
260  void getGroupStateRepresentation(const DistanceFieldCacheEntryConstPtr& dfce, const moveit::core::RobotState& state,
261  GroupStateRepresentationPtr& gsr) const;
262 
263  bool compareCacheEntryToState(const DistanceFieldCacheEntryConstPtr& dfce,
264  const moveit::core::RobotState& state) const;
265 
266  bool compareCacheEntryToAllowedCollisionMatrix(const DistanceFieldCacheEntryConstPtr& dfce,
268 
269  void updatedPaddingOrScaling(const std::vector<std::string>& /*links*/) override{};
270 
271  DistanceFieldCacheEntryWorldPtr generateDistanceFieldCacheEntryWorld();
272 
273  void updateDistanceObject(const std::string& id, CollisionEnvDistanceField::DistanceFieldCacheEntryWorldPtr& dfce,
274  EigenSTL::vector_Vector3d& add_points, EigenSTL::vector_Vector3d& subtract_points);
275 
277  const distance_field::DistanceFieldConstPtr& env_distance_field,
278  GroupStateRepresentationPtr& gsr) const;
279 
280  bool getEnvironmentProximityGradients(const distance_field::DistanceFieldConstPtr& env_distance_field,
281  GroupStateRepresentationPtr& gsr) const;
282 
283  void notifyObjectChange(const ObjectConstPtr& obj, World::Action action);
284 
288  double resolution_;
289  double collision_tolerance_;
291 
292  std::vector<BodyDecompositionConstPtr> link_body_decomposition_vector_;
293  std::map<std::string, unsigned int> link_body_decomposition_index_map_;
294 
295  mutable boost::mutex update_cache_lock_;
296  DistanceFieldCacheEntryPtr distance_field_cache_entry_;
297  std::map<std::string, std::map<std::string, bool>> in_group_update_map_;
298  std::map<std::string, GroupStateRepresentationPtr> pregenerated_group_state_representation_map_;
299 
300  planning_scene::PlanningScenePtr planning_scene_;
301 
302  mutable boost::mutex update_cache_lock_world_;
303  DistanceFieldCacheEntryWorldPtr distance_field_cache_entry_world_;
304  GroupStateRepresentationPtr last_gsr_;
306 };
307 } // namespace collision_detection
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:71
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
collision_detection::CollisionEnvDistanceField::observer_handle_
World::ObserverHandle observer_handle_
Definition: collision_env_distance_field.h:337
collision_common_distance_field.h
collision_detection::CollisionEnvDistanceField::~CollisionEnvDistanceField
~CollisionEnvDistanceField() override
Definition: collision_env_distance_field.cpp:150
collision_detection::CollisionEnvDistanceField::updateGroupStateRepresentationState
void updateGroupStateRepresentationState(const moveit::core::RobotState &state, GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:1150
collision_detection::CollisionEnvDistanceField::use_signed_distance_field_
bool use_signed_distance_field_
Definition: collision_env_distance_field.h:319
collision_detection::DEFAULT_COLLISION_TOLERANCE
static const double DEFAULT_COLLISION_TOLERANCE
Definition: collision_env_distance_field.h:85
collision_distance_field_types.h
collision_detection::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix)
collision_detection::CollisionEnvDistanceField::getPosedLinkBodyPointDecomposition
PosedBodyPointDecompositionPtr getPosedLinkBodyPointDecomposition(const moveit::core::LinkModel *ls) const
Definition: collision_env_distance_field.cpp:1137
collision_detection::CollisionEnvDistanceField::getLastDistanceFieldEntry
DistanceFieldCacheEntryConstPtr getLastDistanceFieldEntry() const
Definition: collision_env_distance_field.h:157
collision_detection::CollisionEnvDistanceField::distance_field_cache_entry_
DistanceFieldCacheEntryPtr distance_field_cache_entry_
Definition: collision_env_distance_field.h:328
collision_detection::CollisionEnvDistanceField::checkSelfCollisionHelper
void checkSelfCollisionHelper(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:214
collision_detection::CollisionEnvDistanceField::generateDistanceFieldCacheEntry
DistanceFieldCacheEntryPtr generateDistanceFieldCacheEntry(const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, bool generate_distance_field) const
Definition: collision_env_distance_field.cpp:760
planning_scene.h
collision_detection::CollisionEnvDistanceField::getIntraGroupProximityGradients
bool getIntraGroupProximityGradients(GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:694
collision_detection::World::Action
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Definition: world.h:265
collision_detection::World::ObserverHandle
Definition: world.h:287
class_forward.h
collision_detection::DEFAULT_MAX_PROPOGATION_DISTANCE
static const double DEFAULT_MAX_PROPOGATION_DISTANCE
Definition: collision_env_distance_field.h:86
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
collision_detection::CollisionEnvDistanceField::compareCacheEntryToState
bool compareCacheEntryToState(const DistanceFieldCacheEntryConstPtr &dfce, const moveit::core::RobotState &state) const
Definition: collision_env_distance_field.cpp:1302
collision_detection::CollisionEnvDistanceField::planning_scene_
planning_scene::PlanningScenePtr planning_scene_
Definition: collision_env_distance_field.h:332
collision_detection::CollisionEnvDistanceField::checkCollision
void checkCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const override
Check whether the robot model is in collision with itself or the world at a particular state....
Definition: collision_env_distance_field.cpp:1433
collision_detection::CollisionEnvDistanceField::pregenerated_group_state_representation_map_
std::map< std::string, GroupStateRepresentationPtr > pregenerated_group_state_representation_map_
Definition: collision_env_distance_field.h:330
collision_detection::CollisionRequest
Representation of a collision checking request.
Definition: include/moveit/collision_detection/collision_common.h:179
collision_detection::DEFAULT_SIZE_X
static const double DEFAULT_SIZE_X
Definition: collision_env_distance_field.h:80
collision_detection::CollisionEnvDistanceField::getEnvironmentCollisions
bool getEnvironmentCollisions(const CollisionRequest &req, CollisionResult &res, const distance_field::DistanceFieldConstPtr &env_distance_field, GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:1612
collision_detection::CollisionEnvDistanceField::distance_field_cache_entry_world_
DistanceFieldCacheEntryWorldPtr distance_field_cache_entry_world_
Definition: collision_env_distance_field.h:335
collision_detection::CollisionEnvDistanceField::size_
Eigen::Vector3d size_
Definition: collision_env_distance_field.h:317
collision_detection::AllowedCollisionMatrix
Definition of a structure for the allowed collision matrix.
Definition: collision_matrix.h:112
collision_detection::CollisionEnvDistanceField::update_cache_lock_
boost::mutex update_cache_lock_
Definition: collision_env_distance_field.h:327
collision_detection::CollisionEnvDistanceField::getSelfProximityGradients
bool getSelfProximityGradients(GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:390
collision_detection::DEFAULT_SIZE_Z
static const double DEFAULT_SIZE_Z
Definition: collision_env_distance_field.h:82
collision_detection::CollisionEnvDistanceField::collision_tolerance_
double collision_tolerance_
Definition: collision_env_distance_field.h:321
collision_detection::CollisionEnvDistanceField::distanceSelf
virtual double distanceSelf(const moveit::core::RobotState &) const
Definition: collision_env_distance_field.h:140
collision_detection::CollisionEnvDistanceField::DistanceFieldCacheEntryWorld::posed_body_point_decompositions_
std::map< std::string, std::vector< PosedBodyPointDecompositionPtr > > posed_body_point_decompositions_
Definition: collision_env_distance_field.h:173
collision_detection::CollisionEnvDistanceField::compareCacheEntryToAllowedCollisionMatrix
bool compareCacheEntryToAllowedCollisionMatrix(const DistanceFieldCacheEntryConstPtr &dfce, const collision_detection::AllowedCollisionMatrix &acm) const
Definition: collision_env_distance_field.cpp:1362
collision_detection::DEFAULT_SIZE_Y
static const double DEFAULT_SIZE_Y
Definition: collision_env_distance_field.h:81
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:382
collision_detection::CollisionEnvDistanceField::in_group_update_map_
std::map< std::string, std::map< std::string, bool > > in_group_update_map_
Definition: collision_env_distance_field.h:329
collision_env.h
collision_detection::CollisionEnvDistanceField::update_cache_lock_world_
boost::mutex update_cache_lock_world_
Definition: collision_env_distance_field.h:334
EigenSTL::vector_Vector3d
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
collision_detection::CollisionEnvDistanceField::link_body_decomposition_vector_
std::vector< BodyDecompositionConstPtr > link_body_decomposition_vector_
Definition: collision_env_distance_field.h:324
collision_detection::CollisionEnvDistanceField::setWorld
void setWorld(const WorldPtr &world) override
Definition: collision_env_distance_field.cpp:1734
collision_detection::CollisionEnvDistanceField::DistanceFieldCacheEntryWorld::distance_field_
distance_field::DistanceFieldPtr distance_field_
Definition: collision_env_distance_field.h:174
collision_detection::CollisionEnvDistanceField::getPosedLinkBodySphereDecomposition
PosedBodySphereDecompositionPtr getPosedLinkBodySphereDecomposition(const moveit::core::LinkModel *ls, unsigned int ind) const
Definition: collision_env_distance_field.cpp:1128
collision_detection::CollisionEnvDistanceField
Definition: collision_env_distance_field.h:90
collision_detection::CollisionEnvDistanceField::getAllCollisions
void getAllCollisions(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:1591
collision_detection::CollisionEnv::ObjectConstPtr
World::ObjectConstPtr ObjectConstPtr
Definition: collision_env.h:279
collision_detection::CollisionEnvDistanceField::generateCollisionCheckingStructures
void generateCollisionCheckingStructures(const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr, bool generate_distance_field) const
Definition: collision_env_distance_field.cpp:195
collision_detection::CollisionEnvDistanceField::origin_
Eigen::Vector3d origin_
Definition: collision_env_distance_field.h:318
collision_detection::CollisionEnvDistanceField::getSelfCollisions
bool getSelfCollisions(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:309
collision_detection::CollisionEnvDistanceField::updatedPaddingOrScaling
void updatedPaddingOrScaling(const std::vector< std::string > &) override
When the scale or padding is changed for a set of links by any of the functions in this class,...
Definition: collision_env_distance_field.h:301
collision_detection::CollisionEnvDistanceField::link_body_decomposition_index_map_
std::map< std::string, unsigned int > link_body_decomposition_index_map_
Definition: collision_env_distance_field.h:325
collision_detection::CollisionEnvDistanceField::getLastGroupStateRepresentation
collision_detection::GroupStateRepresentationConstPtr getLastGroupStateRepresentation() const
Definition: collision_env_distance_field.h:239
collision_detection::CollisionEnvDistanceField::getDistanceField
distance_field::DistanceFieldConstPtr getDistanceField() const
Definition: collision_env_distance_field.h:234
collision_detection::CollisionEnvDistanceField::getGroupStateRepresentation
void getGroupStateRepresentation(const DistanceFieldCacheEntryConstPtr &dfce, const moveit::core::RobotState &state, GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:1204
collision_detection::CollisionEnvDistanceField::updateDistanceObject
void updateDistanceObject(const std::string &id, CollisionEnvDistanceField::DistanceFieldCacheEntryWorldPtr &dfce, EigenSTL::vector_Vector3d &add_points, EigenSTL::vector_Vector3d &subtract_points)
Definition: collision_env_distance_field.cpp:1781
collision_detection::CollisionEnvDistanceField::addLinkBodyDecompositions
void addLinkBodyDecompositions(double resolution)
Definition: collision_env_distance_field.cpp:1009
collision_detection::CollisionEnvDistanceField::notifyObjectChange
void notifyObjectChange(const ObjectConstPtr &obj, World::Action action)
Definition: collision_env_distance_field.cpp:1755
collision_detection::DistanceResult
Result of a distance request.
Definition: include/moveit/collision_detection/collision_common.h:357
collision_detection::CollisionEnvDistanceField::getDistanceFieldCacheEntry
DistanceFieldCacheEntryConstPtr getDistanceFieldCacheEntry(const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm) const
Definition: collision_env_distance_field.cpp:239
collision_detection::CollisionEnvDistanceField::distanceRobot
virtual double distanceRobot(const moveit::core::RobotState &state, bool verbose=false) const
Definition: collision_env_distance_field.h:210
collision_detection::CollisionEnvDistanceField::MOVEIT_STRUCT_FORWARD
MOVEIT_STRUCT_FORWARD(DistanceFieldCacheEntryWorld)
collision_detection::CollisionEnvDistanceField::getCollisionGradients
void getCollisionGradients(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:1568
collision_detection::CollisionEnvDistanceField::initialize
void initialize(const std::map< std::string, std::vector< CollisionSphere >> &link_body_decompositions, const Eigen::Vector3d &size, const Eigen::Vector3d &origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance)
Definition: collision_env_distance_field.cpp:155
collision_detection::CollisionEnvDistanceField::CollisionEnvDistanceField
EIGEN_MAKE_ALIGNED_OPERATOR_NEW CollisionEnvDistanceField(const moveit::core::RobotModelConstPtr &robot_model, const std::map< std::string, std::vector< CollisionSphere >> &link_body_decompositions=std::map< std::string, std::vector< CollisionSphere >>(), double size_x=DEFAULT_SIZE_X, double size_y=DEFAULT_SIZE_Y, double size_z=DEFAULT_SIZE_Z, const Eigen::Vector3d &origin=Eigen::Vector3d(0, 0, 0), bool use_signed_distance_field=DEFAULT_USE_SIGNED_DISTANCE_FIELD, double resolution=DEFAULT_RESOLUTION, double collision_tolerance=DEFAULT_COLLISION_TOLERANCE, double max_propogation_distance=DEFAULT_MAX_PROPOGATION_DISTANCE, double padding=0.0, double scale=1.0)
Definition: collision_env_distance_field.cpp:89
collision_detection::CollisionEnvDistanceField::getEnvironmentProximityGradients
bool getEnvironmentProximityGradients(const distance_field::DistanceFieldConstPtr &env_distance_field, GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:1696
collision_detection::CollisionEnvDistanceField::last_gsr_
GroupStateRepresentationPtr last_gsr_
Definition: collision_env_distance_field.h:336
collision_detection::CollisionEnvDistanceField::checkRobotCollision
void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const override
Check whether the robot model is in collision with the world. Any collisions between a robot link and...
Definition: collision_env_distance_field.cpp:1498
collision_detection::CollisionEnvDistanceField::getIntraGroupCollisions
bool getIntraGroupCollisions(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:465
collision_detection::CollisionEnvDistanceField::generateDistanceFieldCacheEntryWorld
DistanceFieldCacheEntryWorldPtr generateDistanceFieldCacheEntryWorld()
Definition: collision_env_distance_field.cpp:1833
collision_detection::DistanceRequest
Representation of a distance-reporting request.
Definition: include/moveit/collision_detection/collision_common.h:241
collision_detection::CollisionEnvDistanceField::max_propogation_distance_
double max_propogation_distance_
Definition: collision_env_distance_field.h:322
collision_detection::CollisionEnvDistanceField::resolution_
double resolution_
Definition: collision_env_distance_field.h:320
collision_detection::CollisionEnvDistanceField::checkSelfCollision
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state) const override
Check for robot self collision. Any collision between any pair of links is checked for,...
Definition: collision_env_distance_field.cpp:270
collision_detection::CollisionEnvDistanceField::createCollisionModelMarker
void createCollisionModelMarker(const moveit::core::RobotState &state, visualization_msgs::MarkerArray &model_markers) const
Definition: collision_env_distance_field.cpp:1029
collision_detection::DEFAULT_USE_SIGNED_DISTANCE_FIELD
static const bool DEFAULT_USE_SIGNED_DISTANCE_FIELD
Definition: collision_env_distance_field.h:83
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
collision_detection::DEFAULT_RESOLUTION
static const double DEFAULT_RESOLUTION
Definition: collision_env_distance_field.h:84
collision_detection
Definition: collision_detector_allocator_allvalid.h:42


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Mar 3 2024 03:23:35