Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2022 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include <iostream> |
6 |
|
|
|
7 |
|
|
#include "pinocchio/multibody/model.hpp" |
8 |
|
|
#include "pinocchio/multibody/data.hpp" |
9 |
|
|
|
10 |
|
|
#include "pinocchio/multibody/geometry.hpp" |
11 |
|
|
#include "pinocchio/parsers/urdf.hpp" |
12 |
|
|
|
13 |
|
|
#include <vector> |
14 |
|
|
#include <boost/test/unit_test.hpp> |
15 |
|
|
|
16 |
|
|
using namespace pinocchio; |
17 |
|
|
|
18 |
|
|
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
19 |
|
|
|
20 |
|
✗ |
BOOST_AUTO_TEST_CASE(manage_collision_pairs) |
21 |
|
|
{ |
22 |
|
|
std::string filename = |
23 |
|
|
PINOCCHIO_MODEL_DIR |
24 |
|
✗ |
+ std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf"); |
25 |
|
✗ |
std::vector<std::string> package_dirs; |
26 |
|
✗ |
std::string mesh_dir = PINOCCHIO_MODEL_DIR; |
27 |
|
✗ |
package_dirs.push_back(mesh_dir); |
28 |
|
|
|
29 |
|
✗ |
Model model; |
30 |
|
✗ |
pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model); |
31 |
|
✗ |
GeometryModel geom_model; |
32 |
|
✗ |
pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, package_dirs); |
33 |
|
✗ |
geom_model.addAllCollisionPairs(); |
34 |
|
|
|
35 |
|
✗ |
for (Eigen::DenseIndex i = 0; i < (Eigen::DenseIndex)geom_model.ngeoms; ++i) |
36 |
|
|
{ |
37 |
|
✗ |
for (Eigen::DenseIndex j = i + 1; j < (Eigen::DenseIndex)geom_model.ngeoms; ++j) |
38 |
|
|
{ |
39 |
|
✗ |
BOOST_CHECK(geom_model.collisionPairMapping(i, j) < (int)geom_model.collisionPairs.size()); |
40 |
|
✗ |
BOOST_CHECK(geom_model.collisionPairMapping(j, i) < (int)geom_model.collisionPairs.size()); |
41 |
|
✗ |
BOOST_CHECK(geom_model.collisionPairMapping(j, i) == geom_model.collisionPairMapping(i, j)); |
42 |
|
|
|
43 |
|
✗ |
if (geom_model.collisionPairMapping(i, j) != -1) |
44 |
|
|
{ |
45 |
|
✗ |
const PairIndex pair_index = (PairIndex)geom_model.collisionPairMapping(i, j); |
46 |
|
✗ |
const CollisionPair & cp_ref = geom_model.collisionPairs[pair_index]; |
47 |
|
✗ |
const CollisionPair cp((size_t)i, (size_t)j); |
48 |
|
✗ |
BOOST_CHECK(cp == cp_ref); |
49 |
|
|
} |
50 |
|
|
} |
51 |
|
|
} |
52 |
|
|
|
53 |
|
✗ |
GeometryModel::MatrixXb collision_map(GeometryModel::MatrixXb::Zero( |
54 |
|
✗ |
(Eigen::DenseIndex)geom_model.ngeoms, (Eigen::DenseIndex)geom_model.ngeoms)); |
55 |
|
|
|
56 |
|
✗ |
for (size_t k = 0; k < geom_model.collisionPairs.size(); ++k) |
57 |
|
|
{ |
58 |
|
✗ |
const CollisionPair & cp = geom_model.collisionPairs[k]; |
59 |
|
✗ |
collision_map((Eigen::DenseIndex)cp.first, (Eigen::DenseIndex)cp.second) = true; |
60 |
|
|
} |
61 |
|
✗ |
GeometryModel::MatrixXb collision_map_lower = collision_map.transpose(); |
62 |
|
|
|
63 |
|
✗ |
GeometryModel geom_model_copy, geom_model_copy_lower; |
64 |
|
✗ |
pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model_copy, package_dirs); |
65 |
|
✗ |
pinocchio::urdf::buildGeom( |
66 |
|
|
model, filename, pinocchio::COLLISION, geom_model_copy_lower, package_dirs); |
67 |
|
✗ |
geom_model_copy.setCollisionPairs(collision_map); |
68 |
|
✗ |
geom_model_copy_lower.setCollisionPairs(collision_map_lower, false); |
69 |
|
|
|
70 |
|
✗ |
BOOST_CHECK(geom_model_copy.collisionPairs.size() == geom_model.collisionPairs.size()); |
71 |
|
✗ |
BOOST_CHECK(geom_model_copy_lower.collisionPairs.size() == geom_model.collisionPairs.size()); |
72 |
|
✗ |
for (size_t k = 0; k < geom_model_copy.collisionPairs.size(); ++k) |
73 |
|
|
{ |
74 |
|
✗ |
BOOST_CHECK(geom_model.existCollisionPair(geom_model_copy.collisionPairs[k])); |
75 |
|
✗ |
BOOST_CHECK(geom_model.existCollisionPair(geom_model_copy_lower.collisionPairs[k])); |
76 |
|
|
} |
77 |
|
✗ |
for (size_t k = 0; k < geom_model.collisionPairs.size(); ++k) |
78 |
|
|
{ |
79 |
|
✗ |
BOOST_CHECK(geom_model_copy.existCollisionPair(geom_model.collisionPairs[k])); |
80 |
|
✗ |
BOOST_CHECK(geom_model_copy_lower.existCollisionPair(geom_model.collisionPairs[k])); |
81 |
|
|
} |
82 |
|
|
|
83 |
|
|
{ |
84 |
|
✗ |
GeometryData geom_data(geom_model); |
85 |
|
✗ |
geom_data.activateAllCollisionPairs(); |
86 |
|
|
|
87 |
|
✗ |
for (size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k) |
88 |
|
✗ |
BOOST_CHECK(geom_data.activeCollisionPairs[k]); |
89 |
|
|
} |
90 |
|
|
|
91 |
|
|
{ |
92 |
|
✗ |
GeometryData geom_data(geom_model); |
93 |
|
✗ |
geom_data.deactivateAllCollisionPairs(); |
94 |
|
|
|
95 |
|
✗ |
for (size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k) |
96 |
|
✗ |
BOOST_CHECK(!geom_data.activeCollisionPairs[k]); |
97 |
|
|
} |
98 |
|
|
|
99 |
|
|
{ |
100 |
|
✗ |
GeometryData geom_data(geom_model), geom_data_copy(geom_model), |
101 |
|
✗ |
geom_data_copy_lower(geom_model); |
102 |
|
✗ |
geom_data_copy.deactivateAllCollisionPairs(); |
103 |
|
✗ |
geom_data_copy_lower.deactivateAllCollisionPairs(); |
104 |
|
|
|
105 |
|
✗ |
GeometryData::MatrixXb collision_map(GeometryModel::MatrixXb::Zero( |
106 |
|
✗ |
(Eigen::DenseIndex)geom_model.ngeoms, (Eigen::DenseIndex)geom_model.ngeoms)); |
107 |
|
✗ |
for (size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k) |
108 |
|
|
{ |
109 |
|
✗ |
const CollisionPair & cp = geom_model.collisionPairs[k]; |
110 |
|
✗ |
collision_map((Eigen::DenseIndex)cp.first, (Eigen::DenseIndex)cp.second) = |
111 |
|
✗ |
geom_data.activeCollisionPairs[k]; |
112 |
|
|
} |
113 |
|
✗ |
GeometryData::MatrixXb collision_map_lower = collision_map.transpose(); |
114 |
|
|
|
115 |
|
✗ |
geom_data_copy.setActiveCollisionPairs(geom_model, collision_map); |
116 |
|
✗ |
BOOST_CHECK(geom_data_copy.activeCollisionPairs == geom_data.activeCollisionPairs); |
117 |
|
|
|
118 |
|
✗ |
geom_data_copy_lower.setActiveCollisionPairs(geom_model, collision_map_lower, false); |
119 |
|
✗ |
BOOST_CHECK(geom_data_copy_lower.activeCollisionPairs == geom_data.activeCollisionPairs); |
120 |
|
|
} |
121 |
|
|
|
122 |
|
|
// Test security margins |
123 |
|
|
{ |
124 |
|
✗ |
GeometryData geom_data_upper(geom_model), geom_data_lower(geom_model); |
125 |
|
|
|
126 |
|
✗ |
const GeometryData::MatrixXs security_margin_map(GeometryData::MatrixXs::Ones( |
127 |
|
✗ |
(Eigen::DenseIndex)geom_model.ngeoms, (Eigen::DenseIndex)geom_model.ngeoms)); |
128 |
|
✗ |
GeometryData::MatrixXs security_margin_map_upper(security_margin_map); |
129 |
|
✗ |
security_margin_map_upper.triangularView<Eigen::Lower>().fill(0.); |
130 |
|
|
|
131 |
|
✗ |
geom_data_upper.setSecurityMargins(geom_model, security_margin_map, true, true); |
132 |
|
✗ |
for (size_t k = 0; k < geom_data_upper.collisionRequests.size(); ++k) |
133 |
|
|
{ |
134 |
|
✗ |
BOOST_CHECK(geom_data_upper.collisionRequests[k].security_margin == 1.); |
135 |
|
✗ |
BOOST_CHECK( |
136 |
|
|
geom_data_upper.collisionRequests[k].security_margin |
137 |
|
|
== geom_data_upper.collisionRequests[k].distance_upper_bound); |
138 |
|
|
} |
139 |
|
|
|
140 |
|
✗ |
geom_data_lower.setSecurityMargins(geom_model, security_margin_map, false); |
141 |
|
✗ |
for (size_t k = 0; k < geom_data_lower.collisionRequests.size(); ++k) |
142 |
|
|
{ |
143 |
|
✗ |
BOOST_CHECK(geom_data_lower.collisionRequests[k].security_margin == 1.); |
144 |
|
|
} |
145 |
|
|
} |
146 |
|
|
|
147 |
|
|
// Test enableGeometryCollision |
148 |
|
|
{ |
149 |
|
✗ |
GeometryData geom_data(geom_model); |
150 |
|
✗ |
geom_data.deactivateAllCollisionPairs(); |
151 |
|
✗ |
geom_data.setGeometryCollisionStatus(geom_model, 0, true); |
152 |
|
|
|
153 |
|
✗ |
for (size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k) |
154 |
|
|
{ |
155 |
|
✗ |
const CollisionPair & cp = geom_model.collisionPairs[k]; |
156 |
|
✗ |
if (cp.first == 0 || cp.second == 0) |
157 |
|
|
{ |
158 |
|
✗ |
BOOST_CHECK(geom_data.activeCollisionPairs[k]); |
159 |
|
|
} |
160 |
|
|
else |
161 |
|
|
{ |
162 |
|
✗ |
BOOST_CHECK(!geom_data.activeCollisionPairs[k]); |
163 |
|
|
} |
164 |
|
|
} |
165 |
|
|
} |
166 |
|
|
|
167 |
|
|
// Test disableGeometryCollision |
168 |
|
|
{ |
169 |
|
✗ |
GeometryData geom_data(geom_model); |
170 |
|
✗ |
geom_data.activateAllCollisionPairs(); |
171 |
|
✗ |
geom_data.setGeometryCollisionStatus(geom_model, 0, false); |
172 |
|
|
|
173 |
|
✗ |
for (size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k) |
174 |
|
|
{ |
175 |
|
✗ |
const CollisionPair & cp = geom_model.collisionPairs[k]; |
176 |
|
✗ |
if (cp.first == 0 || cp.second == 0) |
177 |
|
|
{ |
178 |
|
✗ |
BOOST_CHECK(!geom_data.activeCollisionPairs[k]); |
179 |
|
|
} |
180 |
|
|
else |
181 |
|
|
{ |
182 |
|
✗ |
BOOST_CHECK(geom_data.activeCollisionPairs[k]); |
183 |
|
|
} |
184 |
|
|
} |
185 |
|
|
} |
186 |
|
|
} |
187 |
|
|
|
188 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_clone) |
189 |
|
|
{ |
190 |
|
|
std::string filename = |
191 |
|
|
PINOCCHIO_MODEL_DIR |
192 |
|
✗ |
+ std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf"); |
193 |
|
✗ |
std::vector<std::string> package_dirs; |
194 |
|
✗ |
std::string mesh_dir = PINOCCHIO_MODEL_DIR; |
195 |
|
✗ |
package_dirs.push_back(mesh_dir); |
196 |
|
|
|
197 |
|
✗ |
Model model; |
198 |
|
✗ |
pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model); |
199 |
|
✗ |
GeometryModel geom_model; |
200 |
|
✗ |
pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, package_dirs); |
201 |
|
✗ |
geom_model.addAllCollisionPairs(); |
202 |
|
|
|
203 |
|
✗ |
geom_model.geometryObjects[0].geometry = |
204 |
|
✗ |
GeometryObject::CollisionGeometryPtr(new ::hpp::fcl::Sphere(0.5)); |
205 |
|
✗ |
GeometryModel geom_model_clone = geom_model.clone(); |
206 |
|
✗ |
GeometryModel geom_model_copy = geom_model; |
207 |
|
|
|
208 |
|
✗ |
BOOST_CHECK(geom_model_clone == geom_model); |
209 |
|
✗ |
BOOST_CHECK(geom_model_copy == geom_model); |
210 |
|
|
|
211 |
|
✗ |
static_cast<::hpp::fcl::Sphere *>(geom_model.geometryObjects[0].geometry.get())->radius = 1.; |
212 |
|
✗ |
BOOST_CHECK(geom_model_clone != geom_model); |
213 |
|
✗ |
BOOST_CHECK(geom_model_copy == geom_model); |
214 |
|
|
} |
215 |
|
|
|
216 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
217 |
|
|
|