Go to the documentation of this file.
30 #ifndef EXOTICA_CORE_COLLISION_SCENE_H_
31 #define EXOTICA_CORE_COLLISION_SCENE_H_
33 #include <Eigen/Dense>
38 #include <unordered_map>
39 #include <unordered_set>
45 #define REGISTER_COLLISION_SCENE_TYPE(TYPE, DERIV) EXOTICA_CORE_REGISTER(exotica::CollisionScene, TYPE, DERIV)
66 inline void setEntry(
const std::string& name1,
const std::string& name2) {
entries_[name1].insert(name2); }
72 names.push_back(it.first);
79 if (it ==
entries_.end())
return true;
80 return it->second.find(name2) == it->second.end();
84 std::unordered_map<std::string, std::unordered_set<std::string>>
entries_;
89 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
92 std::shared_ptr<KinematicElement>
e1;
93 std::shared_ptr<KinematicElement>
e2;
106 std::stringstream ss;
113 ss <<
"ContinuousCollisionProxy (empty)";
121 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
124 std::shared_ptr<KinematicElement>
e1;
125 std::shared_ptr<KinematicElement>
e2;
134 std::stringstream ss;
137 ss <<
"Proxy: '" <<
e1->segment.getName() <<
"' - '" <<
e2->segment.getName() <<
"', c1: " <<
contact1.transpose() <<
" c2: " <<
contact2.transpose() <<
" n1: " <<
normal1.transpose() <<
" n2: " <<
normal2.transpose() <<
" d: " <<
distance;
141 ss <<
"Proxy (empty)";
163 virtual bool IsAllowedToCollide(
const std::string& o1,
const std::string& o2,
const bool&
self);
168 virtual bool IsStateValid(
bool self =
true,
double safe_distance = 0.0) = 0;
193 virtual std::vector<CollisionProxy>
GetCollisionDistance(
const std::string& o1,
const bool&
self,
const bool& disable_collision_scene_update) {
ThrowPretty(
"Not implemented!"); }
225 virtual std::vector<ContinuousCollisionProxy>
ContinuousCollisionCast(
const std::vector<std::vector<std::tuple<std::string, Eigen::Isometry3d, Eigen::Isometry3d>>>& motion_transforms) {
ThrowPretty(
"Not implemented!"); }
228 virtual Eigen::Vector3d
GetTranslation(
const std::string& name) = 0;
245 ThrowPretty(
"Link scaling needs to be greater than or equal to 0");
254 ThrowPretty(
"Link scaling needs to be greater than or equal to 0");
263 HIGHLIGHT_NAMED(
"SetRobotLinkPadding",
"Generally, padding should be positive.");
272 HIGHLIGHT_NAMED(
"SetRobotLinkPadding",
"Generally, padding should be positive.");
286 virtual void UpdateCollisionObjects(
const std::map<std::string, std::weak_ptr<KinematicElement>>& objects) = 0;
344 #endif // EXOTICA_CORE_COLLISION_SCENE_H_
std::shared_ptr< KinematicElement > e2
void SetWorldLinkPadding(const double padding)
virtual std::vector< ContinuousCollisionProxy > ContinuousCollisionCast(const std::vector< std::vector< std::tuple< std::string, Eigen::Isometry3d, Eigen::Isometry3d >>> &motion_transforms)
Performs a continuous collision check by casting the active objects passed in against the static envi...
std::weak_ptr< Scene > scene_
Stores a pointer to the Scene which owns this CollisionScene.
double world_link_padding_
World link padding.
void SetACM(const AllowedCollisionMatrix &acm)
std::unordered_map< std::string, std::unordered_set< std::string > > entries_
virtual void InstantiateBase(const Initializer &init)
Instantiates the base properties of the CollisionScene.
The class of EXOTica Scene.
bool getAllowedCollision(const std::string &name1, const std::string &name2) const
virtual void Setup()
Setup additional construction that requires initialiser parameter.
Eigen::Vector3d contact_normal
size_t getNumberOfEntries() const
void SetWorldLinkScale(const double scale)
void SetRobotLinkScale(const double scale)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW CollisionProxy()
virtual bool IsCollisionFree(const std::string &o1, const std::string &o2, double safe_distance=0.0)
Checks if two objects are in collision.
virtual std::vector< std::string > GetCollisionRobotLinks()=0
Gets the collision robot links.
virtual void UpdateCollisionObjectTransforms()=0
Updates collision object transformations from the kinematic tree.
std::shared_ptr< KinematicElement > e2
virtual std::vector< CollisionProxy > GetCollisionDistance(const std::string &o1, const bool &self)
Gets the closest distance of any collision object which is allowed to collide with any collision obje...
virtual std::vector< CollisionProxy > GetCollisionDistance(bool self)
Computes collision distances.
double GetWorldLinkScale() const
virtual void UpdateCollisionObjects(const std::map< std::string, std::weak_ptr< KinematicElement >> &objects)=0
Creates the collision scene from kinematic elements.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW ContinuousCollisionProxy()
virtual std::vector< CollisionProxy > GetCollisionDistance(const std::vector< std::string > &objects, const bool &self)
Gets the closest distance of any collision object which is allowed to collide with any collision obje...
std::string Print() const
double robot_link_padding_
Robot link padding.
virtual ~CollisionScene()
#define HIGHLIGHT_NAMED(name, x)
std::shared_ptr< KinematicElement > e1
virtual std::vector< CollisionProxy > GetRobotToRobotCollisionDistance(double check_margin)
Gets the closest distances between links within the robot that are closer than check_margin.
virtual std::vector< CollisionProxy > GetCollisionDistance(const std::string &o1, const std::string &o2)
Computes collision distances between two objects.
void SetAlwaysExternallyUpdatedCollisionScene(const bool value)
bool replace_primitive_shapes_with_meshes_
Replace primitive shapes with meshes internally (e.g. when primitive shape algorithms are brittle,...
bool GetReplacePrimitiveShapesWithMeshes() const
AllowedCollisionMatrix(const AllowedCollisionMatrix &acm)
bool GetAlwaysExternallyUpdatedCollisionScene() const
AllowedCollisionMatrix & operator=(const AllowedCollisionMatrix &other)
AllowedCollisionMatrix acm_
The allowed collision matrix.
std::string robot_link_replacement_config_
Filename for config file (YAML) which contains shape replacements.
bool get_replace_cylinders_with_capsules() const
double GetRobotLinkScale() const
virtual bool IsAllowedToCollide(const std::string &o1, const std::string &o2, const bool &self)
Returns whether two collision objects/shapes are allowed to collide by name.
void setEntry(const std::string &name1, const std::string &name2)
std::shared_ptr< CollisionScene > CollisionScenePtr
bool always_externally_updated_collision_scene_
Whether the collision scene is automatically updated - if not, update on queries.
~AllowedCollisionMatrix()
Eigen::Vector3d contact_pos
double robot_link_scale_
Robot link scaling.
virtual Eigen::Vector3d GetTranslation(const std::string &name)=0
Returns the translation of the named collision object.
double GetRobotLinkPadding() const
bool hasEntry(const std::string &name) const
virtual std::vector< CollisionProxy > GetRobotToWorldCollisionDistance(double check_margin)
Gets the closest distances between links of the robot and the environment that are closer than check_...
The class of collision scene.
void set_replace_cylinders_with_capsules(const bool value)
bool replace_cylinders_with_capsules_
Replace cylinders with capsules internally.
double GetWorldLinkPadding() const
virtual std::vector< CollisionProxy > GetCollisionDistance(const std::string &o1, const bool &self, const bool &disable_collision_scene_update)
Gets the closest distance of any collision object which is allowed to collide with any collision obje...
virtual std::vector< std::string > GetCollisionWorldLinks()=0
Gets the collision world links.
double world_link_scale_
World link scaling.
std::string Print() const
virtual bool IsStateValid(bool self=true, double safe_distance=0.0)=0
Checks if the whole robot is valid (collision only).
void SetRobotLinkPadding(const double padding)
void SetReplacePrimitiveShapesWithMeshes(const bool value)
void AssignScene(std::shared_ptr< Scene > scene)
Sets a scene pointer to the CollisionScene for access to methods.
virtual 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)
Performs a continuous collision check between two objects with a linear interpolation between two giv...
std::shared_ptr< KinematicElement > e1
bool needs_update_of_collision_objects_
Indicates whether TriggerUpdateCollisionObjects needs to be called.
void getAllEntryNames(std::vector< std::string > &names) const
exotica_core
Author(s): Yiming Yang, Michael Camilleri
autogenerated on Fri Oct 20 2023 02:59:49