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();
181 , max_contacts_per_pair(1)
182 , max_cost_sources(1)
183 , min_cost_density(0.2)
217 boost::function<bool(const CollisionResult&)>
is_done;
223 namespace DistanceRequestTypes
238 : enable_nearest_points(false)
239 , enable_signed_distance(false)
240 , type(DistanceRequestType::
GLOBAL)
241 , max_contacts_per_body(1)
242 , active_components_only(nullptr)
244 , distance_threshold(
std::numeric_limits<double>::
max())
246 , compute_gradient(false)
253 if (kmodel->hasJointModelGroup(group_name))
254 active_components_only = &kmodel->getJointModelGroup(group_name)->getUpdatedLinkModelsSet();
256 active_components_only =
nullptr;
305 Eigen::Vector3d nearest_points[2];
308 std::string link_names[2];
311 BodyType body_types[2];
323 distance = std::numeric_limits<double>::max();
324 nearest_points[0].setZero();
325 nearest_points[1].setZero();
359 typedef std::map<const std::pair<std::string, std::string>, std::vector<DistanceResultsData> >
DistanceMap;
380 minimum_distance.
clear();
double getVolume() const
Get the volume of the AABB around the cost source.
DistanceRequestTypes::DistanceRequestType DistanceRequestType
Representation of a collision checking request.
BodyType body_types[2]
The object body type.
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...
bool operator<(const CostSource &other) const
Order cost sources so that the most costly source is at the top.
std::map< const std::pair< std::string, std::string >, std::vector< DistanceResultsData > > DistanceMap
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 ids of the bodies in contact, plus information about the contacts themse...
bool distance
If true, compute proximity distance.
DistanceResultsData minimum_distance
ResultsData for the two objects with the minimum distance.
Eigen::Vector3d nearest_points[2]
The nearest points.
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.
void enableGroup(const robot_model::RobotModelConstPtr &kmodel)
Compute active_components_only_ based on req_.
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
When costs are computed, the individual cost sources are.
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...
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 a limited(max_contacts_per_body) set of contacts for a given pair.
double distance
The distance between two objects. If two objects are in collision, distance <= 0. ...
bool contacts
If true, compute contacts.
bool collision
Indicates if two objects were in collision.
void operator=(const DistanceResultsData &other)
Update structure data given DistanceResultsData object.
std::string link_names[2]
The object link names.
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 the global minimum for each 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)
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.