44 const std::vector<const robot_model::LinkModel*>& links =
robot_model_->getLinkModelsWithCollisionGeometry();
49 for (
auto link : links)
50 for (std::size_t j = 0; j < link->getShapes().size(); ++j)
56 index = link->getFirstCollisionBodyTransformIndex() + j;
66 ROS_ERROR_NAMED(
"collision_detection.fcl",
"Unable to construct collision geometry for link '%s'",
67 link->getName().c_str());
78 std::vector<FCLGeometryConstPtr>& geoms)
const 81 for (std::size_t i = 0; i < shapes.size(); ++i)
92 fcl::Transform3f fcl_tf;
94 for (std::size_t i = 0; i <
geoms_.size(); ++i)
98 geoms_[i]->collision_geometry_data_->shape_index),
100 auto collObj =
new fcl::CollisionObject(*
fcl_objs_[i]);
101 collObj->setTransform(fcl_tf);
102 collObj->computeAABB();
107 std::vector<const robot_state::AttachedBody*> ab;
109 for (
auto& body : ab)
111 std::vector<FCLGeometryConstPtr> objs;
114 for (std::size_t k = 0; k < objs.size(); ++k)
115 if (objs[k]->collision_geometry_)
129 auto m =
new fcl::DynamicAABBTreeCollisionManager();
154 ROS_ERROR_NAMED(
"collision_detection.fcl",
"FCL continuous collision checking not yet implemented");
161 ROS_ERROR_NAMED(
"collision_detection.fcl",
"FCL continuous collision checking not yet implemented");
207 ROS_ERROR_NAMED(
"collision_detection.fcl",
"FCL continuous collision checking not yet implemented");
217 ROS_ERROR_NAMED(
"collision_detection.fcl",
"FCL continuous collision checking not yet implemented");
254 for (
const auto& link : links)
259 for (std::size_t j = 0; j < lmodel->
getShapes().size(); ++j)
272 ROS_ERROR_NAMED(
"collision_detection.fcl",
"Updating padding or scaling for unknown link: '%s'", link.c_str());
const std::string & getName() const
The name of this link.
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
Representation of a collision checking request.
void constructFCLObject(const robot_state::RobotState &state, FCLObject &fcl_obj) const
bool done_
Flag indicating whether collision checking is complete.
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, const robot_model::LinkModel *link, int shape_index)
virtual void checkOtherCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state, const CollisionRobot &other_robot, const robot_state::RobotState &other_state) const
Check for collision with a different robot (possibly a different kinematic model as well)...
const AllowedCollisionMatrix * acm
The allowed collision matrix used to filter checks.
bool collisionCallback(fcl::CollisionObject *o1, fcl::CollisionObject *o2, void *data)
bool distance
If true, compute proximity distance.
DistanceResultsData minimum_distance
ResultsData for the two objects with the minimum distance.
CollisionRobotFCL(const robot_model::RobotModelConstPtr &kmodel, double padding=0.0, double scale=1.0)
const std::map< std::string, double > & getLinkPadding() const
Get the link paddings as a map (from link names to padding value)
Representation of a collision checking result.
void allocSelfCollisionBroadPhase(const robot_state::RobotState &state, FCLManager &manager) const
const robot_model::RobotModelConstPtr & getRobotModel() const
The kinematic model corresponding to this collision model.
std::vector< FCLGeometryConstPtr > geoms_
int getFirstCollisionBodyTransformIndex() const
Generic interface to collision detection.
void enableGroup(const robot_model::RobotModelConstPtr &kmodel)
Compute active_components_only_ based on req_.
robot_model::RobotModelConstPtr robot_model_
The kinematic model corresponding to this collision model.
std::shared_ptr< fcl::BroadPhaseCollisionManager > manager_
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get the shapes that make up this attached body.
double distance
Closest distance between two bodies.
virtual void checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state) const
Check for self collision. Any collision between any pair of links is checked for, NO collisions are i...
void checkSelfCollisionHelper(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state, const AllowedCollisionMatrix *acm) const
const Eigen::Affine3d & getCollisionBodyTransform(const LinkModel *link, std::size_t index)
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
void getAttachedBodyObjects(const robot_state::AttachedBody *ab, std::vector< FCLGeometryConstPtr > &geoms) const
This class represents a collision model of the robot and can be used for self collision checks (to ch...
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot) ...
std::vector< FCLGeometryConstPtr > collision_geometry_
std::vector< FCLCollisionObjectConstPtr > fcl_objs_
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get shape associated to the collision geometry for this link.
double distance
The distance between two objects. If two objects are in collision, distance <= 0. ...
std::vector< FCLCollisionObjectPtr > collision_objects_
std::shared_ptr< const fcl::CollisionObject > FCLCollisionObjectConstPtr
const std::map< std::string, double > & getLinkScale() const
Get the link scaling as a map (from link names to scale value)
void enableGroup(const robot_model::RobotModelConstPtr &kmodel)
Compute active_components_only_ based on req_.
virtual void distanceSelf(const DistanceRequest &req, DistanceResult &res, const robot_state::RobotState &state) const override
The distance to self-collision given the robot is at state state.
void transform2fcl(const Eigen::Affine3d &b, fcl::Transform3f &f)
#define ROS_ERROR_NAMED(name,...)
Representation of a robot's state. This includes position, velocity, acceleration and effort...
A link from the robot. Contains the constant transform applied to the link and its geometry...
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...
bool done
Indicates if distance query is finished.
void checkOtherCollisionHelper(const CollisionRequest &req, CollisionResult &res, const robot_state::RobotState &state, const CollisionRobot &other_robot, const robot_state::RobotState &other_state, const AllowedCollisionMatrix *acm) const
bool distanceCallback(fcl::CollisionObject *o1, fcl::CollisionObject *o2, void *data, double &min_dist)
std::string group_name
The group name.
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
std::shared_ptr< fcl::CollisionObject > FCLCollisionObjectPtr
void registerTo(fcl::BroadPhaseCollisionManager *manager)
virtual void distanceOther(const DistanceRequest &req, DistanceResult &res, const robot_state::RobotState &state, const CollisionRobot &other_robot, const robot_state::RobotState &other_state) const override
The distance to self-collision given the robot is at state state.