Tesseract
Motion Planning Environment
Loading...
Searching...
No Matches
collision_compound_compound_unit.hpp
Go to the documentation of this file.
1#ifndef TESSERACT_COLLISION_COLLISION_COMPOUND_COMPOUND_UNIT_HPP
2#define TESSERACT_COLLISION_COLLISION_COMPOUND_COMPOUND_UNIT_HPP
3
6#include <octomap/octomap.h>
8
12
14{
15namespace detail
16{
17template <class T>
18inline void addCollisionObjects(T& checker)
19{
21 // Add Octomap
23 std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/meshes/box_2m.bt";
24 auto ot = std::make_shared<octomap::OcTree>(path);
25 CollisionShapePtr dense_octomap = std::make_shared<tesseract_geometry::Octree>(ot, tesseract_geometry::Octree::BOX);
26 Eigen::Isometry3d octomap_pose;
27 octomap_pose.setIdentity();
28 octomap_pose.translation() = Eigen::Vector3d(1.1, 0, 0);
29
30 CollisionShapesConst obj1_shapes;
32 obj1_shapes.push_back(dense_octomap);
33 obj1_poses.push_back(octomap_pose);
34
35 checker.addCollisionObject("octomap1_link", 0, obj1_shapes, obj1_poses);
36
38 // Add second octomap
40 auto ot_b = std::make_shared<octomap::OcTree>(path);
41 CollisionShapePtr dense_octomap_b =
42 std::make_shared<tesseract_geometry::Octree>(ot_b, tesseract_geometry::Octree::BOX);
43 Eigen::Isometry3d octomap_pose_b;
44 octomap_pose_b.setIdentity();
45 octomap_pose_b.translation() = Eigen::Vector3d(-1.1, 0, 0);
46
47 CollisionShapesConst obj2_shapes;
49 obj2_shapes.push_back(dense_octomap_b);
50 obj2_poses.push_back(octomap_pose_b);
51
52 checker.addCollisionObject("octomap2_link", 0, obj2_shapes, obj2_poses);
53
54 EXPECT_TRUE(checker.getCollisionObjects().size() == 2);
55 const auto& co = checker.getCollisionObjects();
56 for (std::size_t i = 0; i < co.size(); ++i)
57 {
58 EXPECT_TRUE(checker.getCollisionObjectGeometries(co[i]).size() == 1);
59 EXPECT_TRUE(checker.getCollisionObjectGeometriesTransforms(co[i]).size() == 1);
60 const auto& cgt = checker.getCollisionObjectGeometriesTransforms(co[i]);
61 if (i == 0)
62 {
63 EXPECT_TRUE(cgt[0].isApprox(octomap_pose, 1e-5));
64 }
65 else
66 {
67 EXPECT_TRUE(cgt[0].isApprox(octomap_pose_b, 1e-5));
68 }
69 }
70}
71
73{
75 // Test when object is in collision
77 std::vector<std::string> active_links{ "octomap1_link", "octomap2_link" };
79 std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
81
82 EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr);
83
86
87 // Set the collision object transforms
89 location["octomap1_link"] = Eigen::Isometry3d::Identity();
90 location["octomap2_link"] = Eigen::Isometry3d::Identity();
91 checker.setCollisionObjectsTransform(location);
92
93 // Perform collision check
94 ContactResultMap result;
96
99
100 EXPECT_TRUE(!result_vector.empty());
101 for (const auto& cr : result_vector)
102 {
103 EXPECT_NEAR(cr.distance, 0.20, 0.001);
104 }
105}
106
108{
110 // Test when object is in collision
112 std::vector<std::string> active_links{ "octomap1_link" };
114 std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
116
117 EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr);
118
121
122 // Set Pair
123 checker.setPairCollisionMarginData("octomap1_link", "octomap2_link", 0.25);
124
125 // Set the collision object transforms
127 location["octomap2_link"] = Eigen::Isometry3d::Identity();
128 checker.setCollisionObjectsTransform(location);
129
130 // Set the collision object transforms
131 Eigen::Isometry3d start_pos, end_pos;
132 start_pos = Eigen::Isometry3d::Identity();
133 end_pos = Eigen::Isometry3d::Identity();
134 start_pos.translation() = Eigen::Vector3d(0, -2.0, 0);
135 end_pos.translation() = Eigen::Vector3d(0, 2.0, 0);
136 checker.setCollisionObjectsTransform("octomap1_link", start_pos, end_pos);
137
138 // Perform collision check
139 ContactResultMap result;
141
144
145 EXPECT_TRUE(!result_vector.empty());
146 for (const auto& cr : result_vector)
147 {
148 EXPECT_NEAR(cr.distance, 0.20, 0.001);
149 }
150}
151} // namespace detail
152
153inline void runTest(ContinuousContactManager& checker)
154{
155 detail::addCollisionObjects<ContinuousContactManager>(checker);
156
157 // Call it again to test adding same object
158 detail::addCollisionObjects<ContinuousContactManager>(checker);
159
161
162 ContinuousContactManager::Ptr cloned_checker = checker.clone();
163 detail::runTestCompound(*cloned_checker);
164}
165
166inline void runTest(DiscreteContactManager& checker)
167{
168 detail::addCollisionObjects<DiscreteContactManager>(checker);
169
170 // Call it again to test adding same object
171 detail::addCollisionObjects<DiscreteContactManager>(checker);
172
174
175 DiscreteContactManager::Ptr cloned_checker = checker.clone();
176 detail::runTestCompound(*cloned_checker);
177}
178} // namespace tesseract_collision::test_suite
179#endif // TESSERACT_COLLISION_COLLISION_COMPOUND_COMPOUND_UNIT_HPP
This structure hold contact results for link pairs.
Definition: types.h:154
void flattenMoveResults(ContactResultVector &v)
Definition: types.cpp:215
void flattenCopyResults(ContactResultVector &v) const
Definition: types.cpp:227
Definition: continuous_contact_manager.h:41
virtual void setActiveCollisionObjects(const std::vector< std::string > &names)=0
Set which collision objects can move.
virtual ContinuousContactManager::UPtr clone() const =0
Clone the manager.
virtual void setCollisionObjectsTransform(const std::string &name, const Eigen::Isometry3d &pose)=0
Set a single static collision object's tansforms.
std::shared_ptr< ContinuousContactManager > Ptr
Definition: continuous_contact_manager.h:45
virtual void setPairCollisionMarginData(const std::string &name1, const std::string &name2, double collision_margin)=0
Set the margin for a given contact pair.
virtual void setCollisionMarginData(CollisionMarginData collision_margin_data, CollisionMarginOverrideType override_type=CollisionMarginOverrideType::REPLACE)=0
Set the contact distance thresholds for which collision should be considered on a per pair basis.
virtual const std::vector< std::string > & getActiveCollisionObjects() const =0
Get which collision objects can move.
virtual const CollisionMarginData & getCollisionMarginData() const =0
Get the contact distance threshold.
virtual IsContactAllowedFn getIsContactAllowedFn() const =0
Get the active function for determining if two links are allowed to be in collision.
virtual void contactTest(ContactResultMap &collisions, const ContactRequest &request)=0
Perform a contact test for all objects based.
Definition: discrete_contact_manager.h:41
std::shared_ptr< DiscreteContactManager > Ptr
Definition: discrete_contact_manager.h:47
virtual void setActiveCollisionObjects(const std::vector< std::string > &names)=0
Set which collision objects can move.
virtual void contactTest(ContactResultMap &collisions, const ContactRequest &request)=0
Perform a contact test for all objects based.
virtual void setCollisionObjectsTransform(const std::string &name, const Eigen::Isometry3d &pose)=0
Set a single collision object's transforms.
virtual const std::vector< std::string > & getActiveCollisionObjects() const =0
Get which collision objects can move.
virtual const CollisionMarginData & getCollisionMarginData() const =0
Get the contact distance threshold.
virtual void setCollisionMarginData(CollisionMarginData collision_margin_data, CollisionMarginOverrideType override_type=CollisionMarginOverrideType::REPLACE)=0
Set the contact distance thresholds for which collision should be considered on a per pair basis.
virtual IsContactAllowedFn getIsContactAllowedFn() const =0
Get the active function for determining if two links are allowed to be in collision.
double getMaxCollisionMargin() const
Get the largest collision margin.
Definition: collision_margin_data.h:165
@ BOX
Definition: octree.h:50
Definition: polygon_mesh.h:46
EXPECT_NEAR(config.contact_manager_config.margin_data.getDefaultCollisionMargin(), 5, 1e-6)
tesseract_collision::ContactResultVector result_vector
Definition: collision_core_unit.cpp:410
EXPECT_TRUE(tesseract_common::isIdentical< tesseract_collision::ObjectPairKey >(pairs, check_pairs, false))
This is the continuous contact manager base class.
This is the discrete contact manager base class.
Tesseract Geometries.
Common Tesseract Macros.
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
Definition: macros.h:71
Definition: create_convex_hull.cpp:36
void addCollisionObjects(ContinuousContactManager &checker)
Definition: collision_box_box_cast_unit.hpp:11
void runTestCompound(DiscreteContactManager &checker)
Definition: collision_compound_compound_unit.hpp:72
Definition: large_dataset_benchmarks.hpp:17
void runTest(ContinuousContactManager &checker)
Definition: collision_box_box_cast_unit.hpp:111
tesseract_common::CollisionMarginData CollisionMarginData
Definition: types.h:50
tesseract_geometry::Geometry::Ptr CollisionShapePtr
Definition: types.h:49
std::vector< tesseract_geometry::Geometry::ConstPtr > CollisionShapesConst
Definition: types.h:47
tesseract_common::AlignedVector< ContactResult > ContactResultVector
Definition: types.h:134
AlignedMap< std::string, Eigen::Isometry3d > TransformMap
Definition: types.h:66
template bool isIdentical< std::string >(const std::vector< std::string > &, const std::vector< std::string > &, bool, const std::function< bool(const std::string &, const std::string &)> &, const std::function< bool(const std::string &, const std::string &)> &)
AlignedVector< Eigen::Isometry3d > VectorIsometry3d
Definition: types.h:62
The ContactRequest struct.
Definition: types.h:294
std::vector< std::string > active_links
Definition: tesseract_environment_collision.cpp:112
tesseract_common::fs::path path
Definition: tesseract_srdf_unit.cpp:1992