1 from __future__
import print_function
2 import pinocchio
as pin, hppfcl
5 from os.path import dirname, join, abspath
7 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),
"models")
9 model_path = join(pinocchio_model_dir,
"example-robot-data/robots")
10 mesh_dir = pinocchio_model_dir
11 urdf_filename =
"romeo_small.urdf" 12 urdf_model_path = join(join(model_path,
"romeo_description/urdf"),urdf_filename)
15 model = pin.buildModelFromUrdf(urdf_model_path,pin.JointModelFreeFlyer())
18 geom_model = pin.buildGeomFromUrdf(model,urdf_model_path,mesh_dir,pin.GeometryType.COLLISION)
21 geom_model.addAllCollisionPairs()
22 print(
"num collision pairs - initial:",len(geom_model.collisionPairs))
25 srdf_filename =
"romeo.srdf" 26 srdf_model_path = model_path +
"/romeo_description/srdf/" + srdf_filename
28 pin.removeCollisionPairs(model,geom_model,srdf_model_path)
29 print(
"num collision pairs - after removing useless collision pairs:",len(geom_model.collisionPairs))
32 pin.loadReferenceConfigurations(model,srdf_model_path)
35 q = model.referenceConfigurations[
"half_sitting"]
38 data = model.createData()
39 geom_data = pin.GeometryData(geom_model)
42 pin.computeCollisions(model,data,geom_model,geom_data,q,
False)
45 for k
in range(len(geom_model.collisionPairs)):
46 cr = geom_data.collisionResults[k]
47 cp = geom_model.collisionPairs[k]
48 print(
"collision pair:",cp.first,
",",cp.second,
"- collision:",
"Yes" if cr.isCollision()
else "No")
51 pin.updateGeometryPlacements(model,data,geom_model,geom_data,q)
52 pin.computeCollision(geom_model,geom_data,0)