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