collision_world_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 #ifndef MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_WORLD_DISTANCE_FIELD_
38 #define MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_WORLD_DISTANCE_FIELD_
39 
44 
45 namespace collision_detection
46 {
47 MOVEIT_CLASS_FORWARD(CollisionWorldDistanceField)
48 
50 {
51 public:
53 
56  {
57  std::map<std::string, std::vector<PosedBodyPointDecompositionPtr>> posed_body_point_decompositions_;
58  distance_field::DistanceFieldPtr distance_field_;
59  };
60 
61  CollisionWorldDistanceField(Eigen::Vector3d size = Eigen::Vector3d(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z),
62  Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0),
63  bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD,
65  double collision_tolerance = DEFAULT_COLLISION_TOLERANCE,
66  double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE);
67 
69  const WorldPtr& world, Eigen::Vector3d size = Eigen::Vector3d(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z),
70  Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0),
71  bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, double resolution = DEFAULT_RESOLUTION,
72  double collision_tolerance = DEFAULT_COLLISION_TOLERANCE,
73  double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE);
74 
75  CollisionWorldDistanceField(const CollisionWorldDistanceField& other, const WorldPtr& world);
76 
77  virtual ~CollisionWorldDistanceField();
78 
79  virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
80  const robot_state::RobotState& state) const;
81 
82  virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
83  const robot_state::RobotState& state, GroupStateRepresentationPtr& gsr) const;
84 
85  virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
86  const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const;
87 
88  virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
89  const robot_state::RobotState& state, const AllowedCollisionMatrix& acm,
90  GroupStateRepresentationPtr& gsr) const;
91 
92  virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
93  const robot_state::RobotState& state) const;
94 
95  virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
96  const robot_state::RobotState& state, GroupStateRepresentationPtr& gsr) const;
97 
98  virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
99  const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const;
100 
101  virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
102  const robot_state::RobotState& state, const AllowedCollisionMatrix& acm,
103  GroupStateRepresentationPtr& gsr) const;
104 
105  virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
106  const robot_state::RobotState& state1, const robot_state::RobotState& state2) const
107  {
108  }
109  virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
110  const robot_state::RobotState& state1, const robot_state::RobotState& state2,
111  const AllowedCollisionMatrix& acm) const
112  {
113  }
114  virtual void checkWorldCollision(const CollisionRequest& req, CollisionResult& res,
115  const CollisionWorld& other_world) const
116  {
117  }
118  virtual void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world,
119  const AllowedCollisionMatrix& acm) const
120  {
121  }
122 
123  virtual double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state,
124  bool verbose = false) const
125  {
126  return 0.0;
127  }
128  virtual double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state,
129  const AllowedCollisionMatrix& acm, bool verbose = false) const
130  {
131  return 0.0;
132  }
133  virtual double distanceWorld(const CollisionWorld& world, bool verbose = false) const
134  {
135  return 0.0;
136  }
137  virtual double distanceWorld(const CollisionWorld& world, const AllowedCollisionMatrix& acm,
138  bool verbose = false) const
139  {
140  return 0.0;
141  }
142 
143  virtual void distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot,
144  const robot_state::RobotState& state) const
145  {
146  ROS_ERROR_NAMED("collision_distance_field", "Not implemented");
147  }
148 
149  virtual void distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const
150  {
151  ROS_ERROR_NAMED("collision_distance_field", "Not implemented");
152  }
153 
154  virtual void setWorld(const WorldPtr& world);
155 
156  void generateEnvironmentDistanceField(bool redo = true);
157 
158  distance_field::DistanceFieldConstPtr getDistanceField() const
159  {
160  return distance_field_cache_entry_->distance_field_;
161  }
162 
163  collision_detection::GroupStateRepresentationConstPtr getLastGroupStateRepresentation() const
164  {
165  return last_gsr_;
166  }
167 
168  void getCollisionGradients(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
169  const robot_state::RobotState& state, const AllowedCollisionMatrix* acm,
170  GroupStateRepresentationPtr& gsr) const;
171 
172  void getAllCollisions(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
173  const robot_state::RobotState& state, const AllowedCollisionMatrix* acm,
174  GroupStateRepresentationPtr& gsr) const;
175 
176 protected:
177  DistanceFieldCacheEntryPtr generateDistanceFieldCacheEntry();
178 
179  void updateDistanceObject(const std::string& id, CollisionWorldDistanceField::DistanceFieldCacheEntryPtr& dfce,
180  EigenSTL::vector_Vector3d& add_points, EigenSTL::vector_Vector3d& subtract_points);
181 
182  bool getEnvironmentCollisions(const CollisionRequest& req, CollisionResult& res,
183  const distance_field::DistanceFieldConstPtr& env_distance_field,
184  GroupStateRepresentationPtr& gsr) const;
185 
186  bool getEnvironmentProximityGradients(const distance_field::DistanceFieldConstPtr& env_distance_field,
187  GroupStateRepresentationPtr& gsr) const;
188 
189  static void notifyObjectChange(CollisionWorldDistanceField* self, const ObjectConstPtr& obj, World::Action action);
190 
191  Eigen::Vector3d size_;
192  Eigen::Vector3d origin_;
194  double resolution_;
197 
198  mutable boost::mutex update_cache_lock_;
199  DistanceFieldCacheEntryPtr distance_field_cache_entry_;
200  GroupStateRepresentationPtr last_gsr_;
202 };
203 }
204 
205 #endif
static const double DEFAULT_RESOLUTION
virtual double distanceWorld(const CollisionWorld &world, const AllowedCollisionMatrix &acm, bool verbose=false) const
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
virtual void distanceRobot(const DistanceRequest &req, DistanceResult &res, const CollisionRobot &robot, const robot_state::RobotState &state) const
Compute the distance between a robot and the world.
virtual void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state1, const robot_state::RobotState &state2) const
Check whether the robot model is in collision with the world in a continuous manner (between two robo...
virtual double distanceWorld(const CollisionWorld &world, bool verbose=false) const
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Definition: world.h:195
MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix)
collision_detection::GroupStateRepresentationConstPtr getLastGroupStateRepresentation() const
static const double DEFAULT_MAX_PROPOGATION_DISTANCE
Generic interface to collision detection.
virtual void checkWorldCollision(const CollisionRequest &req, CollisionResult &res, const CollisionWorld &other_world) const
Check whether a given set of objects is in collision with objects from another world. Any contacts are considered.
virtual double distanceRobot(const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix &acm, bool verbose=false) const
World::ObjectConstPtr ObjectConstPtr
static const double DEFAULT_COLLISION_TOLERANCE
static const double resolution
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
This class represents a collision model of the robot and can be used for self collision checks (to ch...
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
virtual void checkWorldCollision(const CollisionRequest &req, CollisionResult &res, const CollisionWorld &other_world, const AllowedCollisionMatrix &acm) const
Check whether a given set of objects is in collision with objects from another world. Allowed collisions are ignored. Any contacts are considered.
virtual void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state1, const robot_state::RobotState &state2, const AllowedCollisionMatrix &acm) const
Check whether the robot model is in collision with the world in a continuous manner (between two robo...
distance_field::DistanceFieldConstPtr getDistanceField() const
Perform collision checking with the environment. The collision world maintains a representation of th...
static const bool DEFAULT_USE_SIGNED_DISTANCE_FIELD
#define ROS_ERROR_NAMED(name,...)
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
virtual double distanceRobot(const CollisionRobot &robot, const robot_state::RobotState &state, bool verbose=false) const
std::map< std::string, std::vector< PosedBodyPointDecompositionPtr > > posed_body_point_decompositions_
virtual void distanceWorld(const DistanceRequest &req, DistanceResult &res, const CollisionWorld &world) const
Compute the distance between another world.


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Sep 15 2019 03:57:56