geom.cpp
Go to the documentation of this file.
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 BOOST_AUTO_TEST_CASE ( simple_boxes )
34 {
35  using namespace pinocchio;
36  Model model;
37  GeometryModel geomModel;
38 
39  Model::JointIndex idx;
40  idx = model.addJoint(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),"planar1_joint");
41  model.addJointFrame(idx);
42  model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
43  model.addBodyFrame("planar1_body", idx, SE3::Identity());
44 
45  idx = model.addJoint(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),"planar2_joint");
46  model.addJointFrame(idx);
47  model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
48  model.addBodyFrame("planar2_body", idx, SE3::Identity());
49 
50  shared_ptr<fcl::Box> sample(new fcl::Box(1, 1, 1));
51  Model::FrameIndex body_id_1 = model.getBodyId("planar1_body");
52  Model::JointIndex joint_parent_1 = model.frames[body_id_1].parent;
53  Model::JointIndex idx_geom1 = geomModel.addGeometryObject(GeometryObject("ff1_collision_object",
54  model.getBodyId("planar1_body"),joint_parent_1,
55  sample,SE3::Identity(), "", Eigen::Vector3d::Ones())
56  );
57  geomModel.geometryObjects[idx_geom1].parentJoint = model.frames[body_id_1].parent;
58 
59 
60  shared_ptr<fcl::Box> sample2(new fcl::Box(1, 1, 1));
61  Model::FrameIndex body_id_2 = model.getBodyId("planar2_body");
62  Model::JointIndex joint_parent_2 = model.frames[body_id_2].parent;
63  Model::JointIndex idx_geom2 = geomModel.addGeometryObject(GeometryObject("ff2_collision_object",
64  model.getBodyId("planar2_body"),joint_parent_2,
65  sample2,SE3::Identity(), "", Eigen::Vector3d::Ones()),
66  model);
67  BOOST_CHECK(geomModel.geometryObjects[idx_geom2].parentJoint == model.frames[body_id_2].parent);
68 
69  shared_ptr<fcl::Box> universe_body_geometry(new fcl::Box(1, 1, 1));
70  model.addBodyFrame("universe_body", 0, SE3::Identity());
71  Model::FrameIndex body_id_3 = model.getBodyId("universe_body");
72  Model::JointIndex joint_parent_3 = model.frames[body_id_3].parent;
73  SE3 universe_body_placement = SE3::Random();
74  Model::JointIndex idx_geom3 = geomModel.addGeometryObject(GeometryObject("universe_collision_object",
75  model.getBodyId("universe_body"),joint_parent_3,
76  universe_body_geometry,universe_body_placement, "", Eigen::Vector3d::Ones()),
77  model);
78 
79  BOOST_CHECK(geomModel.geometryObjects[idx_geom3].parentJoint == model.frames[body_id_3].parent);
80 
81  geomModel.addAllCollisionPairs();
82  pinocchio::Data data(model);
83  pinocchio::GeometryData geomData(geomModel);
84 
85  BOOST_CHECK(CollisionPair(0,1) == geomModel.collisionPairs[0]);
86 
87  std::cout << "------ Model ------ " << std::endl;
88  std::cout << model;
89  std::cout << "------ Geom ------ " << std::endl;
90  std::cout << geomModel;
91  std::cout << "------ DataGeom ------ " << std::endl;
92  std::cout << geomData;
93 
94  Eigen::VectorXd q(model.nq);
95  q << 0, 0, 1, 0,
96  0, 0, 1, 0 ;
97 
98  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
99  BOOST_CHECK(geomData.oMg[idx_geom3].isApprox(universe_body_placement));
100  BOOST_CHECK(computeCollision(geomModel,geomData,0) == true);
101 
102  q << 2, 0, 1, 0,
103  0, 0, 1, 0 ;
104 
105  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
106  BOOST_CHECK(computeCollision(geomModel,geomData,0) == false);
107 
108  q << 0.99, 0, 1, 0,
109  0, 0, 1, 0 ;
110 
111  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
112  BOOST_CHECK(computeCollision(geomModel,geomData,0) == true);
113 
114  q << 1.01, 0, 1, 0,
115  0, 0, 1, 0 ;
116 
117  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
118  BOOST_CHECK(computeCollision(geomModel,geomData,0) == false);
119 
120  geomModel.removeGeometryObject("ff2_collision_object");
121  geomData = pinocchio::GeometryData(geomModel);
122 
123  BOOST_CHECK(geomModel.ngeoms == 2);
124  BOOST_CHECK(geomModel.geometryObjects.size() == 2);
125  BOOST_CHECK(geomModel.collisionPairs.size() == 1);
126  BOOST_CHECK((geomModel.collisionPairs[0].first == 0 && geomModel.collisionPairs[0].second == 1) ||
127  (geomModel.collisionPairs[0].first == 1 && geomModel.collisionPairs[0].second == 0));
128  BOOST_CHECK(geomData.activeCollisionPairs.size() == 1);
129  BOOST_CHECK(geomData.distanceRequests.size() == 1);
130  BOOST_CHECK(geomData.distanceResults.size() == 1);
131  BOOST_CHECK(geomData.distanceResults.size() == 1);
132  BOOST_CHECK(geomData.collisionResults.size() == 1);
133 }
134 
135 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  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
143  std::vector < std::string > packageDirs;
144  std::string meshDir = PINOCCHIO_MODEL_DIR;
145  packageDirs.push_back(meshDir);
146 
147  Model model;
148  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
149  GeometryModel geomModel;
150  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, packageDirs );
151  geomModel.addAllCollisionPairs();
152 
153  GeometryModel geomModelOther = pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, packageDirs );
154  BOOST_CHECK(geomModelOther == geomModel);
155 
156  Data data(model);
157  GeometryData geomData(geomModel);
158  fcl::CollisionResult result;
159 
160  Eigen::VectorXd q(model.nq);
161  q << 0, 0, 0.840252, 0, 0, 0, 1, 0, 0, -0.3490658, 0.6981317, -0.3490658, 0, 0, 0, -0.3490658,
162  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  1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2 ;
164 
165  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
166  pinocchio::Index idx = geomModel.findCollisionPair(CollisionPair(1,10));
167  BOOST_CHECK(computeCollision(geomModel,geomData,idx) == false);
168 
169  fcl::DistanceResult distance_res = computeDistance(geomModel,geomData,idx);
170  BOOST_CHECK(distance_res.min_distance > 0.);
171 }
172 
173 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  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
180  std::vector < std::string > package_dirs;
181  std::string mesh_dir = PINOCCHIO_MODEL_DIR;
182  package_dirs.push_back(mesh_dir);
183 
184  Model model;
185  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
186  GeometryModel geom_model;
187  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, package_dirs);
188  geom_model.addAllCollisionPairs();
189 
190  GeometryModel::MatrixXb collision_map(GeometryModel::MatrixXb::Zero((Eigen::DenseIndex)geom_model.ngeoms,(Eigen::DenseIndex)geom_model.ngeoms));
191 
192  for(size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
193  {
194  const CollisionPair & cp = geom_model.collisionPairs[k];
195  collision_map((Eigen::DenseIndex)cp.first,(Eigen::DenseIndex)cp.second) = true;
196  }
197  GeometryModel::MatrixXb collision_map_lower = collision_map.transpose();
198 
199  GeometryModel geom_model_copy, geom_model_copy_lower;
200  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model_copy, package_dirs);
201  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model_copy_lower, package_dirs);
202  geom_model_copy.setCollisionPairs(collision_map);
203  geom_model_copy_lower.setCollisionPairs(collision_map_lower,false);
204 
205  BOOST_CHECK(geom_model_copy.collisionPairs.size() == geom_model.collisionPairs.size());
206  BOOST_CHECK(geom_model_copy_lower.collisionPairs.size() == geom_model.collisionPairs.size());
207  for(size_t k = 0; k < geom_model_copy.collisionPairs.size(); ++k)
208  {
209  BOOST_CHECK(geom_model.existCollisionPair(geom_model_copy.collisionPairs[k]));
210  BOOST_CHECK(geom_model.existCollisionPair(geom_model_copy_lower.collisionPairs[k]));
211  }
212  for(size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
213  {
214  BOOST_CHECK(geom_model_copy.existCollisionPair(geom_model.collisionPairs[k]));
215  BOOST_CHECK(geom_model_copy_lower.existCollisionPair(geom_model.collisionPairs[k]));
216  }
217 
218  {
219  GeometryData geom_data(geom_model);
220  geom_data.activateAllCollisionPairs();
221 
222  for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
223  BOOST_CHECK(geom_data.activeCollisionPairs[k]);
224  }
225 
226  {
227  GeometryData geom_data(geom_model);
228  geom_data.deactivateAllCollisionPairs();
229 
230  for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
231  BOOST_CHECK(!geom_data.activeCollisionPairs[k]);
232  }
233 
234  {
235  GeometryData geom_data(geom_model), geom_data_copy(geom_model), geom_data_copy_lower(geom_model);
236  geom_data_copy.deactivateAllCollisionPairs();
237  geom_data_copy_lower.deactivateAllCollisionPairs();
238 
239  GeometryData::MatrixXb collision_map(GeometryModel::MatrixXb::Zero((Eigen::DenseIndex)geom_model.ngeoms,(Eigen::DenseIndex)geom_model.ngeoms));
240  for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
241  {
242  const CollisionPair & cp = geom_model.collisionPairs[k];
243  collision_map((Eigen::DenseIndex)cp.first,(Eigen::DenseIndex)cp.second) = geom_data.activeCollisionPairs[k];
244  }
245  GeometryData::MatrixXb collision_map_lower = collision_map.transpose();
246 
247  geom_data_copy.setActiveCollisionPairs(geom_model, collision_map);
248  BOOST_CHECK(geom_data_copy.activeCollisionPairs == geom_data.activeCollisionPairs);
249 
250  geom_data_copy_lower.setActiveCollisionPairs(geom_model, collision_map_lower, false);
251  BOOST_CHECK(geom_data_copy_lower.activeCollisionPairs == geom_data.activeCollisionPairs);
252  }
253 
254  // Test security margins
255  {
256  GeometryData geom_data_upper(geom_model), geom_data_lower(geom_model);
257 
258  const GeometryData::MatrixXs security_margin_map(GeometryData::MatrixXs::Ones((Eigen::DenseIndex)geom_model.ngeoms,(Eigen::DenseIndex)geom_model.ngeoms));
259  GeometryData::MatrixXs security_margin_map_upper(security_margin_map);
260  security_margin_map_upper.triangularView<Eigen::Lower>().fill(0.);
261 
262  geom_data_upper.setSecurityMargins(geom_model, security_margin_map);
263  for(size_t k = 0; k < geom_data_upper.collisionRequests.size(); ++k)
264  {
265  BOOST_CHECK(geom_data_upper.collisionRequests[k].security_margin == 1.);
266  }
267 
268  geom_data_lower.setSecurityMargins(geom_model, security_margin_map, false);
269  for(size_t k = 0; k < geom_data_lower.collisionRequests.size(); ++k)
270  {
271  BOOST_CHECK(geom_data_lower.collisionRequests[k].security_margin == 1.);
272  }
273  }
274 
275  // Test enableGeometryCollision
276  {
277  GeometryData geom_data(geom_model);
278  geom_data.deactivateAllCollisionPairs();
279  geom_data.setGeometryCollisionStatus(geom_model,0,true);
280 
281  for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
282  {
283  const CollisionPair & cp = geom_model.collisionPairs[k];
284  if(cp.first == 0 || cp.second == 0)
285  {
286  BOOST_CHECK(geom_data.activeCollisionPairs[k]);
287  }
288  else
289  {
290  BOOST_CHECK(!geom_data.activeCollisionPairs[k]);
291  }
292  }
293 
294  }
295 
296  // Test disableGeometryCollision
297  {
298  GeometryData geom_data(geom_model);
299  geom_data.activateAllCollisionPairs();
300  geom_data.setGeometryCollisionStatus(geom_model,0,false);
301 
302  for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
303  {
304  const CollisionPair & cp = geom_model.collisionPairs[k];
305  if(cp.first == 0 || cp.second == 0)
306  {
307  BOOST_CHECK(!geom_data.activeCollisionPairs[k]);
308  }
309  else
310  {
311  BOOST_CHECK(geom_data.activeCollisionPairs[k]);
312  }
313  }
314 
315  }
316 }
317 
318 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  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
326  std::vector < std::string > packageDirs;
327  const std::string meshDir = PINOCCHIO_MODEL_DIR;
328  packageDirs.push_back(meshDir);
329  const std::string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/srdf/romeo.srdf");
330 
331  Model model;
332  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
333  GeometryModel geom_model;
334  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, packageDirs);
335  geom_model.addAllCollisionPairs();
336  pinocchio::srdf::removeCollisionPairs(model,geom_model,srdf_filename,false);
337 
338  Data data(model);
339  GeometryData geom_data(geom_model);
340 
341  pinocchio::srdf::loadReferenceConfigurations(model,srdf_filename,false);
342  Eigen::VectorXd q = model.referenceConfigurations["half_sitting"];
343 
344  pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, q);
345 
346  BOOST_CHECK(computeCollisions(geom_model,geom_data) == false);
347  BOOST_CHECK(computeCollisions(geom_model,geom_data,false) == false);
348 
349  for(size_t cp_index = 0; cp_index < geom_model.collisionPairs.size(); ++cp_index)
350  {
351  const CollisionPair & cp = geom_model.collisionPairs[cp_index];
352  const GeometryObject & obj1 = geom_model.geometryObjects[cp.first];
353  const GeometryObject & obj2 = geom_model.geometryObjects[cp.second];
354 
355  hpp::fcl::CollisionResult other_res;
356  computeCollision(geom_model,geom_data,cp_index);
357 
358  fcl::Transform3f oM1 (toFclTransform3f(geom_data.oMg[cp.first ])),
359  oM2 (toFclTransform3f(geom_data.oMg[cp.second]));
360 
361  fcl::collide(obj1.geometry.get(), oM1,
362  obj2.geometry.get(), oM2,
363  geom_data.collisionRequests[cp_index],
364  other_res);
365 
366  const hpp::fcl::CollisionResult & res = geom_data.collisionResults[cp_index];
367 
368  BOOST_CHECK(res.isCollision() == other_res.isCollision());
369  BOOST_CHECK(!res.isCollision());
370  }
371 
372  // test other signatures
373  {
374  Data data(model);
375  GeometryData geom_data(geom_model);
376  BOOST_CHECK(computeCollisions(model,data,geom_model,geom_data,q) == false);
377  }
378 }
379 
380 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  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
388  std::vector < std::string > packageDirs;
389  const std::string meshDir = PINOCCHIO_MODEL_DIR;
390  packageDirs.push_back(meshDir);
391  const std::string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/srdf/romeo.srdf");
392 
393  Model model;
394  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
395  GeometryModel geom_model;
396  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, packageDirs);
397  geom_model.addAllCollisionPairs();
398  pinocchio::srdf::removeCollisionPairs(model,geom_model,srdf_filename,false);
399 
400  Data data(model);
401  GeometryData geom_data(geom_model);
402 
403  pinocchio::srdf::loadReferenceConfigurations(model,srdf_filename,false);
404  Eigen::VectorXd q = model.referenceConfigurations["half_sitting"];
405 
406  pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, q);
407 
408  BOOST_CHECK(computeDistances(geom_model,geom_data) < geom_model.collisionPairs.size());
409 
410  {
411  Data data(model);
412  GeometryData geom_data(geom_model);
413  BOOST_CHECK(computeDistances(model,data,geom_model,geom_data,q) < geom_model.collisionPairs.size());
414  }
415 }
416 
417 BOOST_AUTO_TEST_CASE ( test_append_geom_models )
418 {
419  typedef pinocchio::Model Model;
420  typedef pinocchio::GeometryModel GeometryModel;
421 
422  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
423  std::vector < std::string > packageDirs;
424  const std::string meshDir = PINOCCHIO_MODEL_DIR;
425  packageDirs.push_back(meshDir);
426 
427  Model model;
428  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
429  GeometryModel geom_model1;
430  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model1, packageDirs);
431 
432  GeometryModel geom_model2;
433  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model2, packageDirs);
434 
435  appendGeometryModel(geom_model2,geom_model1);
436  BOOST_CHECK(geom_model2.ngeoms == 2*geom_model1.ngeoms);
437 
438  // Check that collision pairs between geoms on the same joint are discarded.
439  for (pinocchio::Index i = 0; i < geom_model2.collisionPairs.size(); ++i) {
440  pinocchio::CollisionPair cp = geom_model2.collisionPairs[i];
441  BOOST_CHECK_NE(
442  geom_model2.geometryObjects[cp.first].parentJoint,
443  geom_model2.geometryObjects[cp.second].parentJoint
444  );
445  }
446 
447  {
448  GeometryModel geom_model_empty;
449  GeometryModel geom_model;
450  BOOST_CHECK(geom_model_empty.ngeoms == 0);
451  appendGeometryModel(geom_model,geom_model_empty);
452  BOOST_CHECK(geom_model.ngeoms== 0);
453  }
454 }
455 
456 #if defined(PINOCCHIO_WITH_URDFDOM) && defined(PINOCCHIO_WITH_HPP_FCL)
457 BOOST_AUTO_TEST_CASE (radius)
458 {
459  std::vector < std::string > packageDirs;
460 
461  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
462  std::string meshDir = PINOCCHIO_MODEL_DIR;
463  packageDirs.push_back(meshDir);
464 
465  pinocchio::Model model;
466  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
467  pinocchio::GeometryModel geom;
468  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom, packageDirs);
469  Data data(model);
470  GeometryData geomData(geom);
471 
472  // Test that the algorithm does not crash
473  pinocchio::computeBodyRadius(model, geom, geomData);
474  BOOST_FOREACH( double radius, geomData.radius) BOOST_CHECK(radius>=0.);
475 }
476 #endif // if defined(PINOCCHIO_WITH_URDFDOM) && defined(PINOCCHIO_WITH_HPP_FCL)
477 
478 BOOST_AUTO_TEST_SUITE_END ()
pinocchio::srdf::loadReferenceConfigurations
void loadReferenceConfigurations(ModelTpl< Scalar, Options, JointCollectionTpl > &model, const std::string &filename, const bool verbose=false)
Get the reference configurations of a given model associated to a SRDF file. It throws if the SRDF fi...
pinocchio::GeometryModel::MatrixXb
Eigen::Matrix< bool, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXb
Definition: src/multibody/geometry.hpp:34
pinocchio::GeometryData::MatrixXb
Eigen::Matrix< bool, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXb
Definition: src/multibody/geometry.hpp:197
pinocchio::ModelTpl::addJointFrame
FrameIndex addJointFrame(const JointIndex &joint_index, int previous_frame_index=-1)
Add a joint to the frame tree.
pinocchio::Model
ModelTpl< double > Model
Definition: src/multibody/fwd.hpp:35
getBodiesList
std::vector< std::string > getBodiesList()
pinocchio::srdf::removeCollisionPairs
void removeCollisionPairs(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, GeometryModel &geom_model, const std::string &filename, const bool verbose=false)
Deactive all possible collision pairs mentioned in the SRDF file. It throws if the SRDF file is incor...
pinocchio::InertiaTpl::Random
static InertiaTpl Random()
Definition: src/spatial/inertia.hpp:233
pinocchio::GeometryModel::collisionPairs
CollisionPairVector collisionPairs
Vector of collision pairs.
Definition: src/multibody/geometry.hpp:183
pinocchio::appendGeometryModel
void appendGeometryModel(GeometryModel &geom_model1, const GeometryModel &geom_model2)
pinocchio::GeometryData::deactivateAllCollisionPairs
void deactivateAllCollisionPairs()
Deactivate all collision pairs.
pinocchio::GeometryModel::existCollisionPair
bool existCollisionPair(const CollisionPair &pair) const
Check if a collision pair exists in collisionPairs. See also findCollisitionPair(const CollisionPair ...
pinocchio::ModelTpl::frames
FrameVector frames
Vector of operational frames registered on the model.
Definition: src/multibody/model.hpp:145
pinocchio::GeometryData::activeCollisionPairs
std::vector< bool > activeCollisionPairs
Vector of collision pairs.
Definition: src/multibody/geometry.hpp:217
pinocchio::GeometryModel::findCollisionPair
PairIndex findCollisionPair(const CollisionPair &pair) const
Return the index of a given collision pair in collisionPairs.
pinocchio::GeometryData::setActiveCollisionPairs
void setActiveCollisionPairs(const GeometryModel &geom_model, const MatrixXb &collision_map, const bool upper=true)
Set the collision pair association from a given input array. Each entry of the input matrix defines t...
fill
fill
PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR
Definition: build-reduced-model.cpp:11
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:746
GeometryPositionsMap_t
std::map< std::string, pinocchio::SE3 > GeometryPositionsMap_t
Definition: geom.cpp:23
pinocchio::ModelTpl::referenceConfigurations
ConfigVectorMap referenceConfigurations
Map of reference configurations, indexed by user given names.
Definition: src/multibody/model.hpp:118
pinocchio::GeometryData::setGeometryCollisionStatus
void setGeometryCollisionStatus(const GeometryModel &geom_model, const GeomIndex geom_id, bool enable_collision)
Enable or disable collision for the given geometry given by its geometry id with all the other geomet...
pinocchio::computeCollisions
bool computeCollisions(const int num_threads, const GeometryModel &geom_model, GeometryData &geom_data, const bool stopAtFirstCollision=false)
Definition: src/algorithm/parallel/geometry.hpp:16
fillPinocchioJointPositions
JointPositionsMap_t fillPinocchioJointPositions(const pinocchio::Model &model, const pinocchio::Data &data)
pinocchio::ModelTpl::FrameIndex
pinocchio::FrameIndex FrameIndex
Definition: src/multibody/model.hpp:51
pinocchio::ModelTpl::addJoint
JointIndex addJoint(const JointIndex parent, const JointModel &joint_model, const SE3 &joint_placement, const std::string &joint_name)
Add a joint to the kinematic tree with infinite bounds.
pinocchio::urdf::buildGeom
GeometryModel & buildGeom(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const std::string &filename, const GeometryType type, GeometryModel &geom_model, const std::vector< std::string > &package_paths=std::vector< std::string >(), ::hpp::fcl::MeshLoaderPtr mesh_loader=::hpp::fcl::MeshLoaderPtr())
Build The GeometryModel from a URDF file. Search for meshes in the directories specified by the user ...
pinocchio::GeometryData::activateAllCollisionPairs
void activateAllCollisionPairs()
Activate all collision pairs.
pinocchio::GeometryModel::geometryObjects
GeometryObjectVector geometryObjects
Vector of GeometryObjects used for collision computations.
Definition: src/multibody/geometry.hpp:180
JointPositionsMap_t
std::map< std::string, pinocchio::SE3 > JointPositionsMap_t
Definition: geom.cpp:22
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: src/multibody/model.hpp:49
pinocchio::urdf::buildModel
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a URDF file with a particular joint as root of the model tree inside the model g...
pinocchio::GeometryModel::ngeoms
Index ngeoms
The number of GeometryObjects.
Definition: src/multibody/geometry.hpp:177
pinocchio::GeometryModel::addAllCollisionPairs
void addAllCollisionPairs()
Add all possible collision pairs.
pinocchio::Data
DataTpl< double > Data
Definition: src/multibody/fwd.hpp:36
collisions.srdf_filename
string srdf_filename
Definition: collisions.py:25
pinocchio::GeometryModel::setCollisionPairs
void setCollisionPairs(const MatrixXb &collision_map, const bool upper=true)
Set the collision pairs from a given input array. Each entry of the input matrix defines the activati...
radius
FCL_REAL radius
pinocchio::toFclTransform3f
hpp::fcl::Transform3f toFclTransform3f(const SE3 &m)
Definition: fcl-pinocchio-conversions.hpp:13
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:28
pinocchio::ModelTpl::getJointId
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
res
res
pinocchio::Index
std::size_t Index
Definition: src/multibody/fwd.hpp:25
filename
filename
pinocchio::ModelTpl::getBodyId
FrameIndex getBodyId(const std::string &name) const
Return the index of a body given by its name.
PositionsMap_t
std::map< std::string, pinocchio::SE3 > PositionsMap_t
Definition: geom.cpp:21
fillPinocchioGeometryPositions
GeometryPositionsMap_t fillPinocchioGeometryPositions(const pinocchio::GeometryModel &geomModel, const pinocchio::GeometryData &geomData)
pinocchio::updateGeometryPlacements
void updateGeometryPlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const GeometryModel &geom_model, GeometryData &geom_data, const Eigen::MatrixBase< ConfigVectorType > &q)
Apply a forward kinematics and update the placement of the geometry objects.
pinocchio::ModelTpl::appendBodyToJoint
void appendBodyToJoint(const JointIndex joint_index, const Inertia &Y, const SE3 &body_placement=SE3::Identity())
Append a body to a given joint of the kinematic tree.
pinocchio::python::VectorXd
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:746
pinocchio::GeometryData::MatrixXs
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXs
Definition: src/multibody/geometry.hpp:198
pinocchio::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition: src/multibody/model.hpp:76
pinocchio::JointModelPlanar
JointModelPlanarTpl< double > JointModelPlanar
Definition: src/multibody/joint/fwd.hpp:69
pinocchio::GeometryModel::addGeometryObject
GeomIndex addGeometryObject(const GeometryObject &object, const ModelTpl< S2, O2, _JointCollectionTpl > &model)
Add a geometry object to a GeometryModel and set its parent joint.
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(simple_boxes)
Definition: geom.cpp:33
PairDistanceMap_t
std::map< std::pair< std::string, std::string >, fcl::DistanceResult > PairDistanceMap_t
Definition: geom.cpp:24
Box
Box(FCL_REAL x, FCL_REAL y, FCL_REAL z)
pinocchio::ModelTpl::addBodyFrame
FrameIndex addBodyFrame(const std::string &body_name, const JointIndex &parentJoint, const SE3 &body_placement=SE3::Identity(), int previousFrame=-1)
Add a body to the frame tree.

pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:30