moveit2
The MoveIt Motion Planning Framework for ROS 2.
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/msg/link_padding.hpp>
43 #include <moveit_msgs/msg/link_scale.hpp>
45 
46 namespace collision_detection
47 {
48 MOVEIT_CLASS_FORWARD(CollisionEnv); // Defines CollisionEnvPtr, ConstPtr, WeakPtr... etc
49 
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::msg::LinkPadding>& padding);
295 
297  void getPadding(std::vector<moveit_msgs::msg::LinkPadding>& padding) const;
298 
300  void setScale(const std::vector<moveit_msgs::msg::LinkScale>& scale);
301 
303  void getScale(std::vector<moveit_msgs::msg::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
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
Provides the interface to the individual collision checking libraries.
Definition: collision_env.h:52
virtual void checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix &acm) const =0
Check for self collision. Allowed collisions specified by the allowed collision matrix are taken into...
double distanceRobot(const moveit::core::RobotState &state, const AllowedCollisionMatrix &acm, bool verbose=false) const
Compute the shortest distance between a robot and the world.
virtual void setWorld(const WorldPtr &world)
virtual void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state1, const moveit::core::RobotState &state2, const AllowedCollisionMatrix &acm) const =0
Check whether the robot model is in collision with the world in a continuous manner (between two robo...
void getPadding(std::vector< moveit_msgs::msg::LinkPadding > &padding) const
Get the link padding as a vector of messages.
void setLinkScale(const std::string &link_name, double scale)
Set the scaling for a particular link.
void setLinkPadding(const std::string &link_name, double padding)
Set the link padding for a particular link.
moveit::core::RobotModelConstPtr robot_model_
The kinematic model corresponding to this collision model.
double distanceRobot(const moveit::core::RobotState &state, bool verbose=false) const
Compute the shortest distance between a robot and the world.
virtual void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state1, const moveit::core::RobotState &state2) const =0
Check whether the robot model is in collision with the world in a continuous manner (between two robo...
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,...
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.
const std::map< std::string, double > & getLinkScale() const
Get the link scaling as a map (from link names to scale value)
double distanceSelf(const moveit::core::RobotState &state) const
The distance to self-collision given the robot is at state state.
void setScale(double scale)
Set the link scaling (for every link)
void setPadding(double padding)
Set the link padding (for every link)
virtual void distanceRobot(const DistanceRequest &req, DistanceResult &res, const moveit::core::RobotState &state) const =0
Compute the distance between a robot and the world.
double distanceSelf(const moveit::core::RobotState &state, const AllowedCollisionMatrix &acm) const
The distance to self-collision given the robot is at state state, ignoring the distances between link...
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....
void getScale(std::vector< moveit_msgs::msg::LinkScale > &scale) const
Get the link scaling as a vector of messages.
World::ObjectConstPtr ObjectConstPtr
std::map< std::string, double > link_padding_
The internally maintained map (from link names to padding)
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...
const std::map< std::string, double > & getLinkPadding() const
Get the link paddings as a map (from link names to padding value)
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,...
const moveit::core::RobotModelConstPtr & getRobotModel() const
The kinematic model corresponding to this collision model.
const WorldConstPtr & getWorld() const
std::map< std::string, double > link_scale_
The internally maintained map (from link names to scaling)
virtual void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix &acm) const =0
Check whether the robot model is in collision with the world. Allowed collisions are ignored....
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix)
Representation of a collision checking request.
Representation of a collision checking result.
Representation of a distance-reporting request.
void enableGroup(const moveit::core::RobotModelConstPtr &robot_model)
bool verbose
Log debug information.
const AllowedCollisionMatrix * acm
The allowed collision matrix used to filter checks.
Result of a distance request.
DistanceResultsData minimum_distance
ResultsData for the two objects with the minimum distance.
double distance
The distance between two objects. If two objects are in collision, distance <= 0.