forked from stack-of-tasks/pinocchio
-
Notifications
You must be signed in to change notification settings - Fork 0
/
collisions.py
53 lines (38 loc) · 1.84 KB
/
collisions.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
from __future__ import print_function
import pinocchio as pin, hppfcl
import os
from os.path import dirname, join, abspath
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),"models")
model_path = join(pinocchio_model_dir,"example-robot-data/robots")
mesh_dir = pinocchio_model_dir
urdf_filename = "romeo_small.urdf"
urdf_model_path = join(join(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,mesh_dir,pin.GeometryType.COLLISION)
# 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)