Collision detection and distances

Python

import os
from pathlib import Path

import pinocchio as pin

model_path = Path(os.environ.get("EXAMPLE_ROBOT_DATA_MODEL_DIR"))
mesh_dir = model_path.parent.parent
urdf_filename = "romeo_small.urdf"
urdf_model_path = model_path / "romeo_description/urdf" / urdf_filename

# Load model
model = pin.buildModelFromUrdf(urdf_model_path, pin.JointModelFreeFlyer())

# Load collision geometries
geom_model = pin.buildGeomFromUrdf(
model, urdf_model_path, pin.GeometryType.COLLISION, mesh_dir
)

# Add collisition pairs
geom_model.addAllCollisionPairs()
print("num collision pairs - initial:", len(geom_model.collisionPairs))

# Remove collision pairs listed in the SRDF file
srdf_filename = "romeo.srdf"
srdf_model_path = model_path / "romeo_description/srdf" / srdf_filename

pin.removeCollisionPairs(model, geom_model, srdf_model_path)
print(
"num collision pairs - after removing useless collision pairs:",
len(geom_model.collisionPairs),
)

# Load reference configuration
pin.loadReferenceConfigurations(model, srdf_model_path)

# Retrieve the half sitting position from the SRDF file
q = model.referenceConfigurations["half_sitting"]

# Create data structures
data = model.createData()
geom_data = pin.GeometryData(geom_model)


# Compute all the collisions
pin.computeCollisions(model, data, geom_model, geom_data, q, False)

# Print the status of collision for all collision pairs
for k in range(len(geom_model.collisionPairs)):
cr = geom_data.collisionResults[k]
cp = geom_model.collisionPairs[k]
print(
"collision pair:",
cp.first,
",",
cp.second,
"- collision:",
"Yes" if cr.isCollision() else "No",
)

# Compute for a single pair of collision
pin.updateGeometryPlacements(model, data, geom_model, geom_data, q)
pin.computeCollision(geom_model, geom_data, 0)

C++

#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"

#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/collision/collision.hpp"

#include <iostream>

#include <boost/filesystem.hpp>

// EXAMPLE_ROBOT_DATA_MODEL_DIR is defined by the CMake but you can define your own modeldirectory
// here.
#ifndef EXAMPLE_ROBOT_DATA_MODEL_DIR
#define EXAMPLE_ROBOT_DATA_MODEL_DIR "path_to_the_model_dir"
#endif

int main(int /*argc*/, char ** /*argv*/)
{
using namespace pinocchio;
const std::string robots_model_path = EXAMPLE_ROBOT_DATA_MODEL_DIR;

// You should change here to set up your own URDF file
const std::string urdf_filename =
robots_model_path + std::string("/talos_data/robots/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,
boost::filesystem::path(robots_model_path).parent_path().parent_path().string());

// 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;
}