Exemplo n.º 1
0
    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
Exemplo n.º 2
0
    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))
Exemplo n.º 3
0
    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))
Exemplo n.º 4
0
    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