37 #ifndef MOVEIT_COLLISION_DETECTION_COLLISION_COMMON_ 38 #define MOVEIT_COLLISION_DETECTION_COLLISION_COMMON_ 40 #include <boost/array.hpp> 41 #include <boost/function.hpp> 116 return (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]);
122 double c1 = cost * getVolume();
128 if (cost < other.
cost)
130 if (cost > other.
cost)
142 typedef std::map<std::pair<std::string, std::string>, std::vector<Contact> >
ContactMap;
150 distance = std::numeric_limits<double>::max();
153 cost_sources.clear();
183 , max_contacts_per_pair(1)
184 , max_cost_sources(1)
185 , min_cost_density(0.2)
219 boost::function<bool(const CollisionResult&)>
is_done;
225 namespace DistanceRequestTypes
241 : enable_nearest_points(false)
242 , enable_signed_distance(false)
243 , type(DistanceRequestType::
GLOBAL)
244 , max_contacts_per_body(1)
245 , active_components_only(nullptr)
247 , distance_threshold(
std::numeric_limits<double>::
max())
249 , compute_gradient(false)
256 if (robot_model->hasJointModelGroup(group_name))
257 active_components_only = &robot_model->getJointModelGroup(group_name)->getUpdatedLinkModelsSet();
259 active_components_only =
nullptr;
312 std::string link_names[2];
315 BodyType body_types[2];
327 distance = std::numeric_limits<double>::max();
328 nearest_points[0].setZero();
329 nearest_points[1].setZero();
351 typedef std::map<const std::pair<std::string, std::string>, std::vector<DistanceResultsData> >
DistanceMap;
373 minimum_distance.
clear();
DistanceRequestTypes::DistanceRequestType DistanceRequestType
Representation of a distance-reporting request.
Representation of a collision checking request.
Core components of MoveIt!
virtual ~CollisionRequest()
double cost
The partial cost (the probability of existance for the object there is a collision with) ...
Type
The types of bodies that are considered for collision.
boost::array< double, 3 > aabb_min
The minimum bound of the AABB defining the volume responsible for this partial cost.
double min_cost_density
When costs are computed, this is the minimum cost density for a CostSource to be included in the resu...
Vec3fX< details::Vec3Data< double > > Vector3d
std::map< const std::pair< std::string, std::string >, std::vector< DistanceResultsData > > DistanceMap
Mapping between the names of the collision objects and the DistanceResultData.
double getVolume() const
Get the volume of the AABB around the cost source.
Find the global minimum for each pair.
const AllowedCollisionMatrix * acm
The allowed collision matrix used to filter checks.
bool operator<(const DistanceResultsData &other)
Compare if the distance is less than another.
bool operator>(const DistanceResultsData &other)
Compare if the distance is greater than another.
void clear()
Clear structure data.
A body attached to a robot link.
bool enable_nearest_points
Indicate if nearest point information should be calculated.
ContactMap contacts
A map returning the pairs of body ids in contact, plus their contact details.
bool distance
If true, compute proximity distance.
DistanceResultsData minimum_distance
ResultsData for the two objects with the minimum distance.
Representation of a collision checking result.
MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix)
Generic interface to collision detection.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
Clear a previously stored result.
bool enable_signed_distance
Indicate if a signed distance should be calculated in a collision.
std::size_t contact_count
Number of contacts returned.
bool collision
True if collision was found, false otherwise.
const std::set< const robot_model::LinkModel * > * active_components_only
The set of active components to check.
std::map< std::pair< std::string, std::string >, std::vector< Contact > > ContactMap
std::set< CostSource > cost_sources
These are the individual cost sources when costs are computed.
double distance
Closest distance between two bodies.
boost::function< bool(const CollisionResult &)> is_done
Function call that decides whether collision detection should stop.
std::size_t max_contacts
Overall maximum number of contacts to compute.
bool verbose
Flag indicating whether information about detected collisions should be reported. ...
DistanceMap distances
A map of distance data for each link in the req.active_components_only.
boost::array< double, 3 > aabb_max
The maximum bound of the AABB defining the volume responsible for this partial cost.
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
void enableGroup(const robot_model::RobotModelConstPtr &robot_model)
Compute active_components_only_ based on req_.
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot) ...
Find all the contacts for a given pair.
double distance
The distance between two objects. If two objects are in collision, distance <= 0. ...
Generic representation of the distance information for a pair of objects.
Result of a distance request.
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
bool operator<(const CostSource &other) const
Order cost sources so that the most costly source is at the top.
bool collision
Indicates if two objects were in collision.
std::size_t max_contacts_per_pair
Maximum number of contacts to compute per pair of bodies (multiple bodies may be in contact at differ...
A body in the environment.
std::size_t max_contacts_per_body
Maximum number of contacts to store for bodies (multiple bodies may be within distance threshold) ...
std::size_t max_cost_sources
When costs are computed, this value defines how many of the top cost sources should be returned...
Find a limited(max_contacts_per_body) set of contacts for a given pair.
When collision costs are computed, this structure contains information about the partial cost incurre...
BodyTypes::Type BodyType
The types of bodies that are considered for collision.
double distance_threshold
std::string group_name
The group name.
double max(double a, double b)
void print(PropagationDistanceField &pdf, int numX, int numY, int numZ)
double distance(const urdf::Pose &transform)
bool verbose
Log debug information.
bool cost
If true, a collision cost is computed.
void clear()
Clear structure data.