This class represents a collision model of the robot and can be used for self collision checks (to check if the robot is in collision with itself) or in collision checks with a different robot. Collision checks with the environment are performed using the CollisionWorld class. More...
#include <collision_robot.h>
Public Member Functions | |
virtual void | checkOtherCollision (const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state, const CollisionRobot &other_robot, const robot_state::RobotState &other_state) const =0 |
Check for collision with a different robot (possibly a different kinematic model as well). Any collision between any pair of links is checked for, NO collisions are ignored. | |
virtual void | checkOtherCollision (const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state, const CollisionRobot &other_robot, const robot_state::RobotState &other_state, const AllowedCollisionMatrix &acm) const =0 |
Check for collision with a different robot (possibly a different kinematic model as well). Allowed collisions specified by the allowed collision matrix are taken into account. | |
virtual void | checkOtherCollision (const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2, const CollisionRobot &other_robot, const robot_state::RobotState &other_state1, const robot_state::RobotState &other_state2) const =0 |
Check for collision with a different robot (possibly a different kinematic model as well), in a continuous fashion. Any collision between any pair of links is checked for, NO collisions are ignored. | |
virtual void | checkOtherCollision (const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2, const CollisionRobot &other_robot, const robot_state::RobotState &other_state1, const robot_state::RobotState &other_state2, const AllowedCollisionMatrix &acm) const =0 |
Check for collision with a different robot (possibly a different kinematic model as well), in a continuous fashion. Allowed collisions specified by the allowed collision matrix are taken into account. | |
virtual void | checkSelfCollision (const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state) const =0 |
Check for 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 robot_state::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 | checkSelfCollision (const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2) const =0 |
Check for self collision in a continuous manner. Any collision between any pair of links is checked for, NO collisions are ignored. | |
virtual void | checkSelfCollision (const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state1, const robot_state::RobotState &state2, const AllowedCollisionMatrix &acm) const =0 |
Check for self collision. Allowed collisions specified by the allowed collision matrix are taken into account. | |
CollisionRobot (const robot_model::RobotModelConstPtr &model, double padding=0.0, double scale=1.0) | |
Constructor. | |
CollisionRobot (const CollisionRobot &other) | |
A copy constructor. | |
virtual double | distanceOther (const robot_state::RobotState &state, const CollisionRobot &other_robot, const robot_state::RobotState &other_state) const =0 |
The distance to another robot instance. | |
virtual double | distanceOther (const robot_state::RobotState &state, const CollisionRobot &other_robot, const robot_state::RobotState &other_state, const AllowedCollisionMatrix &acm) const =0 |
The distance to another robot instance, ignoring distances between links that are allowed to always collide. | |
virtual double | distanceSelf (const robot_state::RobotState &state) const =0 |
The distance to self-collision given the robot is at state state. | |
virtual double | distanceSelf (const robot_state::RobotState &state, const AllowedCollisionMatrix &acm) const =0 |
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 | getLinkPadding (const std::string &link_name) const |
Get the link padding for a particular link. | |
const std::map< std::string, double > & | getLinkPadding () const |
Get the link paddings as a map (from link names to padding value) | |
double | getLinkScale (const std::string &link_name) const |
Set the scaling for a particular link. | |
const std::map< std::string, double > & | getLinkScale () const |
Get the link scaling as a map (from link names to scale value) | |
void | getPadding (std::vector< moveit_msgs::LinkPadding > &padding) const |
Get the link padding as a vector of messages. | |
const robot_model::RobotModelConstPtr & | getRobotModel () const |
The kinematic model corresponding to this collision model. | |
void | getScale (std::vector< moveit_msgs::LinkScale > &scale) const |
Get the link scaling as a vector of messages. | |
void | setLinkPadding (const std::string &link_name, double padding) |
Set 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) | |
void | setLinkScale (const std::string &link_name, double scale) |
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) | |
void | setPadding (double padding) |
Set the link padding (for every link) | |
void | setPadding (const std::vector< moveit_msgs::LinkPadding > &padding) |
Set the link padding from a vector of messages. | |
void | setScale (double scale) |
Set the link scaling (for every link) | |
void | setScale (const std::vector< moveit_msgs::LinkScale > &scale) |
Set the link scaling from a vector of messages. | |
virtual | ~CollisionRobot () |
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 | |
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) | |
robot_model::RobotModelConstPtr | robot_model_ |
The kinematic model corresponding to this collision model. |
This class represents a collision model of the robot and can be used for self collision checks (to check if the robot is in collision with itself) or in collision checks with a different robot. Collision checks with the environment are performed using the CollisionWorld class.
Definition at line 51 of file collision_robot.h.
collision_detection::CollisionRobot::CollisionRobot | ( | const robot_model::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 A common scaling to use for all objects/links on the robot |
Definition at line 70 of file collision_robot.cpp.
collision_detection::CollisionRobot::CollisionRobot | ( | const CollisionRobot & | other | ) |
A copy constructor.
Definition at line 86 of file collision_robot.cpp.
virtual collision_detection::CollisionRobot::~CollisionRobot | ( | ) | [inline, virtual] |
Definition at line 65 of file collision_robot.h.
virtual void collision_detection::CollisionRobot::checkOtherCollision | ( | const CollisionRequest & | req, |
CollisionResult & | res, | ||
const robot_state::RobotState & | state, | ||
const CollisionRobot & | other_robot, | ||
const robot_state::RobotState & | other_state | ||
) | const [pure virtual] |
Check for collision with a different robot (possibly a different kinematic model as well). 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. |
other_robot | The collision representation for the other robot |
other_state | The kinematic state corresponding to the other robot |
Implemented in collision_detection::CollisionRobotFCL, and collision_detection::CollisionRobotAllValid.
virtual void collision_detection::CollisionRobot::checkOtherCollision | ( | const CollisionRequest & | req, |
CollisionResult & | res, | ||
const robot_state::RobotState & | state, | ||
const CollisionRobot & | other_robot, | ||
const robot_state::RobotState & | other_state, | ||
const AllowedCollisionMatrix & | acm | ||
) | const [pure virtual] |
Check for collision with a different robot (possibly a different kinematic model as well). 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. |
other_robot | The collision representation for the other robot |
other_state | The kinematic state corresponding to the other robot |
acm | The allowed collision matrix. |
Implemented in collision_detection::CollisionRobotFCL, and collision_detection::CollisionRobotAllValid.
virtual void collision_detection::CollisionRobot::checkOtherCollision | ( | const CollisionRequest & | req, |
CollisionResult & | res, | ||
const robot_state::RobotState & | state1, | ||
const robot_state::RobotState & | state2, | ||
const CollisionRobot & | other_robot, | ||
const robot_state::RobotState & | other_state1, | ||
const robot_state::RobotState & | other_state2 | ||
) | const [pure virtual] |
Check for collision with a different robot (possibly a different kinematic model as well), in a continuous fashion. 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 |
state1 | The kinematic state at the start of the segment for which checks are being made (this robot) |
state2 | The kinematic state at the end of the segment for which checks are being made (this robot) |
other_robot | The collision representation for the other robot |
other_state1 | The kinematic state at the start of the segment for which checks are being made (other robot) |
other_state2 | The kinematic state at the end of the segment for which checks are being made (other robot) |
Implemented in collision_detection::CollisionRobotFCL, and collision_detection::CollisionRobotAllValid.
virtual void collision_detection::CollisionRobot::checkOtherCollision | ( | const CollisionRequest & | req, |
CollisionResult & | res, | ||
const robot_state::RobotState & | state1, | ||
const robot_state::RobotState & | state2, | ||
const CollisionRobot & | other_robot, | ||
const robot_state::RobotState & | other_state1, | ||
const robot_state::RobotState & | other_state2, | ||
const AllowedCollisionMatrix & | acm | ||
) | const [pure virtual] |
Check for collision with a different robot (possibly a different kinematic model as well), in a continuous fashion. 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 |
state1 | The kinematic state at the start of the segment for which checks are being made (this robot) |
state2 | The kinematic state at the end of the segment for which checks are being made (this robot) |
other_robot | The collision representation for the other robot |
other_state1 | The kinematic state at the start of the segment for which checks are being made (other robot) |
other_state2 | The kinematic state at the end of the segment for which checks are being made (other robot) |
acm | The allowed collision matrix. |
Implemented in collision_detection::CollisionRobotFCL, and collision_detection::CollisionRobotAllValid.
virtual void collision_detection::CollisionRobot::checkSelfCollision | ( | const CollisionRequest & | req, |
CollisionResult & | res, | ||
const robot_state::RobotState & | state | ||
) | const [pure virtual] |
Check for 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::CollisionRobotFCL, and collision_detection::CollisionRobotAllValid.
virtual void collision_detection::CollisionRobot::checkSelfCollision | ( | const CollisionRequest & | req, |
CollisionResult & | res, | ||
const robot_state::RobotState & | state, | ||
const AllowedCollisionMatrix & | acm | ||
) | const [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::CollisionRobotFCL, and collision_detection::CollisionRobotAllValid.
virtual void collision_detection::CollisionRobot::checkSelfCollision | ( | const CollisionRequest & | req, |
CollisionResult & | res, | ||
const robot_state::RobotState & | state1, | ||
const robot_state::RobotState & | state2 | ||
) | const [pure virtual] |
Check for self collision in a continuous manner. 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 |
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 |
Implemented in collision_detection::CollisionRobotFCL, and collision_detection::CollisionRobotAllValid.
virtual void collision_detection::CollisionRobot::checkSelfCollision | ( | const CollisionRequest & | req, |
CollisionResult & | res, | ||
const robot_state::RobotState & | state1, | ||
const robot_state::RobotState & | state2, | ||
const AllowedCollisionMatrix & | acm | ||
) | const [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 |
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::CollisionRobotFCL, and collision_detection::CollisionRobotAllValid.
virtual double collision_detection::CollisionRobot::distanceOther | ( | const robot_state::RobotState & | state, |
const CollisionRobot & | other_robot, | ||
const robot_state::RobotState & | other_state | ||
) | const [pure virtual] |
The distance to another robot instance.
state | The state of this robot to consider |
other_robot | The other robot instance to measure distance to |
other_state | The state of the other robot |
Implemented in collision_detection::CollisionRobotFCL, and collision_detection::CollisionRobotAllValid.
virtual double collision_detection::CollisionRobot::distanceOther | ( | const robot_state::RobotState & | state, |
const CollisionRobot & | other_robot, | ||
const robot_state::RobotState & | other_state, | ||
const AllowedCollisionMatrix & | acm | ||
) | const [pure virtual] |
The distance to another robot instance, ignoring distances between links that are allowed to always collide.
state | The state of this robot to consider |
other_robot | The other robot instance to measure distance to |
other_state | The state of the other robot |
acm | The collision matrix specifying which links are allowed to always collide |
Implemented in collision_detection::CollisionRobotFCL, and collision_detection::CollisionRobotAllValid.
virtual double collision_detection::CollisionRobot::distanceSelf | ( | const robot_state::RobotState & | state | ) | const [pure virtual] |
The distance to self-collision given the robot is at state state.
Implemented in collision_detection::CollisionRobotFCL, and collision_detection::CollisionRobotAllValid.
virtual double collision_detection::CollisionRobot::distanceSelf | ( | const robot_state::RobotState & | state, |
const AllowedCollisionMatrix & | acm | ||
) | const [pure virtual] |
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)
Implemented in collision_detection::CollisionRobotFCL, and collision_detection::CollisionRobotAllValid.
double collision_detection::CollisionRobot::getLinkPadding | ( | const std::string & | link_name | ) | const |
Get the link padding for a particular link.
Definition at line 135 of file collision_robot.cpp.
const std::map< std::string, double > & collision_detection::CollisionRobot::getLinkPadding | ( | ) | const |
Get the link paddings as a map (from link names to padding value)
Definition at line 158 of file collision_robot.cpp.
double collision_detection::CollisionRobot::getLinkScale | ( | const std::string & | link_name | ) | const |
Set the scaling for a particular link.
Definition at line 174 of file collision_robot.cpp.
const std::map< std::string, double > & collision_detection::CollisionRobot::getLinkScale | ( | ) | const |
Get the link scaling as a map (from link names to scale value)
Definition at line 197 of file collision_robot.cpp.
void collision_detection::CollisionRobot::getPadding | ( | std::vector< moveit_msgs::LinkPadding > & | padding | ) | const |
Get the link padding as a vector of messages.
Definition at line 230 of file collision_robot.cpp.
const robot_model::RobotModelConstPtr& collision_detection::CollisionRobot::getRobotModel | ( | ) | const [inline] |
The kinematic model corresponding to this collision model.
Definition at line 188 of file collision_robot.h.
void collision_detection::CollisionRobot::getScale | ( | std::vector< moveit_msgs::LinkScale > & | scale | ) | const |
Get the link scaling as a vector of messages.
Definition at line 242 of file collision_robot.cpp.
void collision_detection::CollisionRobot::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 124 of file collision_robot.cpp.
void collision_detection::CollisionRobot::setLinkPadding | ( | const std::map< std::string, double > & | padding | ) |
Set the link paddings using a map (from link names to padding value)
Definition at line 144 of file collision_robot.cpp.
void collision_detection::CollisionRobot::setLinkScale | ( | const std::string & | link_name, |
double | scale | ||
) |
Set the scaling for a particular link.
Definition at line 163 of file collision_robot.cpp.
void collision_detection::CollisionRobot::setLinkScale | ( | const std::map< std::string, double > & | scale | ) |
Set the link scaling using a map (from link names to scale value)
Definition at line 183 of file collision_robot.cpp.
void collision_detection::CollisionRobot::setPadding | ( | double | padding | ) |
Set the link padding (for every link)
Definition at line 92 of file collision_robot.cpp.
void collision_detection::CollisionRobot::setPadding | ( | const std::vector< moveit_msgs::LinkPadding > & | padding | ) |
Set the link padding from a vector of messages.
Definition at line 202 of file collision_robot.cpp.
void collision_detection::CollisionRobot::setScale | ( | double | scale | ) |
Set the link scaling (for every link)
Definition at line 108 of file collision_robot.cpp.
void collision_detection::CollisionRobot::setScale | ( | const std::vector< moveit_msgs::LinkScale > & | scale | ) |
Set the link scaling from a vector of messages.
Definition at line 216 of file collision_robot.cpp.
void collision_detection::CollisionRobot::updatedPaddingOrScaling | ( | const std::vector< std::string > & | links | ) | [protected, virtual] |
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::CollisionRobotFCL.
Definition at line 254 of file collision_robot.cpp.
std::map<std::string, double> collision_detection::CollisionRobot::link_padding_ [protected] |
The internally maintained map (from link names to padding)
Definition at line 250 of file collision_robot.h.
std::map<std::string, double> collision_detection::CollisionRobot::link_scale_ [protected] |
The internally maintained map (from link names to scaling)
Definition at line 253 of file collision_robot.h.
robot_model::RobotModelConstPtr collision_detection::CollisionRobot::robot_model_ [protected] |
The kinematic model corresponding to this collision model.
Definition at line 247 of file collision_robot.h.