unittest/broadphase.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2022 INRIA
3 //
4 
5 #include <iostream>
6 
7 #include "pinocchio/algorithm/geometry.hpp"
8 #include "pinocchio/parsers/urdf.hpp"
9 #include "pinocchio/parsers/srdf.hpp"
10 
11 #include "pinocchio/collision/collision.hpp"
12 #include "pinocchio/collision/distance.hpp"
13 #include "pinocchio/collision/broadphase-manager.hpp"
14 #include "pinocchio/collision/broadphase.hpp"
15 
16 #include <hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h>
17 
18 #include <vector>
19 #include <boost/test/unit_test.hpp>
20 
21 using namespace pinocchio;
22 
23 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
24 
25 BOOST_AUTO_TEST_CASE(test_broadphase_with_empty_models)
26 {
27  Model model;
28  GeometryModel geom_model;
29  GeometryData geom_data(geom_model);
30 
31  BroadPhaseManagerTpl<hpp::fcl::DynamicAABBTreeCollisionManager> broadphase_manager(
32  &model, &geom_model, &geom_data);
33 
34  BOOST_CHECK(broadphase_manager.check());
35 }
36 
37 BOOST_AUTO_TEST_CASE(test_broadphase)
38 {
39  Model model;
40  Data data(model);
41  GeometryModel geom_model;
42 
43  hpp::fcl::CollisionGeometryPtr_t sphere_ptr(new hpp::fcl::Sphere(0.5));
44  sphere_ptr->computeLocalAABB();
45  hpp::fcl::CollisionGeometryPtr_t box_ptr(new hpp::fcl::Box(0.5, 0.5, 0.5));
46  box_ptr->computeLocalAABB();
47 
48  GeometryObject obj1("obj1", 0, SE3::Identity(), sphere_ptr);
49  const GeomIndex obj1_index = geom_model.addGeometryObject(obj1);
50 
51  GeometryObject obj2("obj2", 0, SE3::Identity(), box_ptr);
52  const GeomIndex obj2_index = geom_model.addGeometryObject(obj2);
53 
54  GeometryObject & go = geom_model.geometryObjects[obj1_index];
55 
56  GeometryData geom_data(geom_model);
57  updateGeometryPlacements(model, data, geom_model, geom_data);
58 
59  BroadPhaseManagerTpl<hpp::fcl::DynamicAABBTreeCollisionManager> broadphase_manager(
60  &model, &geom_model, &geom_data);
61  BOOST_CHECK(broadphase_manager.check());
62  BOOST_CHECK(sphere_ptr.get() == go.geometry.get());
63 
64  hpp::fcl::CollisionGeometryPtr_t sphere_new_ptr(new hpp::fcl::Sphere(5.));
65  sphere_new_ptr->computeLocalAABB();
66  go.geometry = sphere_new_ptr;
67  BOOST_CHECK(!broadphase_manager.check());
68  BOOST_CHECK(sphere_ptr.get() != go.geometry.get());
69  BOOST_CHECK(
70  broadphase_manager.getCollisionObjects()[obj1_index].collisionGeometry().get()
71  == sphere_ptr.get());
72  BOOST_CHECK(
73  broadphase_manager.getCollisionObjects()[obj2_index].collisionGeometry().get()
74  == box_ptr.get());
75  // BOOST_CHECK(broadphase_manager.getObjects()[obj1_index]->collisionGeometry().get() ==
76  // sphere_ptr.get());
77  BOOST_CHECK(
78  broadphase_manager.getCollisionObjects()[obj1_index].collisionGeometry().get()
79  != go.geometry.get());
80  BOOST_CHECK(sphere_new_ptr.get() == go.geometry.get());
81 
82  broadphase_manager.update(false);
83  BOOST_CHECK(
84  broadphase_manager.getCollisionObjects()[obj1_index].collisionGeometry().get()
85  != sphere_ptr.get());
86  BOOST_CHECK(
87  broadphase_manager.getCollisionObjects()[obj1_index].collisionGeometry().get()
88  == go.geometry.get());
89  // BOOST_CHECK(broadphase_manager.getObjects()[obj_index]->collisionGeometry().get() ==
90  // go.geometry.get());
91 
92  BOOST_CHECK(broadphase_manager.check());
93 }
94 
95 BOOST_AUTO_TEST_CASE(test_advanced_filters)
96 {
97  const std::string filename =
98  PINOCCHIO_MODEL_DIR
99  + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
100  std::vector<std::string> packageDirs;
101  const std::string meshDir = PINOCCHIO_MODEL_DIR;
102  packageDirs.push_back(meshDir);
103  const std::string srdf_filename =
104  PINOCCHIO_MODEL_DIR
105  + std::string("/example-robot-data/robots/romeo_description/srdf/romeo.srdf");
106 
107  Model model;
108  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
109  GeometryModel geom_model;
110  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, packageDirs);
111  geom_model.addAllCollisionPairs();
112  pinocchio::srdf::removeCollisionPairs(model, geom_model, srdf_filename, false);
113 
114  GeometryData geom_data(geom_model);
115 
116  typedef BroadPhaseManagerTpl<hpp::fcl::DynamicAABBTreeCollisionManager> BroadPhaseManager;
117  for (size_t joint_id = 0; joint_id < (size_t)model.njoints; ++joint_id)
118  {
119  const GeometryObjectFilterSelectByJoint filter(joint_id);
120  BroadPhaseManager manager(&model, &geom_model, &geom_data, filter);
121  BOOST_CHECK(manager.check());
122  }
123 }
124 
125 BOOST_AUTO_TEST_CASE(test_collisions)
126 {
127  const std::string filename =
128  PINOCCHIO_MODEL_DIR
129  + std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf");
130  std::vector<std::string> packageDirs;
131  const std::string meshDir = PINOCCHIO_MODEL_DIR;
132  packageDirs.push_back(meshDir);
133  const std::string srdf_filename =
134  PINOCCHIO_MODEL_DIR
135  + std::string("/example-robot-data/robots/romeo_description/srdf/romeo.srdf");
136 
137  Model model;
138  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
139  GeometryModel geom_model;
140  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, packageDirs);
141  geom_model.addAllCollisionPairs();
142  pinocchio::srdf::removeCollisionPairs(model, geom_model, srdf_filename, false);
143 
144  Data data(model);
145  GeometryData geom_data(geom_model), geom_data_broadphase(geom_model);
146 
147  pinocchio::srdf::loadReferenceConfigurations(model, srdf_filename, false);
148  Eigen::VectorXd q = model.referenceConfigurations["half_sitting"];
149 
150  pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, q);
151  pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data_broadphase, q);
152 
153  BOOST_CHECK(computeCollisions(geom_model, geom_data) == false);
154  BOOST_CHECK(computeCollisions(geom_model, geom_data, false) == false);
155 
156  BroadPhaseManagerTpl<hpp::fcl::DynamicAABBTreeCollisionManager> broadphase_manager(
157  &model, &geom_model, &geom_data_broadphase);
158  std::cout << "map:\n" << geom_model.collisionPairMapping << std::endl;
159  BOOST_CHECK(computeCollisions(broadphase_manager) == false);
160  BOOST_CHECK(computeCollisions(broadphase_manager, false) == false);
161  BOOST_CHECK(computeCollisions(model, data, broadphase_manager, q) == false);
162  BOOST_CHECK(computeCollisions(model, data, broadphase_manager, q, false) == false);
163 
164  const int num_configs = 1000;
165  for (int i = 0; i < num_configs; ++i)
166  {
167  Eigen::VectorXd q_rand = randomConfiguration(model);
168  q_rand.head<7>() = q.head<7>();
169 
170  BOOST_CHECK(
171  computeCollisions(model, data, broadphase_manager, q_rand)
172  == computeCollisions(model, data, geom_model, geom_data, q_rand));
173  }
174 }
175 
176 BOOST_AUTO_TEST_SUITE_END()
broadphase_dynamic_AABB_tree.h
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.
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:17
hpp::fcl::Sphere
setup.data
data
Definition: cmake/cython/setup.in.py:48
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_broadphase_with_empty_models)
Definition: unittest/broadphase.cpp:25
pinocchio::JointModelFreeFlyerTpl
Definition: multibody/joint/fwd.hpp:110
pinocchio::GeometryObject
Definition: multibody/geometry-object.hpp:87
pinocchio::GeometryObjectFilterSelectByJoint
Definition: geometry-object-filter.hpp:27
hpp::fcl::CollisionGeometryPtr_t
shared_ptr< CollisionGeometry > CollisionGeometryPtr_t
pinocchio::GeometryData
Definition: multibody/geometry.hpp:233
broadphase.hpp
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hpp:315
collision.hpp
distance.hpp
pinocchio::BroadPhaseManagerTpl::check
bool check() const
Check whether the base broad phase manager is aligned with the current collision_objects.
geometry.hpp
filename
filename
urdf.hpp
srdf.hpp
pinocchio::urdf::buildModel
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, const std::string &rootJointName, 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::GeometryObject::geometry
CollisionGeometryPtr geometry
The FCL CollisionGeometry (might be a Mesh, a Geometry Primitive, etc.)
Definition: multibody/geometry-object.hpp:110
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 ...
collisions.srdf_filename
string srdf_filename
Definition: collisions.py: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:1083
pinocchio::BroadPhaseManagerTpl::update
void update(bool compute_local_aabb=false)
Update the manager from the current geometry positions and update the underlying FCL broad phase mana...
collisions.geom_data
geom_data
Definition: collisions.py:42
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::BroadPhaseManagerTpl::getCollisionObjects
const CollisionObjectVector & getCollisionObjects() const
Returns the vector of collision objects associated to the manager.
Definition: collision/broadphase-manager.hpp:127
pinocchio::SE3Tpl::Identity
static SE3Tpl Identity()
Definition: spatial/se3-tpl.hpp:136
pinocchio::GeometryModel
Definition: multibody/geometry.hpp:50
pinocchio::COLLISION
@ COLLISION
Definition: multibody/geometry-object.hpp:27
append-urdf-model-with-another-model.joint_id
joint_id
Definition: append-urdf-model-with-another-model.py:34
pinocchio::ModelTpl
Definition: context/generic.hpp:20
pinocchio::GeomIndex
Index GeomIndex
Definition: multibody/fwd.hpp:27
broadphase-manager.hpp
pinocchio::BroadPhaseManagerTpl
Definition: collision/broadphase-manager.hpp:17
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1082
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
hpp::fcl::Box
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 Thu Dec 19 2024 03:41:25