38 #include <rclcpp/logger.hpp>
39 #include <rclcpp/logging.hpp>
51 static inline bool validateScale(
const double scale)
53 if (scale < std::numeric_limits<double>::epsilon())
55 RCLCPP_ERROR(
getLogger(),
"Scale must be positive");
58 if (scale > std::numeric_limits<double>::max())
60 RCLCPP_ERROR(
getLogger(),
"Scale must be finite");
66 static inline bool validatePadding(
const double padding)
70 RCLCPP_ERROR(
getLogger(),
"Padding cannot be negative");
73 if (padding > std::numeric_limits<double>::max())
75 RCLCPP_ERROR(
getLogger(),
"Padding must be finite");
84 : robot_model_(model), world_(std::make_shared<
World>()), world_const_(world_)
86 if (!validateScale(scale))
88 if (!validatePadding(padding))
91 const std::vector<const moveit::core::LinkModel*>& links =
robot_model_->getLinkModelsWithCollisionGeometry();
92 for (
const auto link : links)
101 : robot_model_(model), world_(world), world_const_(world_)
103 if (!validateScale(scale))
105 if (!validatePadding(padding))
108 const std::vector<const moveit::core::LinkModel*>& links =
robot_model_->getLinkModelsWithCollisionGeometry();
109 for (
const auto link : links)
117 : robot_model_(other.robot_model_), world_(world), world_const_(world)
124 if (!validatePadding(padding))
126 std::vector<std::string> u;
127 const std::vector<const moveit::core::LinkModel*>& links =
robot_model_->getLinkModelsWithCollisionGeometry();
128 for (
const auto link : links)
131 u.push_back(link->getName());
140 if (!validateScale(scale))
142 std::vector<std::string> u;
143 const std::vector<const moveit::core::LinkModel*>& links =
robot_model_->getLinkModelsWithCollisionGeometry();
144 for (
const auto link : links)
147 u.push_back(link->getName());
156 validatePadding(padding);
161 std::vector<std::string> u(1, link_name);
181 std::vector<std::string> u;
182 for (
const auto& link_pad_pair : padding)
184 validatePadding(link_pad_pair.second);
188 u.push_back(link_pad_pair.first);
201 validateScale(scale);
206 std::vector<std::string> u(1, link_name);
226 std::vector<std::string> u;
227 for (
const auto& link_scale_pair : scale)
230 link_scale_[link_scale_pair.first] = link_scale_pair.second;
232 u.push_back(link_scale_pair.first);
245 std::vector<std::string> u;
246 for (
const auto& p : padding)
248 validatePadding(p.padding);
252 u.push_back(p.link_name);
260 std::vector<std::string> u;
261 for (
const auto& s : scale)
263 validateScale(s.scale);
267 u.push_back(s.link_name);
278 moveit_msgs::msg::LinkPadding lp_msg;
279 lp_msg.link_name = lp.first;
280 lp_msg.padding = lp.second;
281 padding.push_back(lp_msg);
290 moveit_msgs::msg::LinkScale ls_msg;
291 ls_msg.link_name = ls.first;
292 ls_msg.scale = ls.second;
293 scale.push_back(ls_msg);
305 world_ = std::make_shared<World>();
307 world_const_ = world;
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.
virtual void setWorld(const WorldPtr &world)
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.
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,...
const std::map< std::string, double > & getLinkScale() const
Get the link scaling as a map (from link names to scale value)
void setScale(double scale)
Set the link scaling (for every link)
void setPadding(double padding)
Set the link padding (for every 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.
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,...
std::map< std::string, double > link_scale_
The internally maintained map (from link names to scaling)
Maintain a representation of the environment.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
rclcpp::Logger getLogger()
void update(moveit::core::RobotState *self, bool force, std::string &category)
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
Representation of a collision checking request.
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
std::size_t max_contacts
Overall maximum number of contacts to compute.
Representation of a collision checking result.
bool collision
True if collision was found, false otherwise.