| Line |
Branch |
Exec |
Source |
| 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() |
| 177 |
|
|
|