collision_env.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, 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 copyright holder 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: Ioan Sucan, Jens Petit */
36 
37 #pragma once
38 
42 #include <moveit_msgs/LinkPadding.h>
43 #include <moveit_msgs/LinkScale.h>
45 
46 namespace collision_detection
47 {
48 MOVEIT_CLASS_FORWARD(CollisionEnv); // Defines CollisionEnvPtr, ConstPtr, WeakPtr... etc
49 
51 class CollisionEnv
52 {
53 public:
54  CollisionEnv() = delete;
55 
61  CollisionEnv(const moveit::core::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0);
62 
68  CollisionEnv(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0,
69  double scale = 1.0);
70 
72  CollisionEnv(const CollisionEnv& other, const WorldPtr& world);
73 
74  virtual ~CollisionEnv()
75  {
76  }
77 
84  virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
85  const moveit::core::RobotState& state) const = 0;
86 
93  virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
94  const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const = 0;
95 
101  virtual void checkCollision(const CollisionRequest& req, CollisionResult& res,
102  const moveit::core::RobotState& state) const;
103 
110  virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state,
111  const AllowedCollisionMatrix& acm) const;
112 
120  virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
121  const moveit::core::RobotState& state) const = 0;
122 
130  virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
131  const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const = 0;
132 
142  virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
143  const moveit::core::RobotState& state1, const moveit::core::RobotState& state2,
144  const AllowedCollisionMatrix& acm) const = 0;
145 
155  virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
156  const moveit::core::RobotState& state1,
157  const moveit::core::RobotState& state2) const = 0;
158 
163  virtual void distanceSelf(const DistanceRequest& req, DistanceResult& res,
164  const moveit::core::RobotState& state) const = 0;
165 
167  inline double distanceSelf(const moveit::core::RobotState& state) const
168  {
169  DistanceRequest req;
170  DistanceResult res;
171 
172  req.enableGroup(getRobotModel());
173  distanceSelf(req, res, state);
174  return res.minimum_distance.distance;
175  }
176 
179  inline double distanceSelf(const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const
180  {
181  DistanceRequest req;
182  DistanceResult res;
183 
184  req.enableGroup(getRobotModel());
185  req.acm = &acm;
186  distanceSelf(req, res, state);
187  return res.minimum_distance.distance;
188  }
189 
195  virtual void distanceRobot(const DistanceRequest& req, DistanceResult& res,
196  const moveit::core::RobotState& state) const = 0;
197 
202  inline double distanceRobot(const moveit::core::RobotState& state, bool verbose = false) const
203  {
204  DistanceRequest req;
205  DistanceResult res;
206 
207  req.verbose = verbose;
208  req.enableGroup(getRobotModel());
209 
210  distanceRobot(req, res, state);
211  return res.minimum_distance.distance;
212  }
213 
220  inline double distanceRobot(const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm,
221  bool verbose = false) const
222  {
223  DistanceRequest req;
224  DistanceResult res;
225 
226  req.acm = &acm;
227  req.verbose = verbose;
228  req.enableGroup(getRobotModel());
229 
230  distanceRobot(req, res, state);
231  return res.minimum_distance.distance;
232  }
233 
237  virtual void setWorld(const WorldPtr& world);
238 
240  const WorldPtr& getWorld()
241  {
242  return world_;
243  }
244 
246  const WorldConstPtr& getWorld() const
247  {
248  return world_const_;
249  }
250 
251  using ObjectPtr = World::ObjectPtr;
252  using ObjectConstPtr = World::ObjectConstPtr;
253 
255  const moveit::core::RobotModelConstPtr& getRobotModel() const
256  {
257  return robot_model_;
258  }
259 
264  void setLinkPadding(const std::string& link_name, double padding);
265 
267  double getLinkPadding(const std::string& link_name) const;
268 
270  void setLinkPadding(const std::map<std::string, double>& padding);
271 
273  const std::map<std::string, double>& getLinkPadding() const;
274 
276  void setLinkScale(const std::string& link_name, double scale);
277 
279  double getLinkScale(const std::string& link_name) const;
280 
282  void setLinkScale(const std::map<std::string, double>& scale);
283 
285  const std::map<std::string, double>& getLinkScale() const;
286 
288  void setPadding(double padding);
289 
291  void setScale(double scale);
292 
294  void setPadding(const std::vector<moveit_msgs::LinkPadding>& padding);
295 
297  void getPadding(std::vector<moveit_msgs::LinkPadding>& padding) const;
298 
300  void setScale(const std::vector<moveit_msgs::LinkScale>& scale);
301 
303  void getScale(std::vector<moveit_msgs::LinkScale>& scale) const;
304 
305 protected:
312  virtual void updatedPaddingOrScaling(const std::vector<std::string>& links);
313 
315  moveit::core::RobotModelConstPtr robot_model_;
316 
318  std::map<std::string, double> link_padding_;
319 
321  std::map<std::string, double> link_scale_;
322 
323 private:
324  WorldPtr world_; // The world always valid, never nullptr.
325  WorldConstPtr world_const_; // always same as world_
326 };
327 } // namespace collision_detection
collision_detection::CollisionEnv::getScale
void getScale(std::vector< moveit_msgs::LinkScale > &scale) const
Get the link scaling as a vector of messages.
Definition: collision_env.cpp:266
collision_detection::CollisionEnv::world_
WorldPtr world_
Definition: collision_env.h:356
collision_detection::CollisionEnv::getRobotModel
const moveit::core::RobotModelConstPtr & getRobotModel() const
The kinematic model corresponding to this collision model.
Definition: collision_env.h:287
collision_detection::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix)
collision_detection::CollisionEnv::link_padding_
std::map< std::string, double > link_padding_
The internally maintained map (from link names to padding)
Definition: collision_env.h:350
world.h
collision_detection::CollisionEnv::checkRobotCollision
virtual void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const =0
Check whether the robot model is in collision with the world. Any collisions between a robot link and...
collision_detection::CollisionEnv::checkSelfCollision
virtual void checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const =0
Check for robot self collision. Any collision between any pair of links is checked for,...
class_forward.h
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
collision_detection::CollisionEnv::checkCollision
virtual void checkCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const
Check whether the robot model is in collision with itself or the world at a particular state....
Definition: collision_env.cpp:291
collision_detection::DistanceRequest::verbose
bool verbose
Log debug information.
Definition: include/moveit/collision_detection/collision_common.h:326
collision_detection::CollisionEnv::setLinkPadding
void setLinkPadding(const std::string &link_name, double padding)
Set the link padding for a particular link.
Definition: collision_env.cpp:143
collision_detection::CollisionEnv::setLinkScale
void setLinkScale(const std::string &link_name, double scale)
Set the scaling for a particular link.
Definition: collision_env.cpp:184
collision_detection::AllowedCollisionMatrix
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
Definition: collision_matrix.h:112
collision_detection::CollisionEnv::setPadding
void setPadding(double padding)
Set the link padding (for every link)
Definition: collision_env.cpp:111
collision_detection::CollisionEnv::world_const_
WorldConstPtr world_const_
Definition: collision_env.h:357
collision_matrix.h
collision_detection::DistanceRequest::acm
const AllowedCollisionMatrix * acm
The allowed collision matrix used to filter checks.
Definition: include/moveit/collision_detection/collision_common.h:319
collision_detection::CollisionEnv::ObjectConstPtr
World::ObjectConstPtr ObjectConstPtr
Definition: collision_env.h:284
collision_detection::CollisionEnv::distanceRobot
virtual void distanceRobot(const DistanceRequest &req, DistanceResult &res, const moveit::core::RobotState &state) const =0
Compute the distance between a robot and the world.
collision_detection::CollisionEnv::setScale
void setScale(double scale)
Set the link scaling (for every link)
Definition: collision_env.cpp:127
collision_detection::CollisionEnv::getLinkScale
const std::map< std::string, double > & getLinkScale() const
Get the link scaling as a map (from link names to scale value)
Definition: collision_env.cpp:219
collision_detection::CollisionEnv::getPadding
void getPadding(std::vector< moveit_msgs::LinkPadding > &padding) const
Get the link padding as a vector of messages.
Definition: collision_env.cpp:254
collision_detection::DistanceResult::minimum_distance
DistanceResultsData minimum_distance
ResultsData for the two objects with the minimum distance.
Definition: include/moveit/collision_detection/collision_common.h:400
collision_detection::CollisionEnv::~CollisionEnv
virtual ~CollisionEnv()
Definition: collision_env.h:106
collision_detection::DistanceResult
Result of a distance request.
Definition: include/moveit/collision_detection/collision_common.h:390
collision_detection::CollisionEnv::getLinkPadding
const std::map< std::string, double > & getLinkPadding() const
Get the link paddings as a map (from link names to padding value)
Definition: collision_env.cpp:179
collision_detection::CollisionEnv::ObjectPtr
World::ObjectPtr ObjectPtr
Definition: collision_env.h:283
collision_detection::DistanceResultsData::distance
double distance
The distance between two objects. If two objects are in collision, distance <= 0.
Definition: include/moveit/collision_detection/collision_common.h:342
collision_detection::CollisionEnv::setWorld
virtual void setWorld(const WorldPtr &world)
Definition: collision_env.cpp:282
collision_detection::DistanceRequest
Representation of a distance-reporting request.
Definition: include/moveit/collision_detection/collision_common.h:274
collision_detection::CollisionEnv::CollisionEnv
CollisionEnv()=delete
collision_detection::DistanceRequest::enableGroup
void enableGroup(const moveit::core::RobotModelConstPtr &robot_model)
Compute active_components_only_ based on req_.
Definition: include/moveit/collision_detection/collision_common.h:290
collision_detection::CollisionEnv::robot_model_
moveit::core::RobotModelConstPtr robot_model_
The kinematic model corresponding to this collision model.
Definition: collision_env.h:347
robot_state.h
collision_detection::CollisionEnv::link_scale_
std::map< std::string, double > link_scale_
The internally maintained map (from link names to scaling)
Definition: collision_env.h:353
collision_detection::CollisionEnv::updatedPaddingOrScaling
virtual void updatedPaddingOrScaling(const std::vector< std::string > &links)
When the scale or padding is changed for a set of links by any of the functions in this class,...
Definition: collision_env.cpp:278
collision_detection::CollisionEnv::getWorld
const WorldPtr & getWorld()
Definition: collision_env.h:272
verbose
bool verbose
collision_detection
Definition: collision_detector_allocator_allvalid.h:42
collision_detection::CollisionEnv::distanceSelf
virtual void distanceSelf(const DistanceRequest &req, DistanceResult &res, const moveit::core::RobotState &state) const =0
The distance to self-collision given the robot is at state state.


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri Dec 3 2021 03:23:21