Line |
Branch |
Exec |
Source |
1 |
|
|
#include "pinocchio/parsers/urdf.hpp" |
2 |
|
|
#include "pinocchio/parsers/srdf.hpp" |
3 |
|
|
|
4 |
|
|
#include "pinocchio/algorithm/joint-configuration.hpp" |
5 |
|
|
#include "pinocchio/algorithm/geometry.hpp" |
6 |
|
|
#include "pinocchio/collision/collision.hpp" |
7 |
|
|
|
8 |
|
|
#include <iostream> |
9 |
|
|
|
10 |
|
|
// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own modeldirectory here. |
11 |
|
|
#ifndef PINOCCHIO_MODEL_DIR |
12 |
|
|
#define PINOCCHIO_MODEL_DIR "path_to_the_model_dir" |
13 |
|
|
#endif |
14 |
|
|
|
15 |
|
✗ |
int main(int /*argc*/, char ** /*argv*/) |
16 |
|
|
{ |
17 |
|
|
using namespace pinocchio; |
18 |
|
✗ |
const std::string robots_model_path = PINOCCHIO_MODEL_DIR; |
19 |
|
|
|
20 |
|
|
// You should change here to set up your own URDF file |
21 |
|
|
const std::string urdf_filename = |
22 |
|
|
robots_model_path |
23 |
|
✗ |
+ std::string("/example-robot-data/robots/talos_data/robots/talos_reduced.urdf"); |
24 |
|
|
// You should change here to set up your own SRDF file |
25 |
|
|
const std::string srdf_filename = |
26 |
|
✗ |
robots_model_path + std::string("/example-robot-data/robots/talos_data/srdf/talos.srdf"); |
27 |
|
|
|
28 |
|
|
// Load the URDF model contained in urdf_filename |
29 |
|
✗ |
Model model; |
30 |
|
✗ |
pinocchio::urdf::buildModel(urdf_filename, model); |
31 |
|
|
|
32 |
|
|
// Build the data associated to the model |
33 |
|
✗ |
Data data(model); |
34 |
|
|
|
35 |
|
|
// Load the geometries associated to model which are contained in the URDF file |
36 |
|
✗ |
GeometryModel geom_model; |
37 |
|
✗ |
pinocchio::urdf::buildGeom( |
38 |
|
|
model, urdf_filename, pinocchio::COLLISION, geom_model, robots_model_path); |
39 |
|
|
|
40 |
|
|
// Add all possible collision pairs and remove the ones collected in the SRDF file |
41 |
|
✗ |
geom_model.addAllCollisionPairs(); |
42 |
|
✗ |
pinocchio::srdf::removeCollisionPairs(model, geom_model, srdf_filename); |
43 |
|
|
|
44 |
|
|
// Build the data associated to the geom_model |
45 |
|
✗ |
GeometryData geom_data(geom_model); // contained the intermediate computations, like the placement |
46 |
|
|
// of all the geometries with respect to the world frame |
47 |
|
|
|
48 |
|
|
// Load the reference configuration of the robots (this one should be collision free) |
49 |
|
✗ |
pinocchio::srdf::loadReferenceConfigurations( |
50 |
|
|
model, |
51 |
|
|
srdf_filename); // the reference configuratio stored in the SRDF file is called half_sitting |
52 |
|
|
|
53 |
|
✗ |
const Model::ConfigVectorType & q = model.referenceConfigurations["half_sitting"]; |
54 |
|
|
|
55 |
|
|
// And test all the collision pairs |
56 |
|
✗ |
computeCollisions(model, data, geom_model, geom_data, q); |
57 |
|
|
|
58 |
|
|
// Print the status of all the collision pairs |
59 |
|
✗ |
for (size_t k = 0; k < geom_model.collisionPairs.size(); ++k) |
60 |
|
|
{ |
61 |
|
✗ |
const CollisionPair & cp = geom_model.collisionPairs[k]; |
62 |
|
✗ |
const hpp::fcl::CollisionResult & cr = geom_data.collisionResults[k]; |
63 |
|
|
|
64 |
|
✗ |
std::cout << "collision pair: " << cp.first << " , " << cp.second << " - collision: "; |
65 |
|
✗ |
std::cout << (cr.isCollision() ? "yes" : "no") << std::endl; |
66 |
|
|
} |
67 |
|
|
|
68 |
|
|
// If you want to stop as soon as a collision is encounter, just add false for the final default |
69 |
|
|
// argument stopAtFirstCollision |
70 |
|
✗ |
computeCollisions(model, data, geom_model, geom_data, q, true); |
71 |
|
|
|
72 |
|
|
// And if you to check only one collision pair, e.g. the third one, at the neutral element of the |
73 |
|
|
// Configuration Space of the robot |
74 |
|
✗ |
const PairIndex pair_id = 2; |
75 |
|
✗ |
const Model::ConfigVectorType q_neutral = neutral(model); |
76 |
|
✗ |
updateGeometryPlacements( |
77 |
|
|
model, data, geom_model, geom_data, |
78 |
|
|
q_neutral); // performs a forward kinematics over the whole kinematics model + update the |
79 |
|
|
// placement of all the geometries contained inside geom_model |
80 |
|
✗ |
computeCollision(geom_model, geom_data, pair_id); |
81 |
|
|
|
82 |
|
✗ |
return 0; |
83 |
|
|
} |
84 |
|
|
|