GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: examples/collisions.cpp Lines: 26 26 100.0 %
Date: 2024-01-23 21:41:47 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
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
}