| 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 |
|
|
|