collisions.cpp
Go to the documentation of this file.
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 int main(int /*argc*/, char ** /*argv*/)
15 {
16  using namespace pinocchio;
17  const std::string robots_model_path = PINOCCHIO_MODEL_DIR;
18 
19  // You should change here to set up your own URDF file
20  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  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  Model model;
26  pinocchio::urdf::buildModel(urdf_filename,model);
27 
28  // Build the data associated to the model
29  Data data(model);
30 
31  // Load the geometries associated to model which are contained in the URDF file
33  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  geom_model.addAllCollisionPairs();
37  pinocchio::srdf::removeCollisionPairs(model, geom_model, srdf_filename);
38 
39  // Build the data associated to the geom_model
40  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  pinocchio::srdf::loadReferenceConfigurations(model,srdf_filename); // the reference configuratio stored in the SRDF file is called half_sitting
44 
45  const Model::ConfigVectorType & q = model.referenceConfigurations["half_sitting"];
46 
47  // And test all the collision pairs
48  computeCollisions(model,data,geom_model,geom_data,q);
49 
50  // Print the status of all the collision pairs
51  for(size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
52  {
53  const CollisionPair & cp = geom_model.collisionPairs[k];
54  const hpp::fcl::CollisionResult & cr = geom_data.collisionResults[k];
55 
56  std::cout << "collision pair: " << cp.first << " , " << cp.second << " - collision: ";
57  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  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  const PairIndex pair_id = 2;
65  const Model::ConfigVectorType q_neutral = neutral(model);
66  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  computeCollision(geom_model, geom_data, pair_id);
68 
69  return 0;
70 }
void loadReferenceConfigurations(ModelTpl< Scalar, Options, JointCollectionTpl > &model, const std::string &filename, const bool verbose=false)
Get the reference configurations of a given model associated to a SRDF file. It throws if the SRDF fi...
void removeCollisionPairs(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, GeometryModel &geom_model, const std::string &filename, const bool verbose=false)
Deactive all possible collision pairs mentioned in the SRDF file. It throws if the SRDF file is incor...
int main(int, char **)
Definition: collisions.cpp:14
CollisionPairVector collisionPairs
Vector of collision pairs.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
#define PINOCCHIO_MODEL_DIR
Definition: collisions.cpp:11
ConfigVectorMap referenceConfigurations
Map of reference configurations, indexed by user given names.
bool computeCollisions(const int num_threads, const GeometryModel &geom_model, GeometryData &geom_data, const bool stopAtFirstCollision=false)
GeometryModel & buildGeom(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const std::string &filename, const GeometryType type, GeometryModel &geom_model, const std::vector< std::string > &package_paths=std::vector< std::string >(), ::hpp::fcl::MeshLoaderPtr mesh_loader=::hpp::fcl::MeshLoaderPtr())
Build The GeometryModel from a URDF file. Search for meshes in the directories specified by the user ...
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a URDF file with a particular joint as root of the model tree inside the model g...
void addAllCollisionPairs()
Add all possible collision pairs.
string srdf_filename
Definition: collisions.py:25
Main pinocchio namespace.
Definition: timings.cpp:28
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
cr
void updateGeometryPlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const GeometryModel &geom_model, GeometryData &geom_data, const Eigen::MatrixBase< ConfigVectorType > &q)
Apply a forward kinematics and update the placement of the geometry objects.
JointCollectionTpl & model


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:29