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  boost::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  boost::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  boost::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 
121 BOOST_AUTO_TEST_CASE ( loading_model )
122 {
123  typedef pinocchio::Model Model;
124  typedef pinocchio::GeometryModel GeometryModel;
125  typedef pinocchio::Data Data;
126  typedef pinocchio::GeometryData GeometryData;
127 
128  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
129  std::vector < std::string > packageDirs;
130  std::string meshDir = PINOCCHIO_MODEL_DIR;
131  packageDirs.push_back(meshDir);
132 
133  Model model;
134  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
135  GeometryModel geomModel;
136  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, packageDirs );
137  geomModel.addAllCollisionPairs();
138 
139  GeometryModel geomModelOther = pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, packageDirs );
140  BOOST_CHECK(geomModelOther == geomModel);
141 
142  Data data(model);
143  GeometryData geomData(geomModel);
144  fcl::CollisionResult result;
145 
146  Eigen::VectorXd q(model.nq);
147  q << 0, 0, 0.840252, 0, 0, 0, 1, 0, 0, -0.3490658, 0.6981317, -0.3490658, 0, 0, 0, -0.3490658,
148  0.6981317, -0.3490658, 0, 0, 1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2, 0, 0, 0, 0,
149  1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2 ;
150 
151  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
152  pinocchio::Index idx = geomModel.findCollisionPair(CollisionPair(1,10));
153  BOOST_CHECK(computeCollision(geomModel,geomData,idx) == false);
154 
155  fcl::DistanceResult distance_res = computeDistance(geomModel,geomData,idx);
156  BOOST_CHECK(distance_res.min_distance > 0.);
157 }
158 
159 BOOST_AUTO_TEST_CASE(manage_collision_pairs)
160 {
161  typedef pinocchio::Model Model;
162  typedef pinocchio::GeometryModel GeometryModel;
163  typedef pinocchio::GeometryData GeometryData;
164 
165  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
166  std::vector < std::string > package_dirs;
167  std::string mesh_dir = PINOCCHIO_MODEL_DIR;
168  package_dirs.push_back(mesh_dir);
169 
170  Model model;
171  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
172  GeometryModel geom_model;
173  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, package_dirs);
174  geom_model.addAllCollisionPairs();
175 
176  GeometryModel::MatrixXb collision_map(GeometryModel::MatrixXb::Zero((Eigen::DenseIndex)geom_model.ngeoms,(Eigen::DenseIndex)geom_model.ngeoms));
177 
178  for(size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
179  {
180  const CollisionPair & cp = geom_model.collisionPairs[k];
181  collision_map((Eigen::DenseIndex)cp.first,(Eigen::DenseIndex)cp.second) = true;
182  }
183  GeometryModel::MatrixXb collision_map_lower = collision_map.transpose();
184 
185  GeometryModel geom_model_copy, geom_model_copy_lower;
186  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model_copy, package_dirs);
187  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model_copy_lower, package_dirs);
188  geom_model_copy.setCollisionPairs(collision_map);
189  geom_model_copy_lower.setCollisionPairs(collision_map_lower,false);
190 
191  BOOST_CHECK(geom_model_copy.collisionPairs.size() == geom_model.collisionPairs.size());
192  BOOST_CHECK(geom_model_copy_lower.collisionPairs.size() == geom_model.collisionPairs.size());
193  for(size_t k = 0; k < geom_model_copy.collisionPairs.size(); ++k)
194  {
195  BOOST_CHECK(geom_model.existCollisionPair(geom_model_copy.collisionPairs[k]));
196  BOOST_CHECK(geom_model.existCollisionPair(geom_model_copy_lower.collisionPairs[k]));
197  }
198  for(size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
199  {
200  BOOST_CHECK(geom_model_copy.existCollisionPair(geom_model.collisionPairs[k]));
201  BOOST_CHECK(geom_model_copy_lower.existCollisionPair(geom_model.collisionPairs[k]));
202  }
203 
204  {
205  GeometryData geom_data(geom_model);
206  geom_data.activateAllCollisionPairs();
207 
208  for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
209  BOOST_CHECK(geom_data.activeCollisionPairs[k]);
210  }
211 
212  {
213  GeometryData geom_data(geom_model);
214  geom_data.deactivateAllCollisionPairs();
215 
216  for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
217  BOOST_CHECK(!geom_data.activeCollisionPairs[k]);
218  }
219 
220  {
221  GeometryData geom_data(geom_model), geom_data_copy(geom_model), geom_data_copy_lower(geom_model);
222  geom_data_copy.deactivateAllCollisionPairs();
223  geom_data_copy_lower.deactivateAllCollisionPairs();
224 
225  GeometryData::MatrixXb collision_map(GeometryModel::MatrixXb::Zero((Eigen::DenseIndex)geom_model.ngeoms,(Eigen::DenseIndex)geom_model.ngeoms));
226  for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
227  {
228  const CollisionPair & cp = geom_model.collisionPairs[k];
229  collision_map((Eigen::DenseIndex)cp.first,(Eigen::DenseIndex)cp.second) = geom_data.activeCollisionPairs[k];
230  }
231  GeometryData::MatrixXb collision_map_lower = collision_map.transpose();
232 
233  geom_data_copy.setActiveCollisionPairs(geom_model, collision_map);
234  BOOST_CHECK(geom_data_copy.activeCollisionPairs == geom_data.activeCollisionPairs);
235 
236  geom_data_copy_lower.setActiveCollisionPairs(geom_model, collision_map_lower, false);
237  BOOST_CHECK(geom_data_copy_lower.activeCollisionPairs == geom_data.activeCollisionPairs);
238  }
239 
240  // Test security margins
241  {
242  GeometryData geom_data_upper(geom_model), geom_data_lower(geom_model);
243 
244  const GeometryData::MatrixXs security_margin_map(GeometryData::MatrixXs::Ones((Eigen::DenseIndex)geom_model.ngeoms,(Eigen::DenseIndex)geom_model.ngeoms));
245  GeometryData::MatrixXs security_margin_map_upper(security_margin_map);
246  security_margin_map_upper.triangularView<Eigen::Lower>().fill(0.);
247 
248  geom_data_upper.setSecurityMargins(geom_model, security_margin_map);
249  for(size_t k = 0; k < geom_data_upper.collisionRequests.size(); ++k)
250  {
251  BOOST_CHECK(geom_data_upper.collisionRequests[k].security_margin == 1.);
252  }
253 
254  geom_data_lower.setSecurityMargins(geom_model, security_margin_map, false);
255  for(size_t k = 0; k < geom_data_lower.collisionRequests.size(); ++k)
256  {
257  BOOST_CHECK(geom_data_lower.collisionRequests[k].security_margin == 1.);
258  }
259  }
260 
261  // Test enableGeometryCollision
262  {
263  GeometryData geom_data(geom_model);
264  geom_data.deactivateAllCollisionPairs();
265  geom_data.setGeometryCollisionStatus(geom_model,0,true);
266 
267  for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
268  {
269  const CollisionPair & cp = geom_model.collisionPairs[k];
270  if(cp.first == 0 || cp.second == 0)
271  {
272  BOOST_CHECK(geom_data.activeCollisionPairs[k]);
273  }
274  else
275  {
276  BOOST_CHECK(!geom_data.activeCollisionPairs[k]);
277  }
278  }
279 
280  }
281 
282  // Test disableGeometryCollision
283  {
284  GeometryData geom_data(geom_model);
285  geom_data.activateAllCollisionPairs();
286  geom_data.setGeometryCollisionStatus(geom_model,0,false);
287 
288  for(size_t k = 0; k < geom_data.activeCollisionPairs.size(); ++k)
289  {
290  const CollisionPair & cp = geom_model.collisionPairs[k];
291  if(cp.first == 0 || cp.second == 0)
292  {
293  BOOST_CHECK(!geom_data.activeCollisionPairs[k]);
294  }
295  else
296  {
297  BOOST_CHECK(geom_data.activeCollisionPairs[k]);
298  }
299  }
300 
301  }
302 }
303 
304 BOOST_AUTO_TEST_CASE ( test_collisions )
305 {
306  typedef pinocchio::Model Model;
307  typedef pinocchio::GeometryModel GeometryModel;
308  typedef pinocchio::Data Data;
309  typedef pinocchio::GeometryData GeometryData;
310 
311  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
312  std::vector < std::string > packageDirs;
313  const std::string meshDir = PINOCCHIO_MODEL_DIR;
314  packageDirs.push_back(meshDir);
315  const std::string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/srdf/romeo.srdf");
316 
317  Model model;
318  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
319  GeometryModel geom_model;
320  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, packageDirs);
321  geom_model.addAllCollisionPairs();
322  pinocchio::srdf::removeCollisionPairs(model,geom_model,srdf_filename,false);
323 
324  Data data(model);
325  GeometryData geom_data(geom_model);
326 
327  pinocchio::srdf::loadReferenceConfigurations(model,srdf_filename,false);
328  Eigen::VectorXd q = model.referenceConfigurations["half_sitting"];
329 
330  pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, q);
331 
332  BOOST_CHECK(computeCollisions(geom_model,geom_data) == false);
333  BOOST_CHECK(computeCollisions(geom_model,geom_data,false) == false);
334 
335  for(size_t cp_index = 0; cp_index < geom_model.collisionPairs.size(); ++cp_index)
336  {
337  const CollisionPair & cp = geom_model.collisionPairs[cp_index];
338  const GeometryObject & obj1 = geom_model.geometryObjects[cp.first];
339  const GeometryObject & obj2 = geom_model.geometryObjects[cp.second];
340 
341  hpp::fcl::CollisionResult other_res;
342  computeCollision(geom_model,geom_data,cp_index);
343 
344  fcl::Transform3f oM1 (toFclTransform3f(geom_data.oMg[cp.first ])),
345  oM2 (toFclTransform3f(geom_data.oMg[cp.second]));
346 
347  fcl::collide(obj1.geometry.get(), oM1,
348  obj2.geometry.get(), oM2,
349  geom_data.collisionRequests[cp_index],
350  other_res);
351 
352  const hpp::fcl::CollisionResult & res = geom_data.collisionResults[cp_index];
353 
354  BOOST_CHECK(res.isCollision() == other_res.isCollision());
355  BOOST_CHECK(!res.isCollision());
356  }
357 
358  // test other signatures
359  {
360  Data data(model);
361  GeometryData geom_data(geom_model);
362  BOOST_CHECK(computeCollisions(model,data,geom_model,geom_data,q) == false);
363  }
364 }
365 
366 BOOST_AUTO_TEST_CASE ( test_distances )
367 {
368  typedef pinocchio::Model Model;
369  typedef pinocchio::GeometryModel GeometryModel;
370  typedef pinocchio::Data Data;
371  typedef pinocchio::GeometryData GeometryData;
372 
373  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
374  std::vector < std::string > packageDirs;
375  const std::string meshDir = PINOCCHIO_MODEL_DIR;
376  packageDirs.push_back(meshDir);
377  const std::string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/srdf/romeo.srdf");
378 
379  Model model;
380  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
381  GeometryModel geom_model;
382  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, packageDirs);
383  geom_model.addAllCollisionPairs();
384  pinocchio::srdf::removeCollisionPairs(model,geom_model,srdf_filename,false);
385 
386  Data data(model);
387  GeometryData geom_data(geom_model);
388 
389  pinocchio::srdf::loadReferenceConfigurations(model,srdf_filename,false);
390  Eigen::VectorXd q = model.referenceConfigurations["half_sitting"];
391 
392  pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, q);
393 
394  BOOST_CHECK(computeDistances(geom_model,geom_data) < geom_model.collisionPairs.size());
395 
396  {
397  Data data(model);
398  GeometryData geom_data(geom_model);
399  BOOST_CHECK(computeDistances(model,data,geom_model,geom_data,q) < geom_model.collisionPairs.size());
400  }
401 }
402 
403 BOOST_AUTO_TEST_CASE ( test_append_geom_models )
404 {
405  typedef pinocchio::Model Model;
406  typedef pinocchio::GeometryModel GeometryModel;
407 
408  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
409  std::vector < std::string > packageDirs;
410  const std::string meshDir = PINOCCHIO_MODEL_DIR;
411  packageDirs.push_back(meshDir);
412 
413  Model model;
414  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
415  GeometryModel geom_model1;
416  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model1, packageDirs);
417 
418  GeometryModel geom_model2;
419  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model2, packageDirs);
420 
421  appendGeometryModel(geom_model2,geom_model1);
422  BOOST_CHECK(geom_model2.ngeoms == 2*geom_model1.ngeoms);
423 
424  {
425  GeometryModel geom_model_empty;
426  GeometryModel geom_model;
427  BOOST_CHECK(geom_model_empty.ngeoms == 0);
428  appendGeometryModel(geom_model,geom_model_empty);
429  BOOST_CHECK(geom_model.ngeoms== 0);
430  }
431 }
432 
433 #if defined(PINOCCHIO_WITH_URDFDOM) && defined(PINOCCHIO_WITH_HPP_FCL)
434 BOOST_AUTO_TEST_CASE (radius)
435 {
436  std::vector < std::string > packageDirs;
437 
438  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
439  std::string meshDir = PINOCCHIO_MODEL_DIR;
440  packageDirs.push_back(meshDir);
441 
442  pinocchio::Model model;
443  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),model);
444  pinocchio::GeometryModel geom;
445  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom, packageDirs);
446  Data data(model);
447  GeometryData geomData(geom);
448 
449  // Test that the algorithm does not crash
450  pinocchio::computeBodyRadius(model, geom, geomData);
451  BOOST_FOREACH( double radius, geomData.radius) BOOST_CHECK(radius>=0.);
452 }
453 #endif // if defined(PINOCCHIO_WITH_URDFDOM) && defined(PINOCCHIO_WITH_HPP_FCL)
454 
455 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:188
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:174
pinocchio::appendGeometryModel
void appendGeometryModel(GeometryModel &geom_model1, const GeometryModel &geom_model2)
pinocchio::GeometryData::deactivateAllCollisionPairs
void deactivateAllCollisionPairs()
Deactivate all collision pairs.
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:208
data
data
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...
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::GeometryModel::findCollisionPair
PairIndex findCollisionPair(const CollisionPair &pair) const
Return the index of a given collision pair in collisionPairs.
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::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:171
JointPositionsMap_t
std::map< std::string, pinocchio::SE3 > JointPositionsMap_t
Definition: geom.cpp:22
pinocchio::GeometryModel::existCollisionPair
bool existCollisionPair(const CollisionPair &pair) const
Check if a collision pair exists in collisionPairs. See also findCollisitionPair(const CollisionPair ...
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: src/multibody/model.hpp:49
pinocchio::ModelTpl::getBodyId
FrameIndex getBodyId(const std::string &name) const
Return the index of a body given by its name.
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::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::GeometryModel::ngeoms
Index ngeoms
The number of GeometryObjects.
Definition: src/multibody/geometry.hpp:168
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...
pinocchio::toFclTransform3f
hpp::fcl::Transform3f toFclTransform3f(const SE3 &m)
Definition: fcl-pinocchio-conversions.hpp:13
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:30
res
res
pinocchio::ModelTpl::getJointId
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
pinocchio::Index
std::size_t Index
Definition: src/multibody/fwd.hpp:25
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:189
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
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.
fill
void fill(Eigen::Ref< MatType > mat, const typename MatType::Scalar &value)

pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:03