def tagVisible(self, oMt, size):
""" It assumes that updateGeometryPlacements has been called """
idc = self.model.getFrameId("xtion_rgb_optical_frame")
camera = self.model.frames[idc]
oMc = self.data.oMf[idc]
# oMc.rotation[:,2] view axis
# oMt.rotation[:,2] normal of the tag, on the tag side.
if not isinstance(oMt, pinocchio.SE3):
oMt = pinocchio.XYZQUATToSE3(oMt)
cos_theta = -np.dot(oMc.rotation[:, 2], oMt.rotation[:, 2])
if cos_theta < self.cos_angle_thr:
return False
pts = self.tagToTetahedronPts(oMt, size)
tetahedron = hppfcl.BVHModelOBBRSS()
tetahedron.beginModel(4, 5)
tetahedron.addSubModel(pts, _tetahedron_tris)
tetahedron.endModel()
request = hppfcl.CollisionRequest()
Id = hppfcl.Transform3f()
for go, oMg in zip(self.gmodel.geometryObjects, self.gdata.oMg):
# Don't check for collision with the camera, except with the field_of_view
if go.parentJoint == camera.parent and go.name != "field_of_view":
continue
result = hppfcl.CollisionResult()
hppfcl.collide(go.geometry, hppfcl.Transform3f(oMg), tetahedron,
Id, request, result)
if result.isCollision():
return False
return True
def test_collision(self):
capsule = hppfcl.Capsule(1., 2.)
M1 = hppfcl.Transform3f()
M2 = hppfcl.Transform3f(np.eye(3), np.array([3, 0, 0]))
req = hppfcl.CollisionRequest()
res = hppfcl.CollisionResult()
self.assertTrue(not hppfcl.collide(capsule, M1, capsule, M2, req, res))
def test_convex_halfspace(self):
convex = tetahedron()
halfspace = hppfcl.Halfspace(np.array((0,0,1)), 0)
req=hppfcl.CollisionRequest()
res=hppfcl.CollisionResult()
M1 = hppfcl.Transform3f()
M2 = hppfcl.Transform3f(np.eye(3),np.array([0, 0, -0.1]))
res.clear()
hppfcl.collide(convex, M1, halfspace, M2, req, res)
self.assertFalse(hppfcl.collide(convex, M1, halfspace, M2, req, res))
M1 = hppfcl.Transform3f()
M2 = hppfcl.Transform3f(np.eye(3),np.array([0, 0, 0.1]))
res.clear()
self.assertTrue(hppfcl.collide(convex, M1, halfspace, M2, req, res))
M1 = hppfcl.Transform3f()
M2 = hppfcl.Transform3f(np.eye(3),np.array([0, 0, 2]))
res.clear()
self.assertTrue(hppfcl.collide(convex, M1, halfspace, M2, req, res))
def collide_narrowphase(self, name1, name2, potential_collisions):
import time
start = time.time()
self.logger.info("Starting narrowphase")
collisions = []
for data in potential_collisions:
result = hppfcl.CollisionResult()
hppfcl.collide(
self.groups[name1]["objects"][data["id1"]],
self.groups[name2]["objects"][data["id2"]],
hppfcl.CollisionRequest(),
result,
)
if result.isCollision():
collisions.append({
"id1": data["id1"],
"id2": data["id2"],
"collision": result
})
self.logger.info(f"Finished narrowphase {time.time() - start}")
return collisions
