import trimesh, numpy as np, fcl
def load(f): return trimesh.load(f, force="mesh")
servo, holder = load("st3215.stl"), load("holder_approx_drilled.stl")
thumb, fixed = load("thumb.stl"), load("fixed_jaw.stl")
base = load("gripper_base.stl")
def obj(m):
    b = fcl.BVHModel(); b.beginModel(len(m.vertices), len(m.faces))
    b.addSubModel(np.asarray(m.vertices,float), np.asarray(m.faces,np.int64)); b.endModel()
    return fcl.CollisionObject(b, fcl.Transform())
def hit(a,b):
    r = fcl.CollisionResult(); fcl.collide(obj(a), obj(b), fcl.CollisionRequest(), r); return r.is_collision
print("COLLISIONS (closed pose):")
print("  fixed_jaw vs servo :", hit(fixed, servo))
print("  fixed_jaw vs holder:", hit(fixed, holder))
print("  thumb     vs servo :", hit(thumb, servo))
print("  thumb vs fixed_jaw :", hit(thumb, fixed))
print("  thumb vs base      :", hit(thumb, base))
print("WATERTIGHT: thumb", thumb.is_watertight, " base", base.is_watertight, " bodies", len(base.split(only_watertight=False)))
print("thumb bbox", np.round(thumb.bounds,1).tolist())
print("fixed bbox", np.round(fixed.bounds,1).tolist())
print("servo bbox", np.round(servo.bounds,1).tolist())