pinocchio  2.3.1-dirty
Collision detection and distances

Python

1 from __future__ import print_function
2 import pinocchio as pin
3 pin.switchToNumpyMatrix()
4 
5 import os
6 from os.path import dirname, join, abspath
7 
8 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),"models")
9 
10 model_path = join(pinocchio_model_dir,"others/robots")
11 mesh_dir = model_path
12 urdf_filename = "romeo_small.urdf"
13 urdf_model_path = join(join(model_path,"romeo_description/urdf"),urdf_filename)
14 
15 # Load model
16 model = pin.buildModelFromUrdf(urdf_model_path,pin.JointModelFreeFlyer())
17 
18 # Load collision geometries
19 geom_model = pin.buildGeomFromUrdf(model,urdf_model_path,model_path,pin.GeometryType.COLLISION)
20 
21 # Add collisition pairs
22 geom_model.addAllCollisionPairs()
23 print("num collision pairs - initial:",len(geom_model.collisionPairs))
24 
25 # Remove collision pairs listed in the SRDF file
26 srdf_filename = "romeo.srdf"
27 srdf_model_path = model_path + "/romeo_description/srdf/" + srdf_filename
28 
29 pin.removeCollisionPairs(model,geom_model,srdf_model_path)
30 print("num collision pairs - after removing useless collision pairs:",len(geom_model.collisionPairs))
31 
32 # Load reference configuration
33 pin.loadReferenceConfigurations(model,srdf_model_path)
34 
35 # Retrieve the half sitting position from the SRDF file
36 q = model.referenceConfigurations["half_sitting"]
37 
38 # Create data structures
39 data = model.createData()
40 geom_data = pin.GeometryData(geom_model)
41 
42 # Compute all the collisions
43 pin.computeCollisions(model,data,geom_model,geom_data,q,False)
44 
45 # Print the status of collision for all collision pairs
46 for k in range(len(geom_model.collisionPairs)):
47  cr = geom_data.collisionResults[k]
48  cp = geom_model.collisionPairs[k]
49  print("collision pair:",cp.first,",",cp.second,"- collision:","Yes" if cr.isCollision() else "No")
50 
51 # Compute for a single pair of collision
52 pin.updateGeometryPlacements(model,data,geom_model,geom_data,q)
53 pin.computeCollision(geom_model,geom_data,0)
54 

C++

#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/geometry.hpp"
#include <iostream>
// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own modeldirectory here.
#ifndef PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
#endif
int main(int /*argc*/, char ** /*argv*/)
{
using namespace pinocchio;
const std::string robots_model_path = PINOCCHIO_MODEL_DIR + std::string("/others/robots/");
// You should change here to set up your own URDF file
const std::string urdf_filename = robots_model_path + std::string("talos_data/urdf/talos_reduced.urdf");
// You should change here to set up your own SRDF file
const std::string srdf_filename = robots_model_path + std::string("talos_data/srdf/talos.srdf");
// Load the URDF model contained in urdf_filename
Model model;
pinocchio::urdf::buildModel(urdf_filename,model);
// Build the data associated to the model
Data data(model);
// Load the geometries associated to model which are contained in the URDF file
GeometryModel geom_model;
pinocchio::urdf::buildGeom(model, urdf_filename, pinocchio::COLLISION, geom_model, robots_model_path);
// Add all possible collision pairs and remove the ones collected in the SRDF file
geom_model.addAllCollisionPairs();
pinocchio::srdf::removeCollisionPairs(model, geom_model, srdf_filename);
// Build the data associated to the geom_model
GeometryData geom_data(geom_model); // contained the intermediate computations, like the placement of all the geometries with respect to the world frame
// Load the reference configuration of the robots (this one should be collision free)
pinocchio::srdf::loadReferenceConfigurations(model,srdf_filename); // the reference configuratio stored in the SRDF file is called half_sitting
const Model::ConfigVectorType & q = model.referenceConfigurations["half_sitting"];
// And test all the collision pairs
computeCollisions(model,data,geom_model,geom_data,q);
// Print the status of all the collision pairs
for(size_t k = 0; k < geom_model.collisionPairs.size(); ++k)
{
const CollisionPair & cp = geom_model.collisionPairs[k];
const hpp::fcl::CollisionResult & cr = geom_data.collisionResults[k];
std::cout << "collision pair: " << cp.first << " , " << cp.second << " - collision: ";
std::cout << (cr.isCollision() ? "yes" : "no") << std::endl;
}
// If you want to stop as soon as a collision is encounter, just add false for the final default argument stopAtFirstCollision
computeCollisions(model,data,geom_model,geom_data,q,true);
// 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
const PairIndex pair_id = 2;
const Model::ConfigVectorType q_neutral = neutral(model);
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
computeCollision(geom_model, geom_data, pair_id);
return 0;
}