| Line |
Branch |
Exec |
Source |
| 1 |
|
|
// |
| 2 |
|
|
// Copyright (c) 2022 INRIA |
| 3 |
|
|
// |
| 4 |
|
|
|
| 5 |
|
|
#include <iostream> |
| 6 |
|
|
|
| 7 |
|
|
#include "pinocchio/collision/collision.hpp" |
| 8 |
|
|
#include "pinocchio/collision/broadphase.hpp" |
| 9 |
|
|
#include "pinocchio/collision/tree-broadphase-manager.hpp" |
| 10 |
|
|
#include "pinocchio/parsers/urdf.hpp" |
| 11 |
|
|
#include "pinocchio/parsers/srdf.hpp" |
| 12 |
|
|
|
| 13 |
|
|
#include <hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h> |
| 14 |
|
|
|
| 15 |
|
|
#include <vector> |
| 16 |
|
|
#include <boost/test/unit_test.hpp> |
| 17 |
|
|
|
| 18 |
|
|
using namespace pinocchio; |
| 19 |
|
|
|
| 20 |
|
|
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
| 21 |
|
|
|
| 22 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_tree_broadphase_with_empty_models) |
| 23 |
|
|
{ |
| 24 |
|
✗ |
Model model; |
| 25 |
|
✗ |
GeometryModel geom_model; |
| 26 |
|
✗ |
GeometryData geom_data(geom_model); |
| 27 |
|
|
|
| 28 |
|
|
TreeBroadPhaseManagerTpl<hpp::fcl::DynamicAABBTreeCollisionManager> broadphase_manager( |
| 29 |
|
✗ |
&model, &geom_model, &geom_data); |
| 30 |
|
|
|
| 31 |
|
✗ |
BOOST_CHECK(broadphase_manager.check()); |
| 32 |
|
|
} |
| 33 |
|
|
|
| 34 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_collisions) |
| 35 |
|
|
{ |
| 36 |
|
|
const std::string filename = |
| 37 |
|
|
PINOCCHIO_MODEL_DIR |
| 38 |
|
✗ |
+ std::string("/example-robot-data/robots/romeo_description/urdf/romeo_small.urdf"); |
| 39 |
|
✗ |
std::vector<std::string> packageDirs; |
| 40 |
|
✗ |
const std::string meshDir = PINOCCHIO_MODEL_DIR; |
| 41 |
|
✗ |
packageDirs.push_back(meshDir); |
| 42 |
|
|
const std::string srdf_filename = |
| 43 |
|
|
PINOCCHIO_MODEL_DIR |
| 44 |
|
✗ |
+ std::string("/example-robot-data/robots/romeo_description/srdf/romeo.srdf"); |
| 45 |
|
|
|
| 46 |
|
✗ |
Model model; |
| 47 |
|
✗ |
pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model); |
| 48 |
|
✗ |
GeometryModel geom_model; |
| 49 |
|
✗ |
pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model, packageDirs); |
| 50 |
|
✗ |
geom_model.addAllCollisionPairs(); |
| 51 |
|
✗ |
pinocchio::srdf::removeCollisionPairs(model, geom_model, srdf_filename, false); |
| 52 |
|
|
|
| 53 |
|
✗ |
Data data(model), data_broadphase(model); |
| 54 |
|
✗ |
GeometryData geom_data(geom_model), geom_data_broadphase(geom_model); |
| 55 |
|
|
|
| 56 |
|
✗ |
pinocchio::srdf::loadReferenceConfigurations(model, srdf_filename, false); |
| 57 |
|
✗ |
Eigen::VectorXd q = model.referenceConfigurations["half_sitting"]; |
| 58 |
|
|
|
| 59 |
|
✗ |
pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, q); |
| 60 |
|
✗ |
pinocchio::updateGeometryPlacements(model, data_broadphase, geom_model, geom_data_broadphase, q); |
| 61 |
|
|
|
| 62 |
|
✗ |
BOOST_CHECK(computeCollisions(geom_model, geom_data) == false); |
| 63 |
|
✗ |
BOOST_CHECK(computeCollisions(geom_model, geom_data, false) == false); |
| 64 |
|
|
|
| 65 |
|
|
TreeBroadPhaseManagerTpl<hpp::fcl::DynamicAABBTreeCollisionManager> broadphase_manager( |
| 66 |
|
✗ |
&model, &geom_model, &geom_data_broadphase); |
| 67 |
|
✗ |
BOOST_CHECK(computeCollisions(broadphase_manager) == false); |
| 68 |
|
✗ |
BOOST_CHECK(computeCollisions(broadphase_manager, false) == false); |
| 69 |
|
✗ |
BOOST_CHECK(computeCollisions(model, data_broadphase, broadphase_manager, q) == false); |
| 70 |
|
✗ |
BOOST_CHECK(computeCollisions(model, data_broadphase, broadphase_manager, q, false) == false); |
| 71 |
|
|
|
| 72 |
|
✗ |
const int num_configs = 1000; |
| 73 |
|
✗ |
for (int i = 0; i < num_configs; ++i) |
| 74 |
|
|
{ |
| 75 |
|
✗ |
Eigen::VectorXd q_rand = randomConfiguration(model); |
| 76 |
|
✗ |
q_rand.head<7>() = q.head<7>(); |
| 77 |
|
|
|
| 78 |
|
✗ |
BOOST_CHECK( |
| 79 |
|
|
computeCollisions(model, data_broadphase, broadphase_manager, q_rand) |
| 80 |
|
|
== computeCollisions(model, data, geom_model, geom_data, q_rand)); |
| 81 |
|
|
} |
| 82 |
|
|
} |
| 83 |
|
|
|
| 84 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
| 85 |
|
|
|