1 from __future__
import print_function
2 import pinocchio
as pin
6 from os.path import dirname, join, abspath
8 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),
"models")
10 model_path = join(pinocchio_model_dir,
"example-robot-data/robots")
11 mesh_dir = pinocchio_model_dir
12 urdf_filename =
"romeo_small.urdf"
13 urdf_model_path = join(join(model_path,
"romeo_description/urdf"), urdf_filename)
16 model = pin.buildModelFromUrdf(urdf_model_path, pin.JointModelFreeFlyer())
19 geom_model = pin.buildGeomFromUrdf(
20 model, urdf_model_path, pin.GeometryType.COLLISION, mesh_dir
24 geom_model.addAllCollisionPairs()
25 print(
"num collision pairs - initial:", len(geom_model.collisionPairs))
28 srdf_filename =
"romeo.srdf"
29 srdf_model_path = model_path +
"/romeo_description/srdf/" + srdf_filename
31 pin.removeCollisionPairs(model, geom_model, srdf_model_path)
33 "num collision pairs - after removing useless collision pairs:",
34 len(geom_model.collisionPairs),
38 pin.loadReferenceConfigurations(model, srdf_model_path)
41 q = model.referenceConfigurations[
"half_sitting"]
44 data = model.createData()
45 geom_data = pin.GeometryData(geom_model)
49 pin.computeCollisions(model, data, geom_model, geom_data, q,
False)
52 for k
in range(len(geom_model.collisionPairs)):
53 cr = geom_data.collisionResults[k]
54 cp = geom_model.collisionPairs[k]
61 "Yes" if cr.isCollision()
else "No",
65 pin.updateGeometryPlacements(model, data, geom_model, geom_data, q)
66 pin.computeCollision(geom_model, geom_data, 0)