| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2021 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/algorithm/kinematics.hpp" |
||
12 |
#include "pinocchio/algorithm/geometry.hpp" |
||
13 |
#include "pinocchio/parsers/urdf.hpp" |
||
14 |
#include "pinocchio/parsers/srdf.hpp" |
||
15 |
|||
16 |
#include <vector> |
||
17 |
#include <boost/test/unit_test.hpp> |
||
18 |
|||
19 |
using namespace pinocchio; |
||
20 |
|||
21 |
typedef std::map <std::string, pinocchio::SE3> PositionsMap_t; |
||
22 |
typedef std::map <std::string, pinocchio::SE3> JointPositionsMap_t; |
||
23 |
typedef std::map <std::string, pinocchio::SE3> GeometryPositionsMap_t; |
||
24 |
typedef std::map <std::pair < std::string , std::string >, fcl::DistanceResult > PairDistanceMap_t; |
||
25 |
JointPositionsMap_t fillPinocchioJointPositions(const pinocchio::Model& model, const pinocchio::Data & data); |
||
26 |
GeometryPositionsMap_t fillPinocchioGeometryPositions(const pinocchio::GeometryModel & geomModel, |
||
27 |
const pinocchio::GeometryData & geomData); |
||
28 |
|||
29 |
std::vector<std::string> getBodiesList(); |
||
30 |
|||
31 |
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) |
||
32 |
|||
33 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( simple_boxes ) |
34 |
{ |
||
35 |
using namespace pinocchio; |
||
36 |
✓✗ | 4 |
Model model; |
37 |
✓✗ | 4 |
GeometryModel geomModel; |
38 |
|||
39 |
Model::JointIndex idx; |
||
40 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
idx = model.addJoint(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),"planar1_joint"); |
41 |
✓✗ | 2 |
model.addJointFrame(idx); |
42 |
✓✗✓✗ ✓✗ |
2 |
model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity()); |
43 |
✓✗✓✗ ✓✗ |
2 |
model.addBodyFrame("planar1_body", idx, SE3::Identity()); |
44 |
|||
45 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
idx = model.addJoint(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),"planar2_joint"); |
46 |
✓✗ | 2 |
model.addJointFrame(idx); |
47 |
✓✗✓✗ ✓✗ |
2 |
model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity()); |
48 |
✓✗✓✗ ✓✗ |
2 |
model.addBodyFrame("planar2_body", idx, SE3::Identity()); |
49 |
|||
50 |
✓✗✓✗ ✓✗ |
4 |
shared_ptr<fcl::Box> sample(new fcl::Box(1, 1, 1)); |
51 |
✓✗✓✗ |
2 |
Model::FrameIndex body_id_1 = model.getBodyId("planar1_body"); |
52 |
2 |
Model::JointIndex joint_parent_1 = model.frames[body_id_1].parent; |
|
53 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
10 |
Model::JointIndex idx_geom1 = geomModel.addGeometryObject(GeometryObject("ff1_collision_object", |
54 |
model.getBodyId("planar1_body"),joint_parent_1, |
||
55 |
✓✗✓✗ |
8 |
sample,SE3::Identity(), "", Eigen::Vector3d::Ones()) |
56 |
); |
||
57 |
2 |
geomModel.geometryObjects[idx_geom1].parentJoint = model.frames[body_id_1].parent; |
|
58 |
|||
59 |
|||
60 |
✓✗✓✗ ✓✗ |
4 |
shared_ptr<fcl::Box> sample2(new fcl::Box(1, 1, 1)); |
61 |
✓✗✓✗ |
2 |
Model::FrameIndex body_id_2 = model.getBodyId("planar2_body"); |
62 |
2 |
Model::JointIndex joint_parent_2 = model.frames[body_id_2].parent; |
|
63 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
10 |
Model::JointIndex idx_geom2 = geomModel.addGeometryObject(GeometryObject("ff2_collision_object", |
64 |
model.getBodyId("planar2_body"),joint_parent_2, |
||
65 |
✓✗✓✗ |
8 |
sample2,SE3::Identity(), "", Eigen::Vector3d::Ones()), |
66 |
model); |
||
67 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(geomModel.geometryObjects[idx_geom2].parentJoint == model.frames[body_id_2].parent); |
68 |
|||
69 |
✓✗✓✗ ✓✗ |
4 |
shared_ptr<fcl::Box> universe_body_geometry(new fcl::Box(1, 1, 1)); |
70 |
✓✗✓✗ ✓✗ |
2 |
model.addBodyFrame("universe_body", 0, SE3::Identity()); |
71 |
✓✗✓✗ |
2 |
Model::FrameIndex body_id_3 = model.getBodyId("universe_body"); |
72 |
2 |
Model::JointIndex joint_parent_3 = model.frames[body_id_3].parent; |
|
73 |
✓✗ | 2 |
SE3 universe_body_placement = SE3::Random(); |
74 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
6 |
Model::JointIndex idx_geom3 = geomModel.addGeometryObject(GeometryObject("universe_collision_object", |
75 |
model.getBodyId("universe_body"),joint_parent_3, |
||
76 |
✓✗ | 4 |
universe_body_geometry,universe_body_placement, "", Eigen::Vector3d::Ones()), |
77 |
model); |
||
78 |
|||
79 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(geomModel.geometryObjects[idx_geom3].parentJoint == model.frames[body_id_3].parent); |
80 |
|||
81 |
✓✗ | 2 |
geomModel.addAllCollisionPairs(); |
82 |
✓✗ | 4 |
pinocchio::Data data(model); |
83 |
✓✗ | 4 |
pinocchio::GeometryData geomData(geomModel); |
84 |
|||
85 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(CollisionPair(0,1) == geomModel.collisionPairs[0]); |
86 |
|||
87 |
✓✗✓✗ |
2 |
std::cout << "------ Model ------ " << std::endl; |
88 |
✓✗ | 2 |
std::cout << model; |
89 |
✓✗✓✗ |
2 |
std::cout << "------ Geom ------ " << std::endl; |
90 |
✓✗ | 2 |
std::cout << geomModel; |
91 |
✓✗✓✗ |
2 |
std::cout << "------ DataGeom ------ " << std::endl; |
92 |
✓✗ | 2 |
std::cout << geomData; |
93 |
|||
94 |
✓✗ | 4 |
Eigen::VectorXd q(model.nq); |
95 |
✓✗✓✗ ✓✗✓✗ |
2 |
q << 0, 0, 1, 0, |
96 |
✓✗✓✗ ✓✗✓✗ |
2 |
0, 0, 1, 0 ; |
97 |
|||
98 |
✓✗ | 2 |
pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q); |
99 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(geomData.oMg[idx_geom3].isApprox(universe_body_placement)); |
100 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(computeCollision(geomModel,geomData,0) == true); |
101 |
|||
102 |
✓✗✓✗ ✓✗✓✗ |
2 |
q << 2, 0, 1, 0, |
103 |
✓✗✓✗ ✓✗✓✗ |
2 |
0, 0, 1, 0 ; |
104 |
|||
105 |
✓✗ | 2 |
pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q); |
106 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(computeCollision(geomModel,geomData,0) == false); |
107 |
|||
108 |
✓✗✓✗ ✓✗✓✗ |
2 |
q << 0.99, 0, 1, 0, |
109 |
✓✗✓✗ ✓✗✓✗ |
2 |
0, 0, 1, 0 ; |
110 |
|||
111 |
✓✗ | 2 |
pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q); |
112 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(computeCollision(geomModel,geomData,0) == true); |
113 |
|||
114 |
✓✗✓✗ ✓✗✓✗ |
2 |
q << 1.01, 0, 1, 0, |
115 |
✓✗✓✗ ✓✗✓✗ |
2 |
0, 0, 1, 0 ; |
116 |
|||
117 |
✓✗ | 2 |
pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q); |
118 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(computeCollision(geomModel,geomData,0) == false); |
119 |
|||
120 |
✓✗✓✗ |
2 |
geomModel.removeGeometryObject("ff2_collision_object"); |
121 |
✓✗✓✗ |
2 |
geomData = pinocchio::GeometryData(geomModel); |
122 |
|||
123 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(geomModel.ngeoms == 2); |
124 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(geomModel.geometryObjects.size() == 2); |
125 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(geomModel.collisionPairs.size() == 1); |
126 |
✓✗✓✗ ✓✗✓✗ ✗✓✗✗ ✗✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK((geomModel.collisionPairs[0].first == 0 && geomModel.collisionPairs[0].second == 1) || |
127 |
(geomModel.collisionPairs[0].first == 1 && geomModel.collisionPairs[0].second == 0)); |
||
128 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(geomData.activeCollisionPairs.size() == 1); |
129 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(geomData.distanceRequests.size() == 1); |
130 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(geomData.distanceResults.size() == 1); |
131 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(geomData.distanceResults.size() == 1); |
132 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(geomData.collisionResults.size() == 1); |
133 |
2 |
} |
|
134 |
|||
135 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( loading_model ) |
136 |
{ |
||
137 |
typedef pinocchio::Model Model; |
||
138 |
typedef pinocchio::GeometryModel GeometryModel; |
||
139 |
typedef pinocchio::Data Data; |
||
140 |
typedef pinocchio::GeometryData GeometryData; |
||
141 |
|||
142 |
✓✗✓✗ |
6 |
std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf"); |
143 |
4 |
std::vector < std::string > packageDirs; |
|
144 |
✓✗ | 4 |
std::string meshDir = PINOCCHIO_MODEL_DIR; |
145 |
✓✗ | 2 |
packageDirs.push_back(meshDir); |
146 |
|||
147 |
✓✗ | 4 |
Model model; |
148 |
✓✗✓✗ ✓✗ |
2 |
pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model); |
149 |
✓✗ | 4 |
GeometryModel geomModel; |
150 |
✓✗ | 2 |
pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, packageDirs ); |
151 |
✓✗ | 2 |
geomModel.addAllCollisionPairs(); |
152 |
|||
153 |
✓✗✓✗ |
6 |
GeometryModel geomModelOther = pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, packageDirs ); |
154 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(geomModelOther == geomModel); |
155 |
|||
156 |
✓✗ | 4 |
Data data(model); |
157 |
✓✗ | 4 |
GeometryData geomData(geomModel); |
158 |
✓✗ | 4 |
fcl::CollisionResult result; |
159 |
|||
160 |
✓✗ | 4 |
Eigen::VectorXd q(model.nq); |
161 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
q << 0, 0, 0.840252, 0, 0, 0, 1, 0, 0, -0.3490658, 0.6981317, -0.3490658, 0, 0, 0, -0.3490658, |
162 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
0.6981317, -0.3490658, 0, 0, 1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2, 0, 0, 0, 0, |
163 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2 ; |
164 |
|||
165 |
✓✗ | 2 |
pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q); |
166 |
✓✗✓✗ |
2 |
pinocchio::Index idx = geomModel.findCollisionPair(CollisionPair(1,10)); |
167 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(computeCollision(geomModel,geomData,idx) == false); |
168 |
|||
169 |
✓✗✓✗ |
2 |
fcl::DistanceResult distance_res = computeDistance(geomModel,geomData,idx); |
170 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(distance_res.min_distance > 0.); |
171 |
2 |
} |
|
172 |
|||
173 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(manage_collision_pairs) |
174 |
{ |
||
175 |
typedef pinocchio::Model Model; |
||
176 |
typedef pinocchio::GeometryModel GeometryModel; |
||
177 |
typedef pinocchio::GeometryData GeometryData; |
||
178 |
|||
179 |
✓✗✓✗ |
6 |
std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf"); |
180 |
4 |
std::vector < std::string > package_dirs; |
|
181 |
✓✗ | 4 |
std::string mesh_dir = PINOCCHIO_MODEL_DIR; |
182 |
✓✗ | 2 |
package_dirs.push_back(mesh_dir); |
183 |
|||
184 |
✓✗ | 4 |
Model model; |
185 |
✓✗✓✗ ✓✗ |
2 |
pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model); |
186 |
✓✗ | 4 |
GeometryModel geom_model; |
187 |
✓✗ | 2 |
pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, package_dirs); |
188 |
✓✗ | 2 |
geom_model.addAllCollisionPairs(); |
189 |
|||
190 |
✓✗✓✗ |
4 |
GeometryModel::MatrixXb collision_map(GeometryModel::MatrixXb::Zero((Eigen::DenseIndex)geom_model.ngeoms,(Eigen::DenseIndex)geom_model.ngeoms)); |
191 |
|||
192 |
✓✓ | 382 |
for(size_t k = 0; k < geom_model.collisionPairs.size(); ++k) |
193 |
{ |
||
194 |
380 |
const CollisionPair & cp = geom_model.collisionPairs[k]; |
|
195 |
✓✗ | 380 |
collision_map((Eigen::DenseIndex)cp.first,(Eigen::DenseIndex)cp.second) = true; |
196 |
} |
||
197 |
✓✗✓✗ |
4 |
GeometryModel::MatrixXb collision_map_lower = collision_map.transpose(); |
198 |
|||
199 |
✓✗✓✗ |
4 |
GeometryModel geom_model_copy, geom_model_copy_lower; |
200 |
✓✗ | 2 |
pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model_copy, package_dirs); |
201 |
✓✗ | 2 |
pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model_copy_lower, package_dirs); |
202 |
✓✗ | 2 |
geom_model_copy.setCollisionPairs(collision_map); |
203 |
✓✗ | 2 |
geom_model_copy_lower.setCollisionPairs(collision_map_lower,false); |
204 |
|||
205 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(geom_model_copy.collisionPairs.size() == geom_model.collisionPairs.size()); |
206 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(geom_model_copy_lower.collisionPairs.size() == geom_model.collisionPairs.size()); |
207 |
✓✓ | 382 |
for(size_t k = 0; k < geom_model_copy.collisionPairs.size(); ++k) |
208 |
{ |
||
209 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
380 |
BOOST_CHECK(geom_model.existCollisionPair(geom_model_copy.collisionPairs[k])); |
210 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
380 |
BOOST_CHECK(geom_model.existCollisionPair(geom_model_copy_lower.collisionPairs[k])); |
211 |
} |
||
212 |
✓✓ | 382 |
for(size_t k = 0; k < geom_model.collisionPairs.size(); ++k) |
213 |
{ |
||
214 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
380 |
BOOST_CHECK(geom_model_copy.existCollisionPair(geom_model.collisionPairs[k])); |
215 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
380 |
BOOST_CHECK(geom_model_copy_lower.existCollisionPair(geom_model.collisionPairs[k])); |
216 |
} |
||
217 |
|||
218 |
{ |
||
219 |
✓✗ | 4 |
GeometryData geom_data(geom_model); |
220 |
2 |
geom_data.activateAllCollisionPairs(); |
|
221 |
|||
222 |
✓✓ | 382 |
for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k) |
223 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
380 |
BOOST_CHECK(geom_data.activeCollisionPairs[k]); |
224 |
} |
||
225 |
|||
226 |
{ |
||
227 |
✓✗ | 4 |
GeometryData geom_data(geom_model); |
228 |
2 |
geom_data.deactivateAllCollisionPairs(); |
|
229 |
|||
230 |
✓✓ | 382 |
for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k) |
231 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
380 |
BOOST_CHECK(!geom_data.activeCollisionPairs[k]); |
232 |
} |
||
233 |
|||
234 |
{ |
||
235 |
✓✗✓✗ ✓✗ |
4 |
GeometryData geom_data(geom_model), geom_data_copy(geom_model), geom_data_copy_lower(geom_model); |
236 |
2 |
geom_data_copy.deactivateAllCollisionPairs(); |
|
237 |
2 |
geom_data_copy_lower.deactivateAllCollisionPairs(); |
|
238 |
|||
239 |
✓✗✓✗ |
4 |
GeometryData::MatrixXb collision_map(GeometryModel::MatrixXb::Zero((Eigen::DenseIndex)geom_model.ngeoms,(Eigen::DenseIndex)geom_model.ngeoms)); |
240 |
✓✓ | 382 |
for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k) |
241 |
{ |
||
242 |
380 |
const CollisionPair & cp = geom_model.collisionPairs[k]; |
|
243 |
✓✗ | 380 |
collision_map((Eigen::DenseIndex)cp.first,(Eigen::DenseIndex)cp.second) = geom_data.activeCollisionPairs[k]; |
244 |
} |
||
245 |
✓✗✓✗ |
4 |
GeometryData::MatrixXb collision_map_lower = collision_map.transpose(); |
246 |
|||
247 |
✓✗ | 2 |
geom_data_copy.setActiveCollisionPairs(geom_model, collision_map); |
248 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(geom_data_copy.activeCollisionPairs == geom_data.activeCollisionPairs); |
249 |
|||
250 |
✓✗ | 2 |
geom_data_copy_lower.setActiveCollisionPairs(geom_model, collision_map_lower, false); |
251 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(geom_data_copy_lower.activeCollisionPairs == geom_data.activeCollisionPairs); |
252 |
} |
||
253 |
|||
254 |
// Test security margins |
||
255 |
{ |
||
256 |
✓✗✓✗ |
4 |
GeometryData geom_data_upper(geom_model), geom_data_lower(geom_model); |
257 |
|||
258 |
✓✗✓✗ |
4 |
const GeometryData::MatrixXs security_margin_map(GeometryData::MatrixXs::Ones((Eigen::DenseIndex)geom_model.ngeoms,(Eigen::DenseIndex)geom_model.ngeoms)); |
259 |
✓✗ | 4 |
GeometryData::MatrixXs security_margin_map_upper(security_margin_map); |
260 |
✓✗✓✗ |
2 |
security_margin_map_upper.triangularView<Eigen::Lower>().fill(0.); |
261 |
|||
262 |
✓✗ | 2 |
geom_data_upper.setSecurityMargins(geom_model, security_margin_map); |
263 |
✓✓ | 382 |
for(size_t k = 0; k < geom_data_upper.collisionRequests.size(); ++k) |
264 |
{ |
||
265 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
380 |
BOOST_CHECK(geom_data_upper.collisionRequests[k].security_margin == 1.); |
266 |
} |
||
267 |
|||
268 |
✓✗ | 2 |
geom_data_lower.setSecurityMargins(geom_model, security_margin_map, false); |
269 |
✓✓ | 382 |
for(size_t k = 0; k < geom_data_lower.collisionRequests.size(); ++k) |
270 |
{ |
||
271 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
380 |
BOOST_CHECK(geom_data_lower.collisionRequests[k].security_margin == 1.); |
272 |
} |
||
273 |
} |
||
274 |
|||
275 |
// Test enableGeometryCollision |
||
276 |
{ |
||
277 |
✓✗ | 4 |
GeometryData geom_data(geom_model); |
278 |
2 |
geom_data.deactivateAllCollisionPairs(); |
|
279 |
✓✗ | 2 |
geom_data.setGeometryCollisionStatus(geom_model,0,true); |
280 |
|||
281 |
✓✓ | 382 |
for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k) |
282 |
{ |
||
283 |
380 |
const CollisionPair & cp = geom_model.collisionPairs[k]; |
|
284 |
✓✓✗✓ |
380 |
if(cp.first == 0 || cp.second == 0) |
285 |
{ |
||
286 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
38 |
BOOST_CHECK(geom_data.activeCollisionPairs[k]); |
287 |
} |
||
288 |
else |
||
289 |
{ |
||
290 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
342 |
BOOST_CHECK(!geom_data.activeCollisionPairs[k]); |
291 |
} |
||
292 |
} |
||
293 |
|||
294 |
} |
||
295 |
|||
296 |
// Test disableGeometryCollision |
||
297 |
{ |
||
298 |
✓✗ | 4 |
GeometryData geom_data(geom_model); |
299 |
2 |
geom_data.activateAllCollisionPairs(); |
|
300 |
✓✗ | 2 |
geom_data.setGeometryCollisionStatus(geom_model,0,false); |
301 |
|||
302 |
✓✓ | 382 |
for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k) |
303 |
{ |
||
304 |
380 |
const CollisionPair & cp = geom_model.collisionPairs[k]; |
|
305 |
✓✓✗✓ |
380 |
if(cp.first == 0 || cp.second == 0) |
306 |
{ |
||
307 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
38 |
BOOST_CHECK(!geom_data.activeCollisionPairs[k]); |
308 |
} |
||
309 |
else |
||
310 |
{ |
||
311 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
342 |
BOOST_CHECK(geom_data.activeCollisionPairs[k]); |
312 |
} |
||
313 |
} |
||
314 |
|||
315 |
} |
||
316 |
2 |
} |
|
317 |
|||
318 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_collisions ) |
319 |
{ |
||
320 |
typedef pinocchio::Model Model; |
||
321 |
typedef pinocchio::GeometryModel GeometryModel; |
||
322 |
typedef pinocchio::Data Data; |
||
323 |
typedef pinocchio::GeometryData GeometryData; |
||
324 |
|||
325 |
✓✗✓✗ |
6 |
const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf"); |
326 |
4 |
std::vector < std::string > packageDirs; |
|
327 |
✓✗ | 4 |
const std::string meshDir = PINOCCHIO_MODEL_DIR; |
328 |
✓✗ | 2 |
packageDirs.push_back(meshDir); |
329 |
✓✗✓✗ |
6 |
const std::string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/srdf/romeo.srdf"); |
330 |
|||
331 |
✓✗ | 4 |
Model model; |
332 |
✓✗✓✗ ✓✗ |
2 |
pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model); |
333 |
✓✗ | 4 |
GeometryModel geom_model; |
334 |
✓✗ | 2 |
pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, packageDirs); |
335 |
✓✗ | 2 |
geom_model.addAllCollisionPairs(); |
336 |
✓✗ | 2 |
pinocchio::srdf::removeCollisionPairs(model,geom_model,srdf_filename,false); |
337 |
|||
338 |
✓✗ | 4 |
Data data(model); |
339 |
✓✗ | 4 |
GeometryData geom_data(geom_model); |
340 |
|||
341 |
✓✗ | 2 |
pinocchio::srdf::loadReferenceConfigurations(model,srdf_filename,false); |
342 |
✓✗✓✗ ✓✗ |
6 |
Eigen::VectorXd q = model.referenceConfigurations["half_sitting"]; |
343 |
|||
344 |
✓✗ | 2 |
pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, q); |
345 |
|||
346 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(computeCollisions(geom_model,geom_data) == false); |
347 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(computeCollisions(geom_model,geom_data,false) == false); |
348 |
|||
349 |
✓✓ | 142 |
for(size_t cp_index = 0; cp_index < geom_model.collisionPairs.size(); ++cp_index) |
350 |
{ |
||
351 |
140 |
const CollisionPair & cp = geom_model.collisionPairs[cp_index]; |
|
352 |
140 |
const GeometryObject & obj1 = geom_model.geometryObjects[cp.first]; |
|
353 |
140 |
const GeometryObject & obj2 = geom_model.geometryObjects[cp.second]; |
|
354 |
|||
355 |
✓✗ | 280 |
hpp::fcl::CollisionResult other_res; |
356 |
✓✗ | 140 |
computeCollision(geom_model,geom_data,cp_index); |
357 |
|||
358 |
✓✗ | 140 |
fcl::Transform3f oM1 (toFclTransform3f(geom_data.oMg[cp.first ])), |
359 |
✓✗ | 140 |
oM2 (toFclTransform3f(geom_data.oMg[cp.second])); |
360 |
|||
361 |
✓✗ | 140 |
fcl::collide(obj1.geometry.get(), oM1, |
362 |
140 |
obj2.geometry.get(), oM2, |
|
363 |
140 |
geom_data.collisionRequests[cp_index], |
|
364 |
other_res); |
||
365 |
|||
366 |
140 |
const hpp::fcl::CollisionResult & res = geom_data.collisionResults[cp_index]; |
|
367 |
|||
368 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
140 |
BOOST_CHECK(res.isCollision() == other_res.isCollision()); |
369 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
140 |
BOOST_CHECK(!res.isCollision()); |
370 |
} |
||
371 |
|||
372 |
// test other signatures |
||
373 |
{ |
||
374 |
✓✗ | 4 |
Data data(model); |
375 |
✓✗ | 4 |
GeometryData geom_data(geom_model); |
376 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(computeCollisions(model,data,geom_model,geom_data,q) == false); |
377 |
} |
||
378 |
2 |
} |
|
379 |
|||
380 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_distances ) |
381 |
{ |
||
382 |
typedef pinocchio::Model Model; |
||
383 |
typedef pinocchio::GeometryModel GeometryModel; |
||
384 |
typedef pinocchio::Data Data; |
||
385 |
typedef pinocchio::GeometryData GeometryData; |
||
386 |
|||
387 |
✓✗✓✗ |
6 |
const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf"); |
388 |
4 |
std::vector < std::string > packageDirs; |
|
389 |
✓✗ | 4 |
const std::string meshDir = PINOCCHIO_MODEL_DIR; |
390 |
✓✗ | 2 |
packageDirs.push_back(meshDir); |
391 |
✓✗✓✗ |
6 |
const std::string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/srdf/romeo.srdf"); |
392 |
|||
393 |
✓✗ | 4 |
Model model; |
394 |
✓✗✓✗ ✓✗ |
2 |
pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model); |
395 |
✓✗ | 4 |
GeometryModel geom_model; |
396 |
✓✗ | 2 |
pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, packageDirs); |
397 |
✓✗ | 2 |
geom_model.addAllCollisionPairs(); |
398 |
✓✗ | 2 |
pinocchio::srdf::removeCollisionPairs(model,geom_model,srdf_filename,false); |
399 |
|||
400 |
✓✗ | 4 |
Data data(model); |
401 |
✓✗ | 4 |
GeometryData geom_data(geom_model); |
402 |
|||
403 |
✓✗ | 2 |
pinocchio::srdf::loadReferenceConfigurations(model,srdf_filename,false); |
404 |
✓✗✓✗ ✓✗ |
6 |
Eigen::VectorXd q = model.referenceConfigurations["half_sitting"]; |
405 |
|||
406 |
✓✗ | 2 |
pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, q); |
407 |
|||
408 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(computeDistances(geom_model,geom_data) < geom_model.collisionPairs.size()); |
409 |
|||
410 |
{ |
||
411 |
✓✗ | 4 |
Data data(model); |
412 |
✓✗ | 4 |
GeometryData geom_data(geom_model); |
413 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(computeDistances(model,data,geom_model,geom_data,q) < geom_model.collisionPairs.size()); |
414 |
} |
||
415 |
2 |
} |
|
416 |
|||
417 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_append_geom_models ) |
418 |
{ |
||
419 |
typedef pinocchio::Model Model; |
||
420 |
typedef pinocchio::GeometryModel GeometryModel; |
||
421 |
|||
422 |
✓✗✓✗ |
6 |
const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf"); |
423 |
4 |
std::vector < std::string > packageDirs; |
|
424 |
✓✗ | 4 |
const std::string meshDir = PINOCCHIO_MODEL_DIR; |
425 |
✓✗ | 2 |
packageDirs.push_back(meshDir); |
426 |
|||
427 |
✓✗ | 4 |
Model model; |
428 |
✓✗✓✗ ✓✗ |
2 |
pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model); |
429 |
✓✗ | 4 |
GeometryModel geom_model1; |
430 |
✓✗ | 2 |
pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model1, packageDirs); |
431 |
|||
432 |
✓✗ | 4 |
GeometryModel geom_model2; |
433 |
✓✗ | 2 |
pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model2, packageDirs); |
434 |
|||
435 |
✓✗ | 2 |
appendGeometryModel(geom_model2,geom_model1); |
436 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(geom_model2.ngeoms == 2*geom_model1.ngeoms); |
437 |
|||
438 |
// Check that collision pairs between geoms on the same joint are discarded. |
||
439 |
✓✓ | 762 |
for (pinocchio::Index i = 0; i < geom_model2.collisionPairs.size(); ++i) { |
440 |
760 |
pinocchio::CollisionPair cp = geom_model2.collisionPairs[i]; |
|
441 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
760 |
BOOST_CHECK_NE( |
442 |
geom_model2.geometryObjects[cp.first].parentJoint, |
||
443 |
geom_model2.geometryObjects[cp.second].parentJoint |
||
444 |
); |
||
445 |
} |
||
446 |
|||
447 |
{ |
||
448 |
✓✗ | 4 |
GeometryModel geom_model_empty; |
449 |
✓✗ | 4 |
GeometryModel geom_model; |
450 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(geom_model_empty.ngeoms == 0); |
451 |
✓✗ | 2 |
appendGeometryModel(geom_model,geom_model_empty); |
452 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(geom_model.ngeoms== 0); |
453 |
} |
||
454 |
2 |
} |
|
455 |
|||
456 |
#if defined(PINOCCHIO_WITH_URDFDOM) && defined(PINOCCHIO_WITH_HPP_FCL) |
||
457 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE (radius) |
458 |
{ |
||
459 |
4 |
std::vector < std::string > packageDirs; |
|
460 |
|||
461 |
✓✗✓✗ |
6 |
std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf"); |
462 |
✓✗ | 4 |
std::string meshDir = PINOCCHIO_MODEL_DIR; |
463 |
✓✗ | 2 |
packageDirs.push_back(meshDir); |
464 |
|||
465 |
✓✗ | 4 |
pinocchio::Model model; |
466 |
✓✗✓✗ ✓✗ |
2 |
pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model); |
467 |
✓✗ | 4 |
pinocchio::GeometryModel geom; |
468 |
✓✗ | 2 |
pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom, packageDirs); |
469 |
✓✗ | 4 |
Data data(model); |
470 |
✓✗ | 4 |
GeometryData geomData(geom); |
471 |
|||
472 |
// Test that the algorithm does not crash |
||
473 |
✓✗ | 2 |
pinocchio::computeBodyRadius(model, geom, geomData); |
474 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✓✓ ✓✗✓✗ ✓✓✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗ |
134 |
BOOST_FOREACH( double radius, geomData.radius) BOOST_CHECK(radius>=0.); |
475 |
2 |
} |
|
476 |
#endif // if defined(PINOCCHIO_WITH_URDFDOM) && defined(PINOCCHIO_WITH_HPP_FCL) |
||
477 |
|||
478 |
BOOST_AUTO_TEST_SUITE_END () |
| Generated by: GCOVR (Version 4.2) |