moveit2
The MoveIt Motion Planning Framework for ROS 2.
|
Provides the interface to the individual collision checking libraries. More...
#include <collision_env.h>
Public Types | |
using | ObjectPtr = World::ObjectPtr |
using | ObjectConstPtr = World::ObjectConstPtr |
Public Member Functions | |
CollisionEnv ()=delete | |
CollisionEnv (const moveit::core::RobotModelConstPtr &model, double padding=0.0, double scale=1.0) | |
Constructor. | |
CollisionEnv (const moveit::core::RobotModelConstPtr &model, const WorldPtr &world, double padding=0.0, double scale=1.0) | |
Constructor. | |
CollisionEnv (const CollisionEnv &other, const WorldPtr &world) | |
Copy constructor. | |
virtual | ~CollisionEnv () |
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, NO collisions are ignored. | |
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 account. | |
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. Any collision between any pair of links is checked for, NO collisions are ignored. | |
virtual void | checkCollision (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix &acm) const |
Check whether the robot model is in collision with itself or the world at a particular state. Allowed collisions specified by the allowed collision matrix are taken into account. | |
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 the world are considered. Self collisions are not checked. | |
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. Self collisions are not checked. | |
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 robot states). Allowed collisions are ignored. Self collisions are not checked. | |
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 robot states). Allowed collisions are ignored. Self collisions are not checked. | |
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. | |
double | distanceSelf (const moveit::core::RobotState &state) const |
The distance to self-collision given the robot is at state state. | |
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 links that are allowed to always collide (as specified by acm) | |
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 | distanceRobot (const moveit::core::RobotState &state, bool verbose=false) const |
Compute the shortest distance between a robot and the world. | |
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) |
const WorldPtr & | getWorld () |
const WorldConstPtr & | getWorld () const |
const moveit::core::RobotModelConstPtr & | getRobotModel () const |
The kinematic model corresponding to this collision model. | |
void | setLinkPadding (const std::string &link_name, double padding) |
Set the link padding for a particular link. | |
double | getLinkPadding (const std::string &link_name) const |
Get the link padding for a particular link. | |
void | setLinkPadding (const std::map< std::string, double > &padding) |
Set the link paddings using a map (from link names to padding value) | |
const std::map< std::string, double > & | getLinkPadding () const |
Get the link paddings as a map (from link names to padding value) | |
void | setLinkScale (const std::string &link_name, double scale) |
Set the scaling for a particular link. | |
double | getLinkScale (const std::string &link_name) const |
Set the scaling for a particular link. | |
void | setLinkScale (const std::map< std::string, double > &scale) |
Set the link scaling using a map (from link names to scale value) | |
const std::map< std::string, double > & | getLinkScale () const |
Get the link scaling as a map (from link names to scale value) | |
void | setPadding (double padding) |
Set the link padding (for every link) | |
void | setScale (double scale) |
Set the link scaling (for every link) | |
void | setPadding (const std::vector< moveit_msgs::msg::LinkPadding > &padding) |
Set the link padding from a vector of messages. | |
void | getPadding (std::vector< moveit_msgs::msg::LinkPadding > &padding) const |
Get the link padding as a vector of messages. | |
void | setScale (const std::vector< moveit_msgs::msg::LinkScale > &scale) |
Set the link scaling from a vector of messages. | |
void | getScale (std::vector< moveit_msgs::msg::LinkScale > &scale) const |
Get the link scaling as a vector of messages. | |
Protected Member Functions | |
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, updatedPaddingOrScaling() function is called. This function has an empty default implementation. The intention is to override this function in a derived class to allow for updating additional structures that may need such updating when link scale or padding changes. | |
Protected Attributes | |
moveit::core::RobotModelConstPtr | robot_model_ |
The kinematic model corresponding to this collision model. | |
std::map< std::string, double > | link_padding_ |
The internally maintained map (from link names to padding) | |
std::map< std::string, double > | link_scale_ |
The internally maintained map (from link names to scaling) | |
Provides the interface to the individual collision checking libraries.
Definition at line 51 of file collision_env.h.
using collision_detection::CollisionEnv::ObjectConstPtr = World::ObjectConstPtr |
Definition at line 252 of file collision_env.h.
using collision_detection::CollisionEnv::ObjectPtr = World::ObjectPtr |
Definition at line 251 of file collision_env.h.
|
delete |
collision_detection::CollisionEnv::CollisionEnv | ( | const moveit::core::RobotModelConstPtr & | model, |
double | padding = 0.0 , |
||
double | scale = 1.0 |
||
) |
Constructor.
model | A robot model to construct the collision robot from |
padding | The padding to use for all objects/links on the robot @scale scale A common scaling to use for all objects/links on the robot |
Definition at line 83 of file collision_env.cpp.
collision_detection::CollisionEnv::CollisionEnv | ( | const moveit::core::RobotModelConstPtr & | model, |
const WorldPtr & | world, | ||
double | padding = 0.0 , |
||
double | scale = 1.0 |
||
) |
Constructor.
model | A robot model to construct the collision robot from |
padding | The padding to use for all objects/links on the robot @scale scale A common scaling to use for all objects/links on the robot |
Definition at line 99 of file collision_env.cpp.
collision_detection::CollisionEnv::CollisionEnv | ( | const CollisionEnv & | other, |
const WorldPtr & | world | ||
) |
Copy constructor.
Definition at line 116 of file collision_env.cpp.
|
inlinevirtual |
Definition at line 74 of file collision_env.h.
|
virtual |
Check whether the robot model is in collision with itself or the world at a particular state. Any collision between any pair of links is checked for, NO collisions are ignored.
req | A CollisionRequest object that encapsulates the collision request |
res | A CollisionResult object that encapsulates the collision result |
state | The kinematic state for which checks are being made |
Reimplemented in collision_detection::CollisionEnvDistanceField.
Definition at line 310 of file collision_env.cpp.
|
virtual |
Check whether the robot model is in collision with itself or the world at a particular state. Allowed collisions specified by the allowed collision matrix are taken into account.
req | A CollisionRequest object that encapsulates the collision request |
res | A CollisionResult object that encapsulates the collision result |
state | The kinematic state for which checks are being made |
acm | The allowed collision matrix. |
Reimplemented in collision_detection::CollisionEnvDistanceField.
Definition at line 318 of file collision_env.cpp.
|
pure virtual |
Check whether the robot model is in collision with the world. Any collisions between a robot link and the world are considered. Self collisions are not checked.
req | A CollisionRequest object that encapsulates the collision request |
res | A CollisionResult object that encapsulates the collision result @robot robot The collision model for the robot |
state | The kinematic state for which checks are being made |
Implemented in collision_detection::CollisionEnvAllValid, collision_detection::CollisionEnvBullet, collision_detection::CollisionEnvFCL, and collision_detection::CollisionEnvDistanceField.
|
pure virtual |
Check whether the robot model is in collision with the world. Allowed collisions are ignored. Self collisions are not checked.
req | A CollisionRequest object that encapsulates the collision request |
res | A CollisionResult object that encapsulates the collision result @robot robot The collision model for the robot |
state | The kinematic state for which checks are being made |
acm | The allowed collision matrix. |
Implemented in collision_detection::CollisionEnvAllValid, collision_detection::CollisionEnvBullet, collision_detection::CollisionEnvFCL, and collision_detection::CollisionEnvDistanceField.
|
pure virtual |
Check whether the robot model is in collision with the world in a continuous manner (between two robot states). Allowed collisions are ignored. Self collisions are not checked.
req | A CollisionRequest object that encapsulates the collision request |
res | A CollisionResult object that encapsulates the collision result @robot robot The collision model for the robot |
state1 | The kinematic state at the start of the segment for which checks are being made |
state2 | The kinematic state at the end of the segment for which checks are being made |
acm | The allowed collision matrix. |
Implemented in collision_detection::CollisionEnvAllValid, collision_detection::CollisionEnvBullet, collision_detection::CollisionEnvFCL, and collision_detection::CollisionEnvDistanceField.
|
pure virtual |
Check whether the robot model is in collision with the world in a continuous manner (between two robot states). Allowed collisions are ignored. Self collisions are not checked.
req | A CollisionRequest object that encapsulates the collision request |
res | A CollisionResult object that encapsulates the collision result @robot robot The collision model for the robot |
state1 | The kinematic state at the start of the segment for which checks are being made |
state2 | The kinematic state at the end of the segment for which checks are being made |
acm | The allowed collision matrix. |
Implemented in collision_detection::CollisionEnvAllValid, collision_detection::CollisionEnvBullet, collision_detection::CollisionEnvFCL, and collision_detection::CollisionEnvDistanceField.
|
pure virtual |
Check for robot self collision. Any collision between any pair of links is checked for, NO collisions are ignored.
req | A CollisionRequest object that encapsulates the collision request |
res | A CollisionResult object that encapsulates the collision result |
state | The kinematic state for which checks are being made |
Implemented in collision_detection::CollisionEnvDistanceField, collision_detection::CollisionEnvAllValid, collision_detection::CollisionEnvBullet, and collision_detection::CollisionEnvFCL.
|
pure virtual |
Check for self collision. Allowed collisions specified by the allowed collision matrix are taken into account.
req | A CollisionRequest object that encapsulates the collision request |
res | A CollisionResult object that encapsulates the collision result |
state | The kinematic state for which checks are being made |
acm | The allowed collision matrix. |
Implemented in collision_detection::CollisionEnvDistanceField, collision_detection::CollisionEnvAllValid, collision_detection::CollisionEnvBullet, and collision_detection::CollisionEnvFCL.
|
pure virtual |
Compute the distance between a robot and the world.
req | A DistanceRequest object that encapsulates the distance request |
res | A DistanceResult object that encapsulates the distance result |
robot | The robot to check distance for |
state | The state for the robot to check distances from |
Implemented in collision_detection::CollisionEnvDistanceField, collision_detection::CollisionEnvAllValid, collision_detection::CollisionEnvBullet, and collision_detection::CollisionEnvFCL.
|
inline |
Compute the shortest distance between a robot and the world.
robot | The robot to check distance for |
state | The state for the robot to check distances from |
verbose | Output debug information about distance checks |
Definition at line 202 of file collision_env.h.
|
inline |
Compute the shortest distance between a robot and the world.
robot | The robot to check distance for |
state | The state for the robot to check distances from |
acm | Using an allowed collision matrix has the effect of ignoring distances from links that are always allowed to be in collision. |
verbose | Output debug information about distance checks |
Definition at line 220 of file collision_env.h.
|
pure virtual |
The distance to self-collision given the robot is at state state.
req | A DistanceRequest object that encapsulates the distance request |
res | A DistanceResult object that encapsulates the distance result |
state | The state of this robot to consider |
Implemented in collision_detection::CollisionEnvDistanceField, collision_detection::CollisionEnvAllValid, collision_detection::CollisionEnvBullet, and collision_detection::CollisionEnvFCL.
|
inline |
The distance to self-collision given the robot is at state state.
Definition at line 167 of file collision_env.h.
|
inline |
The distance to self-collision given the robot is at state state, ignoring the distances between links that are allowed to always collide (as specified by acm)
Definition at line 179 of file collision_env.h.
const std::map< std::string, double > & collision_detection::CollisionEnv::getLinkPadding | ( | ) | const |
Get the link paddings as a map (from link names to padding value)
Definition at line 194 of file collision_env.cpp.
double collision_detection::CollisionEnv::getLinkPadding | ( | const std::string & | link_name | ) | const |
Get the link padding for a particular link.
Definition at line 166 of file collision_env.cpp.
const std::map< std::string, double > & collision_detection::CollisionEnv::getLinkScale | ( | ) | const |
Get the link scaling as a map (from link names to scale value)
Definition at line 238 of file collision_env.cpp.
double collision_detection::CollisionEnv::getLinkScale | ( | const std::string & | link_name | ) | const |
Set the scaling for a particular link.
Definition at line 211 of file collision_env.cpp.
void collision_detection::CollisionEnv::getPadding | ( | std::vector< moveit_msgs::msg::LinkPadding > & | padding | ) | const |
Get the link padding as a vector of messages.
Definition at line 273 of file collision_env.cpp.
|
inline |
The kinematic model corresponding to this collision model.
Definition at line 255 of file collision_env.h.
void collision_detection::CollisionEnv::getScale | ( | std::vector< moveit_msgs::msg::LinkScale > & | scale | ) | const |
Get the link scaling as a vector of messages.
Definition at line 285 of file collision_env.cpp.
|
inline |
access the world geometry
Definition at line 240 of file collision_env.h.
|
inline |
access the world geometry
Definition at line 246 of file collision_env.h.
void collision_detection::CollisionEnv::setLinkPadding | ( | const std::map< std::string, double > & | padding | ) |
Set the link paddings using a map (from link names to padding value)
Definition at line 179 of file collision_env.cpp.
void collision_detection::CollisionEnv::setLinkPadding | ( | const std::string & | link_name, |
double | padding | ||
) |
Set the link padding for a particular link.
link_name | The link name to set padding for |
padding | The padding to set (in meters) |
Definition at line 154 of file collision_env.cpp.
void collision_detection::CollisionEnv::setLinkScale | ( | const std::map< std::string, double > & | scale | ) |
Set the link scaling using a map (from link names to scale value)
Definition at line 224 of file collision_env.cpp.
void collision_detection::CollisionEnv::setLinkScale | ( | const std::string & | link_name, |
double | scale | ||
) |
Set the scaling for a particular link.
Definition at line 199 of file collision_env.cpp.
void collision_detection::CollisionEnv::setPadding | ( | const std::vector< moveit_msgs::msg::LinkPadding > & | padding | ) |
Set the link padding from a vector of messages.
Definition at line 243 of file collision_env.cpp.
void collision_detection::CollisionEnv::setPadding | ( | double | padding | ) |
Set the link padding (for every link)
Definition at line 122 of file collision_env.cpp.
void collision_detection::CollisionEnv::setScale | ( | const std::vector< moveit_msgs::msg::LinkScale > & | scale | ) |
Set the link scaling from a vector of messages.
Definition at line 258 of file collision_env.cpp.
void collision_detection::CollisionEnv::setScale | ( | double | scale | ) |
Set the link scaling (for every link)
Definition at line 138 of file collision_env.cpp.
|
virtual |
set the world to use. This can be expensive unless the new and old world are empty. Passing nullptr will result in a new empty world being created.
Reimplemented in collision_detection::CollisionEnvBullet, collision_detection::CollisionEnvFCL, collision_detection::CollisionEnvDistanceField, and collision_detection::CollisionEnvHybrid.
Definition at line 301 of file collision_env.cpp.
|
protectedvirtual |
When the scale or padding is changed for a set of links by any of the functions in this class, updatedPaddingOrScaling() function is called. This function has an empty default implementation. The intention is to override this function in a derived class to allow for updating additional structures that may need such updating when link scale or padding changes.
links | the names of the links whose padding or scaling were updated |
Reimplemented in collision_detection::CollisionEnvDistanceField, collision_detection::CollisionEnvBullet, and collision_detection::CollisionEnvFCL.
Definition at line 297 of file collision_env.cpp.
|
protected |
The internally maintained map (from link names to padding)
Definition at line 318 of file collision_env.h.
|
protected |
The internally maintained map (from link names to scaling)
Definition at line 321 of file collision_env.h.
|
protected |
The kinematic model corresponding to this collision model.
Definition at line 315 of file collision_env.h.