collisions.cpp
Go to the documentation of this file.
3 
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;
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
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();
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)
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
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
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);
78  q_neutral); // performs a forward kinematics over the whole kinematics model + update the
79  // placement of all the geometries contained inside geom_model
81 
82  return 0;
83 }
append-urdf-model-with-another-model.urdf_filename
string urdf_filename
Definition: append-urdf-model-with-another-model.py:16
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::computeCollisions
bool computeCollisions(BroadPhaseManagerBase< BroadPhaseManagerDerived > &broadphase_manager, CollisionCallBackBase *callback)
Calls computeCollision for every active pairs of GeometryData. This function assumes that updateGeome...
Definition: broadphase.hpp:34
pinocchio::updateGeometryPlacements
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.
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::GeometryData
Definition: multibody/geometry.hpp:233
collision.hpp
joint-configuration.hpp
geometry.hpp
pinocchio::CollisionPair
Definition: multibody/geometry.hpp:21
urdf.hpp
srdf.hpp
pinocchio::PairIndex
Index PairIndex
Definition: multibody/fwd.hpp:29
pinocchio::computeCollision
bool computeCollision(const GeometryModel &geom_model, GeometryData &geom_data, const PairIndex pair_id, fcl::CollisionRequest &collision_request)
Compute the collision status between a SINGLE collision pair. The result is store in the collisionRes...
pinocchio::urdf::buildModel
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, const std::string &rootJointName, 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...
hpp::fcl::CollisionResult
pinocchio::urdf::buildGeom
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 ...
main
int main(int, char **)
Definition: collisions.cpp:15
collisions.srdf_filename
string srdf_filename
Definition: collisions.py:25
append-urdf-model-with-another-model.geom_model
geom_model
Definition: append-urdf-model-with-another-model.py:26
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1083
collisions.cp
cp
Definition: collisions.py:51
collisions.geom_data
geom_data
Definition: collisions.py:42
pinocchio::srdf::loadReferenceConfigurations
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...
pinocchio::GeometryModel
Definition: multibody/geometry.hpp:50
pinocchio::COLLISION
@ COLLISION
Definition: multibody/geometry-object.hpp:27
pinocchio::neutral
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
Definition: joint-configuration.hpp:363
pinocchio::ModelTpl::ConfigVectorType
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Definition: multibody/model.hpp:87
PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR
Definition: collisions.cpp:12
pinocchio::ModelTpl
Definition: context/generic.hpp:20
cr
cr
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1082
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
pinocchio::srdf::removeCollisionPairs
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...


pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:08