geometry-algorithms.cpp
Go to the documentation of this file.
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/algorithm/kinematics.hpp"
12 #include "pinocchio/collision/collision.hpp"
13 #include "pinocchio/algorithm/geometry.hpp"
14 #include "pinocchio/parsers/urdf.hpp"
15 #include "pinocchio/parsers/srdf.hpp"
16 
17 #include <vector>
18 #include <boost/test/unit_test.hpp>
19 
20 using namespace pinocchio;
21 
22 typedef std::map<std::string, pinocchio::SE3> PositionsMap_t;
23 typedef std::map<std::string, pinocchio::SE3> JointPositionsMap_t;
24 typedef std::map<std::string, pinocchio::SE3> GeometryPositionsMap_t;
25 typedef std::map<std::pair<std::string, std::string>, fcl::DistanceResult> PairDistanceMap_t;
26 JointPositionsMap_t
27 fillPinocchioJointPositions(const pinocchio::Model & model, const pinocchio::Data & data);
28 GeometryPositionsMap_t fillPinocchioGeometryPositions(
29  const pinocchio::GeometryModel & geomModel, const pinocchio::GeometryData & geomData);
30 
31 std::vector<std::string> getBodiesList();
32 
33 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
34 
35 BOOST_AUTO_TEST_CASE(test_simple_boxes)
36 {
37  using namespace pinocchio;
38  Model model;
39  GeometryModel geomModel;
40 
41  Model::JointIndex idx;
42  idx = model.addJoint(
43  model.getJointId("universe"), JointModelPlanar(), SE3::Identity(), "planar1_joint");
44  model.addJointFrame(idx);
45  model.appendBodyToJoint(idx, Inertia::Random(), SE3::Identity());
46  model.addBodyFrame("planar1_body", idx, SE3::Identity());
47 
48  idx = model.addJoint(
49  model.getJointId("universe"), JointModelPlanar(), SE3::Identity(), "planar2_joint");
50  model.addJointFrame(idx);
51  model.appendBodyToJoint(idx, Inertia::Random(), SE3::Identity());
52  model.addBodyFrame("planar2_body", idx, SE3::Identity());
53 
54  std::shared_ptr<fcl::Box> sample(new fcl::Box(1, 1, 1));
55  Model::FrameIndex body_id_1 = model.getBodyId("planar1_body");
56  Model::JointIndex joint_parent_1 = model.frames[body_id_1].parentJoint;
57  Model::JointIndex idx_geom1 = geomModel.addGeometryObject(GeometryObject(
58  "ff1_collision_object", joint_parent_1, model.getBodyId("planar1_body"), SE3::Identity(),
59  sample, "", Eigen::Vector3d::Ones()));
60  geomModel.geometryObjects[idx_geom1].parentJoint = model.frames[body_id_1].parentJoint;
61 
62  std::shared_ptr<fcl::Box> sample2(new fcl::Box(1, 1, 1));
63  Model::FrameIndex body_id_2 = model.getBodyId("planar2_body");
64  Model::JointIndex joint_parent_2 = model.frames[body_id_2].parentJoint;
65  Model::JointIndex idx_geom2 = geomModel.addGeometryObject(
66  GeometryObject(
67  "ff2_collision_object", joint_parent_2, model.getBodyId("planar2_body"), SE3::Identity(),
68  sample2, "", Eigen::Vector3d::Ones()),
69  model);
70  BOOST_CHECK(
71  geomModel.geometryObjects[idx_geom2].parentJoint == model.frames[body_id_2].parentJoint);
72 
73  std::shared_ptr<fcl::Box> universe_body_geometry(new fcl::Box(1, 1, 1));
74  model.addBodyFrame("universe_body", 0, SE3::Identity());
75  Model::FrameIndex body_id_3 = model.getBodyId("universe_body");
76  Model::JointIndex joint_parent_3 = model.frames[body_id_3].parentJoint;
77  SE3 universe_body_placement = SE3::Random();
78  Model::JointIndex idx_geom3 = geomModel.addGeometryObject(
79  GeometryObject(
80  "universe_collision_object", joint_parent_3, model.getBodyId("universe_body"),
81  universe_body_placement, universe_body_geometry, "", Eigen::Vector3d::Ones()),
82  model);
83 
84  BOOST_CHECK(
85  geomModel.geometryObjects[idx_geom3].parentJoint == model.frames[body_id_3].parentJoint);
86 
87  geomModel.addAllCollisionPairs();
88  pinocchio::Data data(model);
89  pinocchio::GeometryData geomData(geomModel);
90 
91  BOOST_CHECK(CollisionPair(0, 1) == geomModel.collisionPairs[0]);
92 
93  std::cout << "------ Model ------ " << std::endl;
94  std::cout << model;
95  std::cout << "------ Geom ------ " << std::endl;
96  std::cout << geomModel;
97  std::cout << "------ DataGeom ------ " << std::endl;
98  std::cout << geomData;
99 
100  Eigen::VectorXd q(model.nq);
101  q << 0, 0, 1, 0, 0, 0, 1, 0;
102 
103  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
104  BOOST_CHECK(geomData.oMg[idx_geom3].isApprox(universe_body_placement));
105  BOOST_CHECK(computeCollision(geomModel, geomData, 0) == true);
106 
107  q << 2, 0, 1, 0, 0, 0, 1, 0;
108 
109  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
110  BOOST_CHECK(computeCollision(geomModel, geomData, 0) == false);
111 
112  q << 0.99, 0, 1, 0, 0, 0, 1, 0;
113 
114  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
115  BOOST_CHECK(computeCollision(geomModel, geomData, 0) == true);
116 
117  q << 1.01, 0, 1, 0, 0, 0, 1, 0;
118 
119  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
120  BOOST_CHECK(computeCollision(geomModel, geomData, 0) == false);
121 
122  geomModel.removeGeometryObject("ff2_collision_object");
123  geomData = pinocchio::GeometryData(geomModel);
124 
125  BOOST_CHECK(geomModel.ngeoms == 2);
126  BOOST_CHECK(geomModel.geometryObjects.size() == 2);
127  BOOST_CHECK(geomModel.collisionPairs.size() == 1);
128  BOOST_CHECK(
129  (geomModel.collisionPairs[0].first == 0 && geomModel.collisionPairs[0].second == 1)
130  || (geomModel.collisionPairs[0].first == 1 && geomModel.collisionPairs[0].second == 0));
131  BOOST_CHECK(geomData.activeCollisionPairs.size() == 1);
132  BOOST_CHECK(geomData.distanceRequests.size() == 1);
133  BOOST_CHECK(geomData.distanceResults.size() == 1);
134  BOOST_CHECK(geomData.distanceResults.size() == 1);
135  BOOST_CHECK(geomData.collisionResults.size() == 1);
136 }
137 
138 #if defined(PINOCCHIO_WITH_URDFDOM)
139 BOOST_AUTO_TEST_CASE(loading_model_and_check_distance)
140 {
141  typedef pinocchio::Model Model;
142  typedef pinocchio::GeometryModel GeometryModel;
143  typedef pinocchio::Data Data;
144  typedef pinocchio::GeometryData GeometryData;
145 
146  std::string filename =
147  PINOCCHIO_MODEL_DIR
148  + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
149  std::vector<std::string> packageDirs;
150  std::string meshDir = PINOCCHIO_MODEL_DIR;
151  packageDirs.push_back(meshDir);
152 
153  Model model;
154  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
155  GeometryModel geomModel;
156  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, packageDirs);
157  geomModel.addAllCollisionPairs();
158 
159  GeometryModel geomModelOther =
160  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geomModel, packageDirs);
161  BOOST_CHECK(geomModelOther == geomModel);
162 
163  Data data(model);
164  GeometryData geomData(geomModel);
165  fcl::CollisionResult result;
166 
167  Eigen::VectorXd q(model.nq);
168  q << 0, 0, 0.840252, 0, 0, 0, 1, 0, 0, -0.3490658, 0.6981317, -0.3490658, 0, 0, 0, -0.3490658,
169  0.6981317, -0.3490658, 0, 0, 1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2, 0, 0, 0, 0, 1.5, -0.6,
170  0.5, 1.05, -0.4, -0.3, -0.2;
171 
172  pinocchio::updateGeometryPlacements(model, data, geomModel, geomData, q);
173  pinocchio::Index idx = geomModel.findCollisionPair(CollisionPair(1, 10));
174  BOOST_CHECK(computeCollision(geomModel, geomData, idx) == false);
175 
176  fcl::DistanceResult distance_res = computeDistance(geomModel, geomData, idx);
177  BOOST_CHECK(distance_res.min_distance > 0.);
178 }
179 
180 BOOST_AUTO_TEST_CASE(test_collisions)
181 {
182  typedef pinocchio::Model Model;
183  typedef pinocchio::GeometryModel GeometryModel;
184  typedef pinocchio::Data Data;
185  typedef pinocchio::GeometryData GeometryData;
186 
187  const std::string filename =
188  PINOCCHIO_MODEL_DIR
189  + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
190  std::vector<std::string> packageDirs;
191  const std::string meshDir = PINOCCHIO_MODEL_DIR;
192  packageDirs.push_back(meshDir);
193  const std::string srdf_filename =
194  PINOCCHIO_MODEL_DIR
195  + std::string("/example-robot-data/robots/romeo_description/srdf/romeo.srdf");
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, packageDirs);
201  geom_model.addAllCollisionPairs();
202  pinocchio::srdf::removeCollisionPairs(model, geom_model, srdf_filename, false);
203 
204  Data data(model);
205  GeometryData geom_data(geom_model);
206 
207  pinocchio::srdf::loadReferenceConfigurations(model, srdf_filename, false);
208  Eigen::VectorXd q = model.referenceConfigurations["half_sitting"];
209 
210  pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, q);
211 
212  BOOST_CHECK(computeCollisions(geom_model, geom_data) == false);
213  BOOST_CHECK(computeCollisions(geom_model, geom_data, false) == false);
214 
215  for (size_t cp_index = 0; cp_index < geom_model.collisionPairs.size(); ++cp_index)
216  {
217  const CollisionPair & cp = geom_model.collisionPairs[cp_index];
218  const GeometryObject & obj1 = geom_model.geometryObjects[cp.first];
219  const GeometryObject & obj2 = geom_model.geometryObjects[cp.second];
220 
221  hpp::fcl::CollisionResult other_res;
222  computeCollision(geom_model, geom_data, cp_index);
223 
224  fcl::Transform3f oM1(toFclTransform3f(geom_data.oMg[cp.first])),
225  oM2(toFclTransform3f(geom_data.oMg[cp.second]));
226 
227  fcl::collide(
228  obj1.geometry.get(), oM1, obj2.geometry.get(), oM2, geom_data.collisionRequests[cp_index],
229  other_res);
230 
231  const hpp::fcl::CollisionResult & res = geom_data.collisionResults[cp_index];
232 
233  BOOST_CHECK(res.isCollision() == other_res.isCollision());
234  BOOST_CHECK(!res.isCollision());
235  }
236 
237  // test other signatures
238  {
239  Data data(model);
240  GeometryData geom_data(geom_model);
241  BOOST_CHECK(computeCollisions(model, data, geom_model, geom_data, q) == false);
242  }
243 }
244 
245 BOOST_AUTO_TEST_CASE(test_distances)
246 {
247  typedef pinocchio::Model Model;
248  typedef pinocchio::GeometryModel GeometryModel;
249  typedef pinocchio::Data Data;
250  typedef pinocchio::GeometryData GeometryData;
251 
252  const std::string filename =
253  PINOCCHIO_MODEL_DIR
254  + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
255  std::vector<std::string> packageDirs;
256  const std::string meshDir = PINOCCHIO_MODEL_DIR;
257  packageDirs.push_back(meshDir);
258  const std::string srdf_filename =
259  PINOCCHIO_MODEL_DIR
260  + std::string("/example-robot-data/robots/romeo_description/srdf/romeo.srdf");
261 
262  Model model;
263  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
264  GeometryModel geom_model;
265  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, packageDirs);
266  geom_model.addAllCollisionPairs();
267  pinocchio::srdf::removeCollisionPairs(model, geom_model, srdf_filename, false);
268 
269  Data data(model);
270  GeometryData geom_data(geom_model);
271 
272  pinocchio::srdf::loadReferenceConfigurations(model, srdf_filename, false);
273  Eigen::VectorXd q = model.referenceConfigurations["half_sitting"];
274 
275  pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, q);
276 
277  BOOST_CHECK(computeDistances(geom_model, geom_data) < geom_model.collisionPairs.size());
278 
279  {
280  Data data(model);
281  GeometryData geom_data(geom_model);
282  BOOST_CHECK(
283  computeDistances(model, data, geom_model, geom_data, q) < geom_model.collisionPairs.size());
284  }
285 }
286 
287 BOOST_AUTO_TEST_CASE(test_append_geom_models)
288 {
289  typedef pinocchio::Model Model;
290  typedef pinocchio::GeometryModel GeometryModel;
291 
292  const std::string filename =
293  PINOCCHIO_MODEL_DIR
294  + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
295  std::vector<std::string> packageDirs;
296  const std::string meshDir = PINOCCHIO_MODEL_DIR;
297  packageDirs.push_back(meshDir);
298 
299  Model model;
300  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
301  GeometryModel geom_model1;
302  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model1, packageDirs);
303 
304  GeometryModel geom_model2;
305  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model2, packageDirs);
306 
307  appendGeometryModel(geom_model2, geom_model1);
308  BOOST_CHECK(geom_model2.ngeoms == 2 * geom_model1.ngeoms);
309 
310  // Check that collision pairs between geoms on the same joint are discarded.
311  for (pinocchio::Index i = 0; i < geom_model2.collisionPairs.size(); ++i)
312  {
313  pinocchio::CollisionPair cp = geom_model2.collisionPairs[i];
314  BOOST_CHECK_NE(
315  geom_model2.geometryObjects[cp.first].parentJoint,
316  geom_model2.geometryObjects[cp.second].parentJoint);
317  }
318 
319  {
320  GeometryModel geom_model_empty;
321  GeometryModel geom_model;
322  BOOST_CHECK(geom_model_empty.ngeoms == 0);
323  appendGeometryModel(geom_model, geom_model_empty);
324  BOOST_CHECK(geom_model.ngeoms == 0);
325  }
326 }
327 
328 BOOST_AUTO_TEST_CASE(test_compute_body_radius)
329 {
330  std::vector<std::string> packageDirs;
331 
332  std::string filename =
333  PINOCCHIO_MODEL_DIR
334  + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
335  std::string meshDir = PINOCCHIO_MODEL_DIR;
336  packageDirs.push_back(meshDir);
337 
338  pinocchio::Model model;
339  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
340  pinocchio::GeometryModel geom;
341  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom, packageDirs);
342  Data data(model);
343  GeometryData geomData(geom);
344 
345  // Test that the algorithm does not crash
346  pinocchio::computeBodyRadius(model, geom, geomData);
347  BOOST_FOREACH (double radius, geomData.radius)
348  BOOST_CHECK(radius >= 0.);
349 }
350 #endif // if defined(PINOCCHIO_WITH_URDFDOM)
351 
352 BOOST_AUTO_TEST_SUITE_END()
getBodiesList
std::vector< std::string > getBodiesList()
PositionsMap_t
std::map< std::string, pinocchio::SE3 > PositionsMap_t
Definition: geometry-algorithms.cpp:22
pinocchio::appendGeometryModel
void appendGeometryModel(GeometryModel &geom_model1, const GeometryModel &geom_model2)
GeometryPositionsMap_t
std::map< std::string, pinocchio::SE3 > GeometryPositionsMap_t
Definition: geometry-algorithms.cpp:24
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::computeCollisions
bool computeCollisions(BroadPhaseManagerBase< BroadPhaseManagerDerived > &broadphase_manager, CollisionCallBackBase *callback)
Calls computeCollision for every active pairs of GeometryData. This function assumes that updateGeome...
Definition: broadphase.hpp:34
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::JointModelPlanarTpl
Definition: multibody/joint/fwd.hpp:118
fillPinocchioJointPositions
JointPositionsMap_t fillPinocchioJointPositions(const pinocchio::Model &model, const pinocchio::Data &data)
pinocchio::SE3Tpl< context::Scalar, context::Options >
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:20
kinematics.hpp
model.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::computeDistance
fcl::DistanceResult & computeDistance(const GeometryModel &geom_model, GeometryData &geom_data, const PairIndex pair_id)
Compute the minimal distance between collision objects of a SINGLE collison pair.
pinocchio::JointModelFreeFlyerTpl
Definition: multibody/joint/fwd.hpp:110
pinocchio::GeometryObject
Definition: multibody/geometry-object.hpp:87
pinocchio::res
ReturnType res
Definition: spatial/classic-acceleration.hpp:57
pinocchio::GeometryData
Definition: multibody/geometry.hpp:233
pinocchio::GeometryModel::addAllCollisionPairs
void addAllCollisionPairs()
Add all possible collision pairs.
collision.hpp
pinocchio::Index
PINOCCHIO_COMPILER_DIAGNOSTIC_POP typedef std::size_t Index
Definition: multibody/fwd.hpp:22
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:68
pinocchio::GeometryModel::geometryObjects
GeometryObjectVector geometryObjects
Vector of GeometryObjects used for collision computations.
Definition: multibody/geometry.hpp:217
pinocchio::toFclTransform3f
hpp::fcl::Transform3f toFclTransform3f(const SE3Tpl< Scalar > &m)
Definition: collision/fcl-pinocchio-conversions.hpp:14
geometry.hpp
pinocchio::CollisionPair
Definition: multibody/geometry.hpp:21
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_simple_boxes)
Definition: geometry-algorithms.cpp:35
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::Data
DataTpl< context::Scalar, context::Options > Data
Definition: multibody/fwd.hpp:34
filename
filename
urdf.hpp
srdf.hpp
data.hpp
pinocchio::ModelTpl::FrameIndex
pinocchio::FrameIndex FrameIndex
Definition: multibody/model.hpp:70
pinocchio::GeometryModel::findCollisionPair
PairIndex findCollisionPair(const CollisionPair &pair) const
Return the index of a given collision pair in collisionPairs.
geometry.hpp
pinocchio::computeCollision
bool computeCollision(const GeometryModel &geom_model, GeometryData &geom_data, const PairIndex pair_id, fcl::CollisionRequest &collision_request)
Compute the collision status between a SINGLE collision pair. The result is store in the collisionRes...
pinocchio::computeDistances
std::size_t computeDistances(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const GeometryModel &geom_model, GeometryData &geom_data)
pinocchio::InertiaTpl< context::Scalar, context::Options >::Random
static InertiaTpl Random()
Definition: spatial/inertia.hpp:356
pinocchio::GeometryObject::geometry
CollisionGeometryPtr geometry
The FCL CollisionGeometry (might be a Mesh, a Geometry Primitive, etc.)
Definition: multibody/geometry-object.hpp:110
hpp::fcl::CollisionResult
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 ...
hpp::fcl::CollisionResult::isCollision
bool isCollision() const
collisions.srdf_filename
string srdf_filename
Definition: collisions.py:28
pinocchio::GeometryModel::ngeoms
Index ngeoms
The number of GeometryObjects.
Definition: multibody/geometry.hpp:214
PairDistanceMap_t
std::map< std::pair< std::string, std::string >, fcl::DistanceResult > PairDistanceMap_t
Definition: geometry-algorithms.cpp:25
append-urdf-model-with-another-model.geom_model
geom_model
Definition: append-urdf-model-with-another-model.py:26
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1117
collisions.cp
cp
Definition: collisions.py:54
collisions.geom_data
geom_data
Definition: collisions.py:45
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.
pinocchio::SE3Tpl< context::Scalar, context::Options >::Random
static SE3Tpl Random()
Definition: spatial/se3-tpl.hpp:154
pinocchio::GeometryModel::collisionPairs
CollisionPairVector collisionPairs
Vector of collision pairs.
Definition: multibody/geometry.hpp:220
JointPositionsMap_t
std::map< std::string, pinocchio::SE3 > JointPositionsMap_t
Definition: geometry-algorithms.cpp:23
pinocchio::computeBodyRadius
void computeBodyRadius(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const GeometryModel &geom_model, GeometryData &geom_data)
fillPinocchioGeometryPositions
GeometryPositionsMap_t fillPinocchioGeometryPositions(const pinocchio::GeometryModel &geomModel, const pinocchio::GeometryData &geomData)
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::SE3Tpl< context::Scalar, context::Options >::Identity
static SE3Tpl Identity()
Definition: spatial/se3-tpl.hpp:136
pinocchio::GeometryModel
Definition: multibody/geometry.hpp:50
Box
Box()
pinocchio::COLLISION
@ COLLISION
Definition: multibody/geometry-object.hpp:27
pinocchio::ModelTpl
Definition: context/generic.hpp:20
pinocchio::Model
ModelTpl< context::Scalar, context::Options > Model
Definition: multibody/fwd.hpp:33
radius
FCL_REAL radius
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1116
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
BOOST_CHECK
#define BOOST_CHECK(check)
Definition: overview-urdf.cpp:34
PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR
Definition: build-reduced-model.cpp:11
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
Author(s):
autogenerated on Sat Jun 22 2024 02:41:46