collision_scene.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2020, University of Oxford
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without specific
15 // prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
31 #include <exotica_core/scene.h>
32 
33 #include <exotica_core/collision_scene_initializer.h>
34 
35 namespace exotica
36 {
37 inline bool IsRobotLink(std::shared_ptr<KinematicElement> e)
38 {
39  return e->is_robot_link || e->closest_robot_link.lock();
40 }
41 
43 {
45  CollisionSceneInitializer collision_scene_initializer = CollisionSceneInitializer(init);
46 
47  this->SetReplacePrimitiveShapesWithMeshes(collision_scene_initializer.ReplacePrimitiveShapesWithMeshes);
48  this->set_replace_cylinders_with_capsules(collision_scene_initializer.ReplaceCylindersWithCapsules);
49  this->SetWorldLinkPadding(collision_scene_initializer.WorldLinkPadding);
50  this->SetRobotLinkPadding(collision_scene_initializer.RobotLinkPadding);
51  this->SetWorldLinkScale(collision_scene_initializer.WorldLinkScale);
52  this->SetRobotLinkScale(collision_scene_initializer.RobotLinkScale);
53  this->robot_link_replacement_config_ = collision_scene_initializer.RobotLinkReplacementConfig;
54 
55  if (debug_) INFO_NAMED(object_name_, "Initialized CollisionScene of type " << GetObjectName());
56 }
57 
58 bool CollisionScene::IsAllowedToCollide(const std::string& o1, const std::string& o2, const bool& self)
59 {
60  std::shared_ptr<KinematicElement> e1 = scene_.lock()->GetKinematicTree().FindKinematicElementByName(o1);
61  std::shared_ptr<KinematicElement> e2 = scene_.lock()->GetKinematicTree().FindKinematicElementByName(o2);
62 
63  bool isRobot1 = IsRobotLink(e1);
64  bool isRobot2 = IsRobotLink(e2);
65  // Don't check collisions between world objects
66  if (!isRobot1 && !isRobot2) return false;
67  // Skip self collisions if requested
68  if (isRobot1 && isRobot2 && !self) return false;
69  // Skip collisions between shapes within the same objects
70  if (e1->parent.lock() == e2->parent.lock()) return false;
71  // Skip collisions between bodies attached to the same object
72  if (e1->closest_robot_link.lock() && e2->closest_robot_link.lock() && e1->closest_robot_link.lock() == e2->closest_robot_link.lock()) return false;
73 
74  if (isRobot1 && isRobot2)
75  {
76  const std::string& name1 = e1->closest_robot_link.lock() ? e1->closest_robot_link.lock()->segment.getName() : e1->parent.lock()->segment.getName();
77  const std::string& name2 = e2->closest_robot_link.lock() ? e2->closest_robot_link.lock()->segment.getName() : e2->parent.lock()->segment.getName();
78  return acm_.getAllowedCollision(name1, name2);
79  }
80  return true;
81 }
82 
83 } // namespace exotica
exotica::CollisionScene::SetWorldLinkPadding
void SetWorldLinkPadding(const double padding)
Definition: collision_scene.h:273
exotica::IsRobotLink
bool IsRobotLink(std::shared_ptr< KinematicElement > e)
Definition: collision_scene.cpp:37
exotica::Object::InstantiateObject
void InstantiateObject(const Initializer &init)
Definition: object.h:71
exotica::CollisionScene::scene_
std::weak_ptr< Scene > scene_
Stores a pointer to the Scene which owns this CollisionScene.
Definition: collision_scene.h:315
exotica::CollisionScene::InstantiateBase
virtual void InstantiateBase(const Initializer &init)
Instantiates the base properties of the CollisionScene.
Definition: collision_scene.cpp:42
exotica::AllowedCollisionMatrix::getAllowedCollision
bool getAllowedCollision(const std::string &name1, const std::string &name2) const
Definition: collision_scene.h:80
exotica::CollisionScene::SetWorldLinkScale
void SetWorldLinkScale(const double scale)
Definition: collision_scene.h:255
exotica::CollisionScene::SetRobotLinkScale
void SetRobotLinkScale(const double scale)
Definition: collision_scene.h:246
INFO_NAMED
#define INFO_NAMED(name, x)
Definition: printable.h:64
exotica::Object::object_name_
std::string object_name_
Definition: object.h:85
exotica
Definition: collision_scene.h:46
collision_scene.h
scene.h
exotica::CollisionScene::acm_
AllowedCollisionMatrix acm_
The allowed collision matrix.
Definition: collision_scene.h:318
exotica::Initializer
Definition: property.h:70
exotica::CollisionScene::robot_link_replacement_config_
std::string robot_link_replacement_config_
Filename for config file (YAML) which contains shape replacements.
Definition: collision_scene.h:342
exotica::CollisionScene::IsAllowedToCollide
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.
Definition: collision_scene.cpp:58
exotica::CollisionScene::set_replace_cylinders_with_capsules
void set_replace_cylinders_with_capsules(const bool value)
Definition: collision_scene.h:296
exotica::CollisionScene::debug_
bool debug_
Definition: collision_scene.h:302
init
void init(const M_string &remappings)
exotica::CollisionScene::SetRobotLinkPadding
void SetRobotLinkPadding(const double padding)
Definition: collision_scene.h:264
exotica::CollisionScene::SetReplacePrimitiveShapesWithMeshes
void SetReplacePrimitiveShapesWithMeshes(const bool value)
Definition: collision_scene.h:282
exotica::Object::GetObjectName
std::string GetObjectName()
Definition: object.h:66


exotica_core
Author(s): Yiming Yang, Michael Camilleri
autogenerated on Fri Aug 2 2024 08:43:02