collisions.py
Go to the documentation of this file.
1 from __future__ import print_function
2 import pinocchio as pin, hppfcl
3 
4 import os
5 from os.path import dirname, join, abspath
6 
7 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),"models")
8 
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)
13 
14 # Load model
15 model = pin.buildModelFromUrdf(urdf_model_path,pin.JointModelFreeFlyer())
16 
17 # Load collision geometries
18 geom_model = pin.buildGeomFromUrdf(model,urdf_model_path,mesh_dir,pin.GeometryType.COLLISION)
19 
20 # Add collisition pairs
21 geom_model.addAllCollisionPairs()
22 print("num collision pairs - initial:",len(geom_model.collisionPairs))
23 
24 # Remove collision pairs listed in the SRDF file
25 srdf_filename = "romeo.srdf"
26 srdf_model_path = model_path + "/romeo_description/srdf/" + srdf_filename
27 
28 pin.removeCollisionPairs(model,geom_model,srdf_model_path)
29 print("num collision pairs - after removing useless collision pairs:",len(geom_model.collisionPairs))
30 
31 # Load reference configuration
32 pin.loadReferenceConfigurations(model,srdf_model_path)
33 
34 # Retrieve the half sitting position from the SRDF file
35 q = model.referenceConfigurations["half_sitting"]
36 
37 # Create data structures
38 data = model.createData()
39 geom_data = pin.GeometryData(geom_model)
40 
41 # Compute all the collisions
42 pin.computeCollisions(model,data,geom_model,geom_data,q,False)
43 
44 # Print the status of collision for all collision pairs
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")
49 
50 # Compute for a single pair of collision
51 pin.updateGeometryPlacements(model,data,geom_model,geom_data,q)
52 pin.computeCollision(geom_model,geom_data,0)
53 


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:29