30 #ifndef EXOTICA_COLLISION_SCENE_FCL_LATEST_COLLISION_SCENE_FCL_LATEST_H_ 31 #define EXOTICA_COLLISION_SCENE_FCL_LATEST_COLLISION_SCENE_FCL_LATEST_H_ 38 #include <exotica_collision_scene_fcl_latest/collision_scene_fcl_latest_initializer.h> 64 double Distance = 1e300;
68 void Setup()
override;
70 bool IsAllowedToCollide(
const std::string& o1,
const std::string& o2,
const bool&
self)
override;
73 static bool CollisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2,
void* data);
86 std::vector<CollisionProxy>
GetCollisionDistance(
const std::string& o1,
const std::string& o2)
override;
87 std::vector<CollisionProxy>
GetCollisionDistance(
const std::string& o1,
const bool&
self =
true)
override;
88 std::vector<CollisionProxy>
GetCollisionDistance(
const std::vector<std::string>& objects,
const bool&
self =
true)
override;
89 std::vector<CollisionProxy>
GetCollisionDistance(
const std::string& o1,
const bool&
self =
true,
const bool& disable_collision_scene_update =
false)
override;
116 void UpdateCollisionObjects(
const std::map<std::string, std::weak_ptr<KinematicElement>>& objects)
override;
129 std::vector<std::shared_ptr<fcl::CollisionObjectd>>
fcl_cache_;
140 auto it = kinematic_elements_map_.find(frame_name);
141 if (it == kinematic_elements_map_.end())
ThrowPretty(
"KinematicElement is not a valid collision link:" << frame_name);
143 return it->second.lock();
148 auto it = fcl_robot_objects_map_.find(frame_name);
149 if (it == fcl_robot_objects_map_.end())
ThrowPretty(frame_name <<
" is not a robot collision object");
156 auto it = fcl_world_objects_map_.find(frame_name);
157 if (it == fcl_world_objects_map_.end())
ThrowPretty(frame_name <<
" is not a world collision object");
164 auto it = fcl_objects_map_.find(frame_name);
165 if (it == fcl_objects_map_.end())
ThrowPretty(frame_name <<
" is not a collision object");
172 #endif // EXOTICA_COLLISION_SCENE_FCL_LATEST_COLLISION_SCENE_FCL_LATEST_H_ bool IsAllowedToCollide(const std::string &o1, const std::string &o2, const bool &self) override
std::vector< std::shared_ptr< fcl::CollisionObjectd > > fcl_cache_
std::vector< std::string > GetCollisionRobotLinks() override
Gets the collision robot links.
void UpdateCollisionObjectTransforms() override
Updates collision object transformations from the kinematic tree.
std::vector< CollisionProxy > proxies
std::vector< fcl::CollisionObjectd * > GetRobotCollisionObjectsFromMapByName(const std::string &frame_name)
static bool CollisionCallbackDistance(fcl::CollisionObjectd *o1, fcl::CollisionObjectd *o2, void *data, double &dist)
Eigen::Vector3d GetTranslation(const std::string &name) override
static void CheckCollision(fcl::CollisionObjectd *o1, fcl::CollisionObjectd *o2, CollisionData *data)
fcl::CollisionRequestd request
bool IsCollisionFree(const std::string &o1, const std::string &o2, double safe_distance=0.0) override
std::shared_ptr< fcl::CollisionObjectd > ConstructFclCollisionObject(long i, std::shared_ptr< KinematicElement > element)
std::vector< fcl::CollisionObjectd * > fcl_objects_
std::vector< fcl::CollisionObjectd * > GetWorldCollisionObjectsFromMapByName(const std::string &frame_name)
std::vector< CollisionProxy > GetCollisionDistance(bool self) override
Computes collision distances.
CollisionData(CollisionSceneFCLLatest *scene_in)
CollisionSceneFCLLatest * scene
CollisionSceneFCLLatest * scene
bool IsStateValid(bool self=true, double safe_distance=0.0) override
Check if the whole robot is valid (collision only).
std::vector< CollisionProxy > GetRobotToWorldCollisionDistance(double check_margin) override
std::vector< CollisionProxy > GetRobotToRobotCollisionDistance(double check_margin) override
ContinuousCollisionProxy ContinuousCollisionCheck(const std::string &o1, const KDL::Frame &tf1_beg, const KDL::Frame &tf1_end, const std::string &o2, const KDL::Frame &tf2_beg, const KDL::Frame &tf2_end) override
Performs a continuous collision check between two objects with a linear interpolation between two giv...
std::shared_ptr< KinematicElement > GetKinematicElementFromMapByName(const std::string &frame_name)
fcl::DistanceResultd result
std::map< std::string, std::weak_ptr< KinematicElement > > kinematic_elements_map_
std::shared_ptr< fcl::BroadPhaseCollisionManagerd > broad_phase_collision_manager_
void UpdateCollisionObjects(const std::map< std::string, std::weak_ptr< KinematicElement >> &objects) override
Creates the collision scene from kinematic elements.
fcl::DistanceRequestd request
std::map< std::string, std::vector< fcl::CollisionObjectd * > > fcl_world_objects_map_
DistanceData(CollisionSceneFCLLatest *scene_in)
static void ComputeDistance(fcl::CollisionObjectd *o1, fcl::CollisionObjectd *o2, DistanceData *data)
std::vector< std::string > GetCollisionWorldLinks() override
Gets the collision world links.
fcl::CollisionResultd result
static bool CollisionCallback(fcl::CollisionObjectd *o1, fcl::CollisionObjectd *o2, void *data)
std::map< std::string, std::vector< fcl::CollisionObjectd * > > fcl_robot_objects_map_
std::vector< std::weak_ptr< KinematicElement > > kinematic_elements_
std::vector< fcl::CollisionObjectd * > GetCollisionObjectsFromMapByName(const std::string &frame_name)
std::map< std::string, std::vector< fcl::CollisionObjectd * > > fcl_objects_map_