Generic interface to collision detection. More...
Namespaces | |
namespace | AllowedCollision |
Any pair of bodies can have a collision state associated to it. | |
namespace | BodyTypes |
The types of bodies that are considered for collision. | |
Classes | |
class | AllowedCollisionMatrix |
Definition of a structure for the allowed collision matrix. All elements in the collision world are referred to by their names. This class represents which collisions are allowed to happen and which are not. More... | |
struct | CollisionData |
class | CollisionDetectorAllocator |
An allocator for a compatible CollisionWorld/CollisionRobot pair. More... | |
class | CollisionDetectorAllocatorAllValid |
An allocator for AllValid collision detectors. More... | |
class | CollisionDetectorAllocatorFCL |
An allocator for FCL collision detectors. More... | |
class | CollisionDetectorAllocatorTemplate |
Template class to make it easy to create an allocator for a specific CollisionWorld/CollisionRobot pair. More... | |
struct | CollisionGeometryData |
class | CollisionPlugin |
Plugin API for loading a custom collision detection robot/world. More... | |
struct | CollisionRequest |
Representation of a collision checking request. More... | |
struct | CollisionResult |
Representation of a collision checking result. More... | |
class | CollisionRobot |
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... | |
class | CollisionRobotAllValid |
class | CollisionRobotFCL |
class | CollisionWorld |
Perform collision checking with the environment. The collision world maintains a representation of the environment that the robot is operating in. More... | |
class | CollisionWorldAllValid |
class | CollisionWorldFCL |
struct | Contact |
Definition of a contact point. More... | |
struct | CostSource |
When collision costs are computed, this structure contains information about the partial cost incurred in a particular volume. More... | |
struct | FCLGeometry |
struct | FCLManager |
struct | FCLObject |
struct | FCLShapeCache |
struct | IfSameType |
struct | IfSameType< T, T > |
class | World |
Maintain a representation of the environment. More... | |
class | WorldDiff |
Maintain a diff list of changes that have happened to a World. More... | |
Typedefs | |
typedef BodyTypes::Type | BodyType |
The types of bodies that are considered for collision. | |
typedef boost::function< bool(collision_detection::Contact &)> | DecideContactFn |
Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type is CONDITIONAL) | |
typedef boost::shared_ptr < const fcl::CollisionObject > | FCLCollisionObjectConstPtr |
typedef boost::shared_ptr < fcl::CollisionObject > | FCLCollisionObjectPtr |
Functions | |
static bool | andDecideContact (const DecideContactFn &f1, const DecideContactFn &f2, collision_detection::Contact &contact) |
void | cleanCollisionGeometryCache () |
bool | collisionCallback (fcl::CollisionObject *o1, fcl::CollisionObject *o2, void *data) |
void | contactToMsg (const Contact &contact, moveit_msgs::ContactInformation &msg) |
void | costSourceToMsg (const CostSource &cost_source, moveit_msgs::CostSource &msg) |
FCLGeometryConstPtr | createCollisionGeometry (const shapes::ShapeConstPtr &shape, const robot_model::LinkModel *link, int shape_index) |
FCLGeometryConstPtr | createCollisionGeometry (const shapes::ShapeConstPtr &shape, const robot_state::AttachedBody *ab, int shape_index) |
FCLGeometryConstPtr | createCollisionGeometry (const shapes::ShapeConstPtr &shape, const World::Object *obj) |
FCLGeometryConstPtr | createCollisionGeometry (const shapes::ShapeConstPtr &shape, double scale, double padding, const robot_model::LinkModel *link, int shape_index) |
FCLGeometryConstPtr | createCollisionGeometry (const shapes::ShapeConstPtr &shape, double scale, double padding, const robot_state::AttachedBody *ab, int shape_index) |
FCLGeometryConstPtr | createCollisionGeometry (const shapes::ShapeConstPtr &shape, double scale, double padding, const World::Object *obj) |
template<typename BV , typename T > | |
FCLGeometryConstPtr | createCollisionGeometry (const shapes::ShapeConstPtr &shape, const T *data, int shape_index) |
template<typename BV , typename T > | |
FCLGeometryConstPtr | createCollisionGeometry (const shapes::ShapeConstPtr &shape, double scale, double padding, const T *data, int shape_index) |
bool | distanceCallback (fcl::CollisionObject *o1, fcl::CollisionObject *o2, void *data, double &min_dist) |
void | fcl2contact (const fcl::Contact &fc, Contact &c) |
void | fcl2costsource (const fcl::CostSource &fcs, CostSource &cs) |
void | getCollisionMarkersFromContacts (visualization_msgs::MarkerArray &arr, const std::string &frame_id, const CollisionResult::ContactMap &con, const std_msgs::ColorRGBA &color, const ros::Duration &lifetime, const double radius=0.035) |
void | getCollisionMarkersFromContacts (visualization_msgs::MarkerArray &arr, const std::string &frame_id, const CollisionResult::ContactMap &con) |
void | getCostMarkers (visualization_msgs::MarkerArray &arr, const std::string &frame_id, std::set< CostSource > &cost_sources) |
void | getCostMarkers (visualization_msgs::MarkerArray &arr, const std::string &frame_id, std::set< CostSource > &cost_sources, const std_msgs::ColorRGBA &color, const ros::Duration &lifetime) |
bool | getSensorPositioning (geometry_msgs::Point &point, const std::set< CostSource > &cost_sources) |
template<typename BV , typename T > | |
FCLShapeCache & | GetShapeCache () |
double | getTotalCost (const std::set< CostSource > &cost_sources) |
void | intersectCostSources (std::set< CostSource > &cost_sources, const std::set< CostSource > &a, const std::set< CostSource > &b) |
MOVEIT_CLASS_FORWARD (CollisionPlugin) | |
MOVEIT_CLASS_FORWARD (WorldDiff) | |
MOVEIT_CLASS_FORWARD (CollisionDetectorAllocator) | |
MOVEIT_CLASS_FORWARD (CollisionRobot) | |
MOVEIT_CLASS_FORWARD (CollisionGeometryData) | |
MOVEIT_CLASS_FORWARD (CollisionWorld) | |
MOVEIT_CLASS_FORWARD (World) | |
MOVEIT_CLASS_FORWARD (AllowedCollisionMatrix) | |
MOVEIT_CLASS_FORWARD (FCLGeometry) | |
int | refineContactNormals (const World::ObjectConstPtr &object, CollisionResult &res, double cell_bbx_search_distance=1.0, double allowed_angle_divergence=0.0, bool estimate_depth=false, double iso_value=0.5, double metaball_radius_multiple=1.5) |
Re-proceses contact normals for an octomap by estimating a metaball iso-surface using the centers of occupied cells in a neighborhood of the contact point. | |
void | removeCostSources (std::set< CostSource > &cost_sources, const std::set< CostSource > &cost_sources_to_remove, double overlap_fraction) |
void | removeOverlapping (std::set< CostSource > &cost_sources, double overlap_fraction) |
void | transform2fcl (const Eigen::Affine3d &b, fcl::Transform3f &f) |
fcl::Transform3f | transform2fcl (const Eigen::Affine3d &b) |
Generic interface to collision detection.
The types of bodies that are considered for collision.
Definition at line 69 of file include/moveit/collision_detection/collision_common.h.
typedef boost::function<bool(collision_detection::Contact&)> collision_detection::DecideContactFn |
Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type is CONDITIONAL)
Definition at line 73 of file collision_matrix.h.
typedef boost::shared_ptr<const fcl::CollisionObject> collision_detection::FCLCollisionObjectConstPtr |
Definition at line 192 of file fcl/include/moveit/collision_detection_fcl/collision_common.h.
typedef boost::shared_ptr<fcl::CollisionObject> collision_detection::FCLCollisionObjectPtr |
Definition at line 191 of file fcl/include/moveit/collision_detection_fcl/collision_common.h.
static bool collision_detection::andDecideContact | ( | const DecideContactFn & | f1, |
const DecideContactFn & | f2, | ||
collision_detection::Contact & | contact | ||
) | [static] |
Definition at line 268 of file collision_matrix.cpp.
Definition at line 753 of file collision_common.cpp.
bool collision_detection::collisionCallback | ( | fcl::CollisionObject * | o1, |
fcl::CollisionObject * | o2, | ||
void * | data | ||
) |
Definition at line 45 of file collision_common.cpp.
void collision_detection::contactToMsg | ( | const Contact & | contact, |
moveit_msgs::ContactInformation & | msg | ||
) |
Definition at line 277 of file collision_tools.cpp.
void collision_detection::costSourceToMsg | ( | const CostSource & | cost_source, |
moveit_msgs::CostSource & | msg | ||
) |
Definition at line 266 of file collision_tools.cpp.
FCLGeometryConstPtr collision_detection::createCollisionGeometry | ( | const shapes::ShapeConstPtr & | shape, |
const robot_model::LinkModel * | link, | ||
int | shape_index | ||
) |
Definition at line 703 of file collision_common.cpp.
FCLGeometryConstPtr collision_detection::createCollisionGeometry | ( | const shapes::ShapeConstPtr & | shape, |
const robot_state::AttachedBody * | ab, | ||
int | shape_index | ||
) |
Definition at line 709 of file collision_common.cpp.
FCLGeometryConstPtr collision_detection::createCollisionGeometry | ( | const shapes::ShapeConstPtr & | shape, |
const World::Object * | obj | ||
) |
Definition at line 715 of file collision_common.cpp.
FCLGeometryConstPtr collision_detection::createCollisionGeometry | ( | const shapes::ShapeConstPtr & | shape, |
double | scale, | ||
double | padding, | ||
const robot_model::LinkModel * | link, | ||
int | shape_index | ||
) |
Definition at line 735 of file collision_common.cpp.
FCLGeometryConstPtr collision_detection::createCollisionGeometry | ( | const shapes::ShapeConstPtr & | shape, |
double | scale, | ||
double | padding, | ||
const robot_state::AttachedBody * | ab, | ||
int | shape_index | ||
) |
Definition at line 741 of file collision_common.cpp.
FCLGeometryConstPtr collision_detection::createCollisionGeometry | ( | const shapes::ShapeConstPtr & | shape, |
double | scale, | ||
double | padding, | ||
const World::Object * | obj | ||
) |
Definition at line 747 of file collision_common.cpp.
FCLGeometryConstPtr collision_detection::createCollisionGeometry | ( | const shapes::ShapeConstPtr & | shape, |
const T * | data, | ||
int | shape_index | ||
) |
Definition at line 512 of file collision_common.cpp.
FCLGeometryConstPtr collision_detection::createCollisionGeometry | ( | const shapes::ShapeConstPtr & | shape, |
double | scale, | ||
double | padding, | ||
const T * | data, | ||
int | shape_index | ||
) |
Definition at line 721 of file collision_common.cpp.
bool collision_detection::distanceCallback | ( | fcl::CollisionObject * | o1, |
fcl::CollisionObject * | o2, | ||
void * | data, | ||
double & | min_dist | ||
) |
Definition at line 376 of file collision_common.cpp.
void collision_detection::fcl2contact | ( | const fcl::Contact & | fc, |
Contact & | c | ||
) | [inline] |
Definition at line 242 of file fcl/include/moveit/collision_detection_fcl/collision_common.h.
void collision_detection::fcl2costsource | ( | const fcl::CostSource & | fcs, |
CostSource & | cs | ||
) | [inline] |
Definition at line 255 of file fcl/include/moveit/collision_detection_fcl/collision_common.h.
void collision_detection::getCollisionMarkersFromContacts | ( | visualization_msgs::MarkerArray & | arr, |
const std::string & | frame_id, | ||
const CollisionResult::ContactMap & | con, | ||
const std_msgs::ColorRGBA & | color, | ||
const ros::Duration & | lifetime, | ||
const double | radius = 0.035 |
||
) |
Definition at line 95 of file collision_tools.cpp.
void collision_detection::getCollisionMarkersFromContacts | ( | visualization_msgs::MarkerArray & | arr, |
const std::string & | frame_id, | ||
const CollisionResult::ContactMap & | con | ||
) |
Definition at line 51 of file collision_tools.cpp.
void collision_detection::getCostMarkers | ( | visualization_msgs::MarkerArray & | arr, |
const std::string & | frame_id, | ||
std::set< CostSource > & | cost_sources | ||
) |
Definition at line 40 of file collision_tools.cpp.
void collision_detection::getCostMarkers | ( | visualization_msgs::MarkerArray & | arr, |
const std::string & | frame_id, | ||
std::set< CostSource > & | cost_sources, | ||
const std_msgs::ColorRGBA & | color, | ||
const ros::Duration & | lifetime | ||
) |
Definition at line 63 of file collision_tools.cpp.
bool collision_detection::getSensorPositioning | ( | geometry_msgs::Point & | point, |
const std::set< CostSource > & | cost_sources | ||
) |
Definition at line 136 of file collision_tools.cpp.
FCLShapeCache& collision_detection::GetShapeCache | ( | ) |
Definition at line 487 of file collision_common.cpp.
double collision_detection::getTotalCost | ( | const std::set< CostSource > & | cost_sources | ) |
Definition at line 149 of file collision_tools.cpp.
void collision_detection::intersectCostSources | ( | std::set< CostSource > & | cost_sources, |
const std::set< CostSource > & | a, | ||
const std::set< CostSource > & | b | ||
) |
Definition at line 158 of file collision_tools.cpp.
collision_detection::MOVEIT_CLASS_FORWARD | ( | CollisionPlugin | ) |
collision_detection::MOVEIT_CLASS_FORWARD | ( | WorldDiff | ) |
collision_detection::MOVEIT_CLASS_FORWARD | ( | CollisionDetectorAllocator | ) |
collision_detection::MOVEIT_CLASS_FORWARD | ( | CollisionRobot | ) |
collision_detection::MOVEIT_CLASS_FORWARD | ( | CollisionGeometryData | ) |
collision_detection::MOVEIT_CLASS_FORWARD | ( | CollisionWorld | ) |
collision_detection::MOVEIT_CLASS_FORWARD | ( | AllowedCollisionMatrix | ) |
collision_detection::MOVEIT_CLASS_FORWARD | ( | FCLGeometry | ) |
int collision_detection::refineContactNormals | ( | const World::ObjectConstPtr & | object, |
CollisionResult & | res, | ||
double | cell_bbx_search_distance = 1.0 , |
||
double | allowed_angle_divergence = 0.0 , |
||
bool | estimate_depth = false , |
||
double | iso_value = 0.5 , |
||
double | metaball_radius_multiple = 1.5 |
||
) |
Re-proceses contact normals for an octomap by estimating a metaball iso-surface using the centers of occupied cells in a neighborhood of the contact point.
This is an implementation of the algorithm described in: A. Leeper, S. Chan, K. Salisbury. Point Clouds Can Be Represented as Implicit Surfaces for Constraint-Based Haptic Rendering. ICRA, May 2012, St Paul, MN. http://adamleeper.com/research/papers/2012_ICRA_leeper-chan-salisbury.pdf
The | octomap originally used for collision detection. |
The | collision result (which will get its normals updated) |
The | distance, as a multiple of the octomap cell size, from which to include neighboring cells. |
The | minimum angle change required for a normal to be over-written |
Whether | to request a depth estimate from the algorithm (experimental...) |
The | iso-surface threshold value (0.5 is a reasonable default). |
The | metaball radius, as a multiple of the octomap cell size (1.5 is a reasonable default) |
Definition at line 59 of file collision_octomap_filter.cpp.
void collision_detection::removeCostSources | ( | std::set< CostSource > & | cost_sources, |
const std::set< CostSource > & | cost_sources_to_remove, | ||
double | overlap_fraction | ||
) |
Definition at line 212 of file collision_tools.cpp.
void collision_detection::removeOverlapping | ( | std::set< CostSource > & | cost_sources, |
double | overlap_fraction | ||
) |
Definition at line 182 of file collision_tools.cpp.
void collision_detection::transform2fcl | ( | const Eigen::Affine3d & | b, |
fcl::Transform3f & | f | ||
) | [inline] |
Definition at line 228 of file fcl/include/moveit/collision_detection_fcl/collision_common.h.
fcl::Transform3f collision_detection::transform2fcl | ( | const Eigen::Affine3d & | b | ) | [inline] |
Definition at line 235 of file fcl/include/moveit/collision_detection_fcl/collision_common.h.