moveit2
The MoveIt Motion Planning Framework for ROS 2.
|
FCL implementation of the CollisionEnv. More...
#include <collision_env_fcl.h>
Public Member Functions | |
CollisionEnvFCL ()=delete | |
CollisionEnvFCL (const moveit::core::RobotModelConstPtr &model, double padding=0.0, double scale=1.0) | |
CollisionEnvFCL (const moveit::core::RobotModelConstPtr &model, const WorldPtr &world, double padding=0.0, double scale=1.0) | |
CollisionEnvFCL (const CollisionEnvFCL &other, const WorldPtr &world) | |
~CollisionEnvFCL () override | |
void | checkSelfCollision (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const override |
Check for robot self collision. Any collision between any pair of links is checked for, NO collisions are ignored. | |
void | checkSelfCollision (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix &acm) const override |
Check for self collision. Allowed collisions specified by the allowed collision matrix are taken into account. | |
void | checkRobotCollision (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const override |
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. | |
void | checkRobotCollision (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix &acm) const override |
Check whether the robot model is in collision with the world. Allowed collisions are ignored. Self collisions are not checked. | |
void | checkRobotCollision (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state1, const moveit::core::RobotState &state2, const AllowedCollisionMatrix &acm) const override |
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. | |
void | checkRobotCollision (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state1, const moveit::core::RobotState &state2) const override |
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. | |
void | distanceSelf (const DistanceRequest &req, DistanceResult &res, const moveit::core::RobotState &state) const override |
The distance to self-collision given the robot is at state state. | |
void | distanceRobot (const DistanceRequest &req, DistanceResult &res, const moveit::core::RobotState &state) const override |
Compute the distance between a robot and the world. | |
void | setWorld (const WorldPtr &world) override |
![]() | |
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 | 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. | |
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) | |
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. | |
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 | |
void | updatedPaddingOrScaling (const std::vector< std::string > &links) override |
Updates the FCL collision geometry and objects saved in the CollisionRobotFCL members to reflect a new padding or scaling of the robot links. | |
void | checkSelfCollisionHelper (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix *acm) const |
Bundles the different checkSelfCollision functions into a single function. | |
void | checkRobotCollisionHelper (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix *acm) const |
Bundles the different checkRobotCollision functions into a single function. | |
void | constructFCLObjectWorld (const World::Object *obj, FCLObject &fcl_obj) const |
Construct an FCL collision object from MoveIt's World::Object. | |
void | updateFCLObject (const std::string &id) |
Updates the specified object in \m fcl_objs_ and in the manager from new data available in the World. | |
void | constructFCLObjectRobot (const moveit::core::RobotState &state, FCLObject &fcl_obj) const |
Out of the current robot state and its attached bodies construct an FCLObject which can then be used to check for collision. | |
void | allocSelfCollisionBroadPhase (const moveit::core::RobotState &state, FCLManager &manager) const |
Prepares for the collision check through constructing an FCL collision object out of the current robot state and specifying a broadphase collision manager of FCL where the constructed object is registered to. | |
void | getAttachedBodyObjects (const moveit::core::AttachedBody *ab, std::vector< FCLGeometryConstPtr > &geoms) const |
Converts all shapes which make up an attached body into a vector of FCLGeometryConstPtr. | |
Protected Attributes | |
std::vector< FCLGeometryConstPtr > | robot_geoms_ |
Vector of shared pointers to the FCL geometry for the objects in fcl_objs_. | |
std::vector< FCLCollisionObjectConstPtr > | robot_fcl_objs_ |
Vector of shared pointers to the FCL collision objects which make up the robot. | |
std::unique_ptr< fcl::BroadPhaseCollisionManagerd > | manager_ |
FCL collision manager which handles the collision checking process. | |
std::map< std::string, FCLObject > | fcl_objs_ |
![]() | |
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) | |
Additional Inherited Members | |
![]() | |
using | ObjectPtr = World::ObjectPtr |
using | ObjectConstPtr = World::ObjectConstPtr |
FCL implementation of the CollisionEnv.
Definition at line 53 of file collision_env_fcl.h.
|
delete |
collision_detection::CollisionEnvFCL::CollisionEnvFCL | ( | const moveit::core::RobotModelConstPtr & | model, |
double | padding = 0.0 , |
||
double | scale = 1.0 |
||
) |
collision_detection::CollisionEnvFCL::CollisionEnvFCL | ( | const moveit::core::RobotModelConstPtr & | model, |
const WorldPtr & | world, | ||
double | padding = 0.0 , |
||
double | scale = 1.0 |
||
) |
collision_detection::CollisionEnvFCL::CollisionEnvFCL | ( | const CollisionEnvFCL & | other, |
const WorldPtr & | world | ||
) |
|
override |
|
protected |
Prepares for the collision check through constructing an FCL collision object out of the current robot state and specifying a broadphase collision manager of FCL where the constructed object is registered to.
Definition at line 254 of file collision_env_fcl.cpp.
|
overridevirtual |
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 |
Implements collision_detection::CollisionEnv.
Definition at line 300 of file collision_env_fcl.cpp.
|
overridevirtual |
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. |
Implements collision_detection::CollisionEnv.
Definition at line 306 of file collision_env_fcl.cpp.
|
overridevirtual |
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. |
Implements collision_detection::CollisionEnv.
Definition at line 313 of file collision_env_fcl.cpp.
|
overridevirtual |
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. |
Implements collision_detection::CollisionEnv.
Definition at line 320 of file collision_env_fcl.cpp.
|
protected |
Bundles the different checkRobotCollision functions into a single function.
Definition at line 328 of file collision_env_fcl.cpp.
|
overridevirtual |
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 |
Implements collision_detection::CollisionEnv.
Definition at line 262 of file collision_env_fcl.cpp.
|
overridevirtual |
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. |
Implements collision_detection::CollisionEnv.
Definition at line 268 of file collision_env_fcl.cpp.
|
protected |
Bundles the different checkSelfCollision functions into a single function.
Definition at line 274 of file collision_env_fcl.cpp.
|
protected |
Out of the current robot state and its attached bodies construct an FCLObject which can then be used to check for collision.
The current state is used to recalculate the AABB of the FCL collision objects. However they are not computed from scratch (which would require call to computeLocalAABB()) but are only transformed according to the joint states.
state | The current robot state |
fcl_obj | The newly filled object |
Definition at line 212 of file collision_env_fcl.cpp.
|
protected |
Construct an FCL collision object from MoveIt's World::Object.
Definition at line 198 of file collision_env_fcl.cpp.
|
overridevirtual |
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 |
Implements collision_detection::CollisionEnv.
Definition at line 369 of file collision_env_fcl.cpp.
|
overridevirtual |
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 |
Implements collision_detection::CollisionEnv.
Definition at line 357 of file collision_env_fcl.cpp.
|
protected |
Converts all shapes which make up an attached body into a vector of FCLGeometryConstPtr.
When they are converted, they can be added to the FCL representation of the robot for collision checking.
ab | Pointer to the attached body |
geoms | Output vector of geometries |
Definition at line 183 of file collision_env_fcl.cpp.
|
overridevirtual |
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 from collision_detection::CollisionEnv.
Reimplemented in collision_detection::CollisionEnvHybrid.
Definition at line 417 of file collision_env_fcl.cpp.
|
overrideprotectedvirtual |
Updates the FCL collision geometry and objects saved in the CollisionRobotFCL members to reflect a new padding or scaling of the robot links.
It searches for the link through the pointed-to robot model of the CollisionRobot and then constructs new FCL collision objects and geometries depending on the changed robot model.
links | The names of the links which have been updated in the robot model |
Reimplemented from collision_detection::CollisionEnv.
Definition at line 493 of file collision_env_fcl.cpp.
|
protected |
Updates the specified object in \m fcl_objs_ and in the manager from new data available in the World.
If it does not exist in world, it is deleted. If it's not existing in \m fcl_objs_ yet, it's added there.
Definition at line 382 of file collision_env_fcl.cpp.
|
protected |
Definition at line 151 of file collision_env_fcl.h.
|
protected |
FCL collision manager which handles the collision checking process.
Definition at line 149 of file collision_env_fcl.h.
|
protected |
Vector of shared pointers to the FCL collision objects which make up the robot.
Definition at line 146 of file collision_env_fcl.h.
|
protected |
Vector of shared pointers to the FCL geometry for the objects in fcl_objs_.
Definition at line 143 of file collision_env_fcl.h.