| Directory: | ./ |
|---|---|
| File: | examples/collisions.cpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 27 | 27 | 100.0% |
| Branches: | 31 | 60 | 51.7% |
| 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 | 1 | int main(int /*argc*/, char ** /*argv*/) | |
| 16 | { | ||
| 17 | using namespace pinocchio; | ||
| 18 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | 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 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | + 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 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | 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 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Model model; |
| 30 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::urdf::buildModel(urdf_filename, model); |
| 31 | |||
| 32 | // Build the data associated to the model | ||
| 33 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Data data(model); |
| 34 | |||
| 35 | // Load the geometries associated to model which are contained in the URDF file | ||
| 36 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | GeometryModel geom_model; |
| 37 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | 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 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | geom_model.addAllCollisionPairs(); |
| 42 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::srdf::removeCollisionPairs(model, geom_model, srdf_filename); |
| 43 | |||
| 44 | // Build the data associated to the geom_model | ||
| 45 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | 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 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::srdf::loadReferenceConfigurations( |
| 50 | model, | ||
| 51 | srdf_filename); // the reference configuratio stored in the SRDF file is called half_sitting | ||
| 52 | |||
| 53 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | const Model::ConfigVectorType & q = model.referenceConfigurations["half_sitting"]; |
| 54 | |||
| 55 | // And test all the collision pairs | ||
| 56 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | computeCollisions(model, data, geom_model, geom_data, q); |
| 57 | |||
| 58 | // Print the status of all the collision pairs | ||
| 59 |
2/2✓ Branch 1 taken 887 times.
✓ Branch 2 taken 1 times.
|
888 | for (size_t k = 0; k < geom_model.collisionPairs.size(); ++k) |
| 60 | { | ||
| 61 | 887 | const CollisionPair & cp = geom_model.collisionPairs[k]; | |
| 62 | 887 | const hpp::fcl::CollisionResult & cr = geom_data.collisionResults[k]; | |
| 63 | |||
| 64 |
5/10✓ Branch 1 taken 887 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 887 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 887 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 887 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 887 times.
✗ Branch 14 not taken.
|
887 | std::cout << "collision pair: " << cp.first << " , " << cp.second << " - collision: "; |
| 65 |
3/6✗ Branch 1 not taken.
✓ Branch 2 taken 887 times.
✓ Branch 4 taken 887 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 887 times.
✗ Branch 8 not taken.
|
887 | 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 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | 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 | 1 | const PairIndex pair_id = 2; | |
| 75 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | const Model::ConfigVectorType q_neutral = neutral(model); |
| 76 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | 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 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | computeCollision(geom_model, geom_data, pair_id); |
| 81 | |||
| 82 | 1 | return 0; | |
| 83 | 1 | } | |
| 84 |