Go to the documentation of this file.
45 #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
50 #include <fcl/broadphase/broadphase.h>
51 #include <fcl/collision.h>
52 #include <fcl/distance.h>
63 struct CollisionGeometryData
86 const std::string&
getID()
const
91 return ptr.link->getName();
93 return ptr.ab->getName();
108 return "Robot attached";
118 return type == other.type &&
ptr.raw == other.ptr.raw;
147 CollisionData(
const CollisionRequest* req, CollisionResult* res,
const AllowedCollisionMatrix* acm)
230 template <
typename T>
268 std::shared_ptr<fcl::BroadPhaseCollisionManagerd>
manager_;
321 #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
324 Eigen::Quaterniond q(b.linear());
325 f.setTranslation(
fcl::Vector3d(b.translation().x(), b.translation().y(), b.translation().z()));
326 f.setQuatRotation(fcl::Quaternion3f(q.w(), q.x(), q.y(), q.z()));
344 const CollisionGeometryData* cgd1 =
static_cast<const CollisionGeometryData*
>(fc.
o1->
getUserData());
345 c.body_name_1 = cgd1->getID();
346 c.body_type_1 = cgd1->type;
347 const CollisionGeometryData* cgd2 =
static_cast<const CollisionGeometryData*
>(fc.
o2->
getUserData());
348 c.body_name_2 = cgd2->getID();
349 c.body_type_2 = cgd2->type;
A link from the robot. Contains the constant transform applied to the link and its geometry.
Core components of MoveIt.
std::shared_ptr< const fcl::CollisionObjectd > FCLCollisionObjectConstPtr
CollisionResult * res_
The user-specified response location.
const std::set< const moveit::core::LinkModel * > * active_components_only_
If the collision request includes a group name, this set contains the pointers to the link models tha...
DistanceResult * res
Distance query results information.
@ WORLD_OBJECT
A body in the environment.
const CollisionRequest * req_
The collision request passed by the user.
void updateCollisionGeometryData(const T *data, int shape_index, bool newType)
Updates the collision_geometry_data_ with new data while also setting the collision_geometry_ to the ...
const DistanceRequest * req
Distance query request information.
Bundles the CollisionGeometryData and FCL collision geometry representation into a single class.
CollisionGeometryDataPtr collision_geometry_data_
Pointer to the user-defined geometry data.
std::shared_ptr< fcl::BroadPhaseCollisionManagerd > manager_
Transform3< double > Transform3d
BodyType type
Indicates the body type of the object.
bool collisionCallback(fcl::CollisionObjectd *o1, fcl::CollisionObjectd *o2, void *data)
Callback function used by the FCLManager used for each pair of collision objects to calculate object ...
CollisionGeometryData(const moveit::core::LinkModel *link, int index)
Constructor for a robot link collision geometry object.
void fcl2costsource(const fcl::CostSourced &fcs, CostSource &cs)
Transforms the FCL internal representation to the MoveIt CostSource data structure.
const std::string & getID() const
Returns the name which is saved in the member pointed to in ptr.
int shape_index
Multiple CollisionGeometryData objects construct a collision object. The collision object refers to a...
CollisionObject< S > * obj
Representation of a collision checking request.
bool done_
Flag indicating whether collision checking is complete.
std::vector< FCLCollisionObjectPtr > collision_objects_
Vector3< double > Vector3d
void cleanCollisionGeometryCache()
Increases the counter of the caches which can trigger the cleaning of expired entries from them.
Object defining bodies that can be attached to robot links.
const moveit::core::AttachedBody * ab
Definition of a structure for the allowed collision matrix.
void transform2fcl(const Eigen::Isometry3d &b, fcl::Transform3d &f)
Transforms an Eigen Isometry3d to FCL coordinate transformation.
bool distanceCallback(fcl::CollisionObjectd *o1, fcl::CollisionObjectd *o2, void *data, double &min_dist)
Callback function used by the FCLManager used for each pair of collision objects to calculate collisi...
union collision_detection::CollisionGeometryData::@0 ptr
Points to the type of body which contains the geometry.
Representation of a collision checking result.
const AllowedCollisionMatrix * acm_
The user-specified collision matrix (may be NULL).
#define ASSERT_ISOMETRY(transform)
std::shared_ptr< fcl::CollisionObjectd > FCLCollisionObjectPtr
Wrapper around world, link and attached objects' geometry data.
std::shared_ptr< fcl::CollisionGeometryd > collision_geometry_
Pointer to FCL collision geometry.
@ ROBOT_ATTACHED
A body attached to a robot link.
@ ROBOT_LINK
A link on the robot.
A representation of an object.
void unregisterFrom(fcl::BroadPhaseCollisionManagerd *manager)
const World::Object * obj
Bundles an FCLObject and a broadphase FCL collision manager.
Type
The types of bodies that are considered for collision.
void enableGroup(const moveit::core::RobotModelConstPtr &robot_model)
Compute active_components_only_ based on the joint group specified in req_.
const moveit::core::LinkModel * link
DistanceData(const DistanceRequest *req, DistanceResult *res)
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, const moveit::core::LinkModel *link, int shape_index)
Create new FCLGeometry object out of robot link model.
Result of a distance request.
A general high-level object which consists of multiple FCLCollisionObjects. It is the top level data ...
bool sameObject(const CollisionGeometryData &other) const
Check if two CollisionGeometryData objects point to the same source object.
std::shared_ptr< const Shape > ShapeConstPtr
void registerTo(fcl::BroadPhaseCollisionManagerd *manager)
Data structure which is passed to the distance callback function of the collision manager.
std::vector< FCLGeometryConstPtr > collision_geometry_
Geometry data corresponding to collision_objects_.
void * getUserData() const
void fcl2contact(const fcl::Contactd &fc, Contact &c)
Transforms an FCL contact into a MoveIt contact point.
Representation of a distance-reporting request.
geometry_msgs::TransformStamped t
std::string getTypeString() const
Returns a string of the corresponding type.
MOVEIT_STRUCT_FORWARD(CollisionGeometryData)
Vec3fX< details::Vec3Data< double > > Vector3d
bool done
Indicates if distance query is finished.
moveit_core
Author(s): Ioan Sucan
, Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 3 2024 02:25:24