#include <collision_robot_distance_field.h>
Public Member Functions | |
virtual void | checkOtherCollision (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state, const CollisionRobot &other_robot, const moveit::core::RobotState &other_state) const |
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. More... | |
virtual void | checkOtherCollision (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state, const CollisionRobot &other_robot, const moveit::core::RobotState &other_state, const collision_detection::AllowedCollisionMatrix &acm) const |
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. More... | |
virtual void | checkOtherCollision (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state1, const moveit::core::RobotState &state2, const CollisionRobot &other_robot, const moveit::core::RobotState &other_state1, const moveit::core::RobotState &other_state2) const |
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. More... | |
virtual void | checkOtherCollision (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state1, const moveit::core::RobotState &state2, const CollisionRobot &other_robot, const moveit::core::RobotState &other_state1, const moveit::core::RobotState &other_state2, const collision_detection::AllowedCollisionMatrix &acm) const |
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. More... | |
virtual void | checkSelfCollision (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state) const |
Check for self collision. Any collision between any pair of links is checked for, NO collisions are ignored. More... | |
void | checkSelfCollision (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state, GroupStateRepresentationPtr &gsr) const |
virtual void | checkSelfCollision (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix &acm) const |
Check for self collision. Allowed collisions specified by the allowed collision matrix are taken into account. More... | |
void | checkSelfCollision (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix &acm, GroupStateRepresentationPtr &gsr) const |
virtual void | checkSelfCollision (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state1, const moveit::core::RobotState &state2) const |
Check for self collision in a continuous manner. Any collision between any pair of links is checked for, NO collisions are ignored. More... | |
virtual void | checkSelfCollision (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state1, const moveit::core::RobotState &state2, const collision_detection::AllowedCollisionMatrix &acm) const |
Check for self collision. Allowed collisions specified by the allowed collision matrix are taken into account. More... | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | CollisionRobotDistanceField (const robot_model::RobotModelConstPtr &kmodel) |
CollisionRobotDistanceField (const CollisionRobot &col_robot, const Eigen::Vector3d &size, const Eigen::Vector3d &origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance, double padding) | |
CollisionRobotDistanceField (const robot_model::RobotModelConstPtr &kmodel, const std::map< std::string, std::vector< CollisionSphere >> &link_body_decompositions, double size_x=DEFAULT_SIZE_X, double size_y=DEFAULT_SIZE_Y, double size_z=DEFAULT_SIZE_Z, bool use_signed_distance_field=DEFAULT_USE_SIGNED_DISTANCE_FIELD, double resolution=DEFAULT_RESOLUTION, double collision_tolerance=DEFAULT_COLLISION_TOLERANCE, double max_propogation_distance=DEFAULT_MAX_PROPOGATION_DISTANCE, double padding=0.0, double scale=1.0) | |
CollisionRobotDistanceField (const CollisionRobotDistanceField &other) | |
void | createCollisionModelMarker (const moveit::core::RobotState &state, visualization_msgs::MarkerArray &model_markers) const |
virtual double | distanceOther (const moveit::core::RobotState &state, const CollisionRobot &other_robot, const moveit::core::RobotState &other_state) const |
virtual double | distanceOther (const moveit::core::RobotState &state, const CollisionRobot &other_robot, const moveit::core::RobotState &other_state, const collision_detection::AllowedCollisionMatrix &acm) const |
virtual void | distanceOther (const DistanceRequest &req, DistanceResult &res, const robot_state::RobotState &state, const CollisionRobot &other_robot, const robot_state::RobotState &other_state) const |
The distance to self-collision given the robot is at state state. More... | |
virtual double | distanceSelf (const moveit::core::RobotState &state) const |
virtual double | distanceSelf (const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix &acm) const |
virtual void | distanceSelf (const DistanceRequest &req, DistanceResult &res, const robot_state::RobotState &state) const |
The distance to self-collision given the robot is at state state. More... | |
DistanceFieldCacheEntryConstPtr | getLastDistanceFieldEntry () const |
void | initialize (const std::map< std::string, std::vector< CollisionSphere >> &link_body_decompositions, const Eigen::Vector3d &size, const Eigen::Vector3d &origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance) |
Public Member Functions inherited from collision_detection::CollisionRobot | |
CollisionRobot (const robot_model::RobotModelConstPtr &model, double padding=0.0, double scale=1.0) | |
Constructor. More... | |
CollisionRobot (const CollisionRobot &other) | |
A copy constructor. More... | |
double | distanceOther (const robot_state::RobotState &state, const CollisionRobot &other_robot, const robot_state::RobotState &other_state) const |
The distance to another robot instance. More... | |
double | distanceOther (const robot_state::RobotState &state, const CollisionRobot &other_robot, const robot_state::RobotState &other_state, const AllowedCollisionMatrix &acm) const |
The distance to another robot instance, ignoring distances between links that are allowed to always collide. More... | |
double | distanceSelf (const robot_state::RobotState &state) const |
The distance to self-collision given the robot is at state state. More... | |
double | distanceSelf (const robot_state::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) More... | |
double | getLinkPadding (const std::string &link_name) const |
Get the link padding for a particular link. More... | |
const std::map< std::string, double > & | getLinkPadding () const |
Get the link paddings as a map (from link names to padding value) More... | |
double | getLinkScale (const std::string &link_name) const |
Set the scaling for a particular link. More... | |
const std::map< std::string, double > & | getLinkScale () const |
Get the link scaling as a map (from link names to scale value) More... | |
void | getPadding (std::vector< moveit_msgs::LinkPadding > &padding) const |
Get the link padding as a vector of messages. More... | |
const robot_model::RobotModelConstPtr & | getRobotModel () const |
The kinematic model corresponding to this collision model. More... | |
void | getScale (std::vector< moveit_msgs::LinkScale > &scale) const |
Get the link scaling as a vector of messages. More... | |
void | setLinkPadding (const std::string &link_name, double padding) |
Set the link padding for a particular link. More... | |
void | setLinkPadding (const std::map< std::string, double > &padding) |
Set the link paddings using a map (from link names to padding value) More... | |
void | setLinkScale (const std::string &link_name, double scale) |
Set the scaling for a particular link. More... | |
void | setLinkScale (const std::map< std::string, double > &scale) |
Set the link scaling using a map (from link names to scale value) More... | |
void | setPadding (double padding) |
Set the link padding (for every link) More... | |
void | setPadding (const std::vector< moveit_msgs::LinkPadding > &padding) |
Set the link padding from a vector of messages. More... | |
void | setScale (double scale) |
Set the link scaling (for every link) More... | |
void | setScale (const std::vector< moveit_msgs::LinkScale > &scale) |
Set the link scaling from a vector of messages. More... | |
virtual | ~CollisionRobot () |
Protected Member Functions | |
void | addLinkBodyDecompositions (double resolution) |
void | addLinkBodyDecompositions (double resolution, const std::map< std::string, std::vector< CollisionSphere >> &link_body_decompositions) |
void | checkSelfCollisionHelper (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr) const |
bool | compareCacheEntryToAllowedCollisionMatrix (const DistanceFieldCacheEntryConstPtr &dfce, const collision_detection::AllowedCollisionMatrix &acm) const |
bool | compareCacheEntryToState (const DistanceFieldCacheEntryConstPtr &dfce, const moveit::core::RobotState &state) const |
void | generateCollisionCheckingStructures (const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr, bool generate_distance_field) const |
DistanceFieldCacheEntryPtr | generateDistanceFieldCacheEntry (const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, bool generate_distance_field) const |
DistanceFieldCacheEntryConstPtr | getDistanceFieldCacheEntry (const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm) const |
void | getGroupStateRepresentation (const DistanceFieldCacheEntryConstPtr &dfce, const moveit::core::RobotState &state, GroupStateRepresentationPtr &gsr) const |
bool | getIntraGroupCollisions (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, GroupStateRepresentationPtr &gsr) const |
bool | getIntraGroupProximityGradients (GroupStateRepresentationPtr &gsr) const |
PosedBodyPointDecompositionPtr | getPosedLinkBodyPointDecomposition (const moveit::core::LinkModel *ls) const |
PosedBodySphereDecompositionPtr | getPosedLinkBodySphereDecomposition (const moveit::core::LinkModel *ls, unsigned int ind) const |
bool | getSelfCollisions (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, GroupStateRepresentationPtr &gsr) const |
bool | getSelfProximityGradients (GroupStateRepresentationPtr &gsr) const |
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. More... | |
void | updateGroupStateRepresentationState (const moveit::core::RobotState &state, GroupStateRepresentationPtr &gsr) const |
Protected Attributes | |
double | collision_tolerance_ |
DistanceFieldCacheEntryPtr | distance_field_cache_entry_ |
std::map< std::string, std::map< std::string, bool > > | in_group_update_map_ |
std::map< std::string, unsigned int > | link_body_decomposition_index_map_ |
std::vector< BodyDecompositionConstPtr > | link_body_decomposition_vector_ |
double | max_propogation_distance_ |
Eigen::Vector3d | origin_ |
planning_scene::PlanningScenePtr | planning_scene_ |
std::map< std::string, GroupStateRepresentationPtr > | pregenerated_group_state_representation_map_ |
double | resolution_ |
Eigen::Vector3d | size_ |
boost::mutex | update_cache_lock_ |
bool | use_signed_distance_field_ |
Protected Attributes inherited from collision_detection::CollisionRobot | |
std::map< std::string, double > | link_padding_ |
The internally maintained map (from link names to padding) More... | |
std::map< std::string, double > | link_scale_ |
The internally maintained map (from link names to scaling) More... | |
robot_model::RobotModelConstPtr | robot_model_ |
The kinematic model corresponding to this collision model. More... | |
Friends | |
class | CollisionWorldDistanceField |
Definition at line 59 of file collision_robot_distance_field.h.
collision_detection::CollisionRobotDistanceField::CollisionRobotDistanceField | ( | const robot_model::RobotModelConstPtr & | kmodel | ) |
Definition at line 49 of file collision_robot_distance_field.cpp.
collision_detection::CollisionRobotDistanceField::CollisionRobotDistanceField | ( | const CollisionRobot & | col_robot, |
const Eigen::Vector3d & | size, | ||
const Eigen::Vector3d & | origin, | ||
bool | use_signed_distance_field, | ||
double | resolution, | ||
double | collision_tolerance, | ||
double | max_propogation_distance, | ||
double | padding | ||
) |
Definition at line 72 of file collision_robot_distance_field.cpp.
collision_detection::CollisionRobotDistanceField::CollisionRobotDistanceField | ( | const robot_model::RobotModelConstPtr & | kmodel, |
const std::map< std::string, std::vector< CollisionSphere >> & | link_body_decompositions, | ||
double | size_x = DEFAULT_SIZE_X , |
||
double | size_y = DEFAULT_SIZE_Y , |
||
double | size_z = DEFAULT_SIZE_Z , |
||
bool | use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD , |
||
double | resolution = DEFAULT_RESOLUTION , |
||
double | collision_tolerance = DEFAULT_COLLISION_TOLERANCE , |
||
double | max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE , |
||
double | padding = 0.0 , |
||
double | scale = 1.0 |
||
) |
Definition at line 61 of file collision_robot_distance_field.cpp.
collision_detection::CollisionRobotDistanceField::CollisionRobotDistanceField | ( | const CollisionRobotDistanceField & | other | ) |
Definition at line 84 of file collision_robot_distance_field.cpp.
|
protected |
Definition at line 952 of file collision_robot_distance_field.cpp.
|
protected |
Definition at line 1040 of file collision_robot_distance_field.cpp.
|
inlinevirtual |
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 |
Implements collision_detection::CollisionRobot.
Definition at line 118 of file collision_robot_distance_field.h.
|
inlinevirtual |
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. |
Implements collision_detection::CollisionRobot.
Definition at line 125 of file collision_robot_distance_field.h.
|
inlinevirtual |
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) |
Implements collision_detection::CollisionRobot.
Definition at line 133 of file collision_robot_distance_field.h.
|
inlinevirtual |
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. |
Implements collision_detection::CollisionRobot.
Definition at line 142 of file collision_robot_distance_field.h.
|
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 |
Implements collision_detection::CollisionRobot.
Definition at line 210 of file collision_robot_distance_field.cpp.
void collision_detection::CollisionRobotDistanceField::checkSelfCollision | ( | const collision_detection::CollisionRequest & | req, |
collision_detection::CollisionResult & | res, | ||
const moveit::core::RobotState & | state, | ||
GroupStateRepresentationPtr & | gsr | ||
) | const |
Definition at line 218 of file collision_robot_distance_field.cpp.
|
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. |
Implements collision_detection::CollisionRobot.
Definition at line 226 of file collision_robot_distance_field.cpp.
void collision_detection::CollisionRobotDistanceField::checkSelfCollision | ( | const collision_detection::CollisionRequest & | req, |
collision_detection::CollisionResult & | res, | ||
const moveit::core::RobotState & | state, | ||
const collision_detection::AllowedCollisionMatrix & | acm, | ||
GroupStateRepresentationPtr & | gsr | ||
) | const |
Definition at line 235 of file collision_robot_distance_field.cpp.
|
inlinevirtual |
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 |
Implements collision_detection::CollisionRobot.
Definition at line 103 of file collision_robot_distance_field.h.
|
inlinevirtual |
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. |
Implements collision_detection::CollisionRobot.
Definition at line 110 of file collision_robot_distance_field.h.
|
protected |
Definition at line 154 of file collision_robot_distance_field.cpp.
|
protected |
Definition at line 1305 of file collision_robot_distance_field.cpp.
|
protected |
Definition at line 1245 of file collision_robot_distance_field.cpp.
void collision_detection::CollisionRobotDistanceField::createCollisionModelMarker | ( | const moveit::core::RobotState & | state, |
visualization_msgs::MarkerArray & | model_markers | ||
) | const |
Definition at line 972 of file collision_robot_distance_field.cpp.
|
inlinevirtual |
Definition at line 164 of file collision_robot_distance_field.h.
|
inlinevirtual |
Definition at line 169 of file collision_robot_distance_field.h.
|
inlinevirtual |
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 |
other_robot | The other robot instance to measure distance to |
other_state | The state of the other robot |
Implements collision_detection::CollisionRobot.
Definition at line 181 of file collision_robot_distance_field.h.
|
inlinevirtual |
Definition at line 155 of file collision_robot_distance_field.h.
|
inlinevirtual |
Definition at line 159 of file collision_robot_distance_field.h.
|
inlinevirtual |
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::CollisionRobot.
Definition at line 176 of file collision_robot_distance_field.h.
|
protected |
Definition at line 135 of file collision_robot_distance_field.cpp.
|
protected |
Definition at line 700 of file collision_robot_distance_field.cpp.
|
protected |
Definition at line 179 of file collision_robot_distance_field.cpp.
|
protected |
std::cerr << "Attached " << dfce->attached_body_names_[i] << " index " << dfce->attached_body_link_state_indices_[i] << std::endl;
Definition at line 1147 of file collision_robot_distance_field.cpp.
|
protected |
Definition at line 405 of file collision_robot_distance_field.cpp.
|
protected |
Definition at line 634 of file collision_robot_distance_field.cpp.
|
inline |
Definition at line 187 of file collision_robot_distance_field.h.
|
protected |
Definition at line 1080 of file collision_robot_distance_field.cpp.
|
protected |
Definition at line 1071 of file collision_robot_distance_field.cpp.
|
protected |
Definition at line 249 of file collision_robot_distance_field.cpp.
|
protected |
Definition at line 330 of file collision_robot_distance_field.cpp.
void collision_detection::CollisionRobotDistanceField::initialize | ( | const std::map< std::string, std::vector< CollisionSphere >> & | link_body_decompositions, |
const Eigen::Vector3d & | size, | ||
const Eigen::Vector3d & | origin, | ||
bool | use_signed_distance_field, | ||
double | resolution, | ||
double | collision_tolerance, | ||
double | max_propogation_distance | ||
) |
Definition at line 101 of file collision_robot_distance_field.cpp.
|
inlineprotectedvirtual |
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 from collision_detection::CollisionRobot.
Definition at line 250 of file collision_robot_distance_field.h.
|
protected |
Definition at line 1093 of file collision_robot_distance_field.cpp.
|
friend |
Definition at line 61 of file collision_robot_distance_field.h.
|
protected |
Definition at line 256 of file collision_robot_distance_field.h.
|
protected |
Definition at line 263 of file collision_robot_distance_field.h.
|
protected |
Definition at line 264 of file collision_robot_distance_field.h.
|
protected |
Definition at line 260 of file collision_robot_distance_field.h.
|
protected |
Definition at line 259 of file collision_robot_distance_field.h.
|
protected |
Definition at line 257 of file collision_robot_distance_field.h.
|
protected |
Definition at line 253 of file collision_robot_distance_field.h.
|
protected |
Definition at line 267 of file collision_robot_distance_field.h.
|
protected |
Definition at line 265 of file collision_robot_distance_field.h.
|
protected |
Definition at line 255 of file collision_robot_distance_field.h.
|
protected |
Definition at line 250 of file collision_robot_distance_field.h.
|
mutableprotected |
Definition at line 262 of file collision_robot_distance_field.h.
|
protected |
Definition at line 254 of file collision_robot_distance_field.h.