コード例 #1
0
 def _check_collision(self, obstacle, robot):
     if self.section == 1:
         ret, res = fcl.collide(obstacle, robot, fcl.CollisionRequest())
     else:
         for bot_part in robot:
             ret, res = fcl.collide(obstacle, bot_part, fcl.CollisionRequest())
             if ret = False:
                 break
コード例 #2
0
ファイル: fcl_unit_test.py プロジェクト: kjyv/python-fcl
    def test_triangle(self):
        """

            TriangleP is not well supported...

            Perhaps not much of an issue, since BVHModel will deal with meshes / triangle soup
            So, what'evs?

        """
        import numpy as np

        li = [
            [0, 100, 100],
            [0, 0, 0],
            [100, 0, 100],
        ]
        arr = np.array(li, "f")
        _p = fcl.TriangleP(arr[0], arr[1], arr[2])
        p = fcl.CollisionObject(_p)

        collision_object = fcl.CollisionObject(
            self.box, transform.Transform(transform.Quaternion()))

        # TODO: segfault!
        # ....Warning: distance function between node type 9 and node type 17 is not supported
        # dis, result = fcl.distance(collision_object, p, fcl.DistanceRequest(True))

        ret, result = fcl.collide(collision_object, p, fcl.CollisionRequest())
        # Warning: collision function between node type 9 and node type 17 is not supported
        self.assertTrue(ret == 0)
コード例 #3
0
ファイル: fcl_unit_test.py プロジェクト: neka-nat/python-fcl
    def test_triangle(self):
        """

            TriangleP is not well supported...

            Perhaps not much of an issue, since BVHModel will deal with meshes / triangle soup
            So, what'evs?

        """
        import numpy as np

        li = [[0, 100, 100], [0, 0, 0], [100, 0, 100]]
        arr = np.array(li, "f")
        _p = fcl.TriangleP(arr[0], arr[1], arr[2])
        p = fcl.CollisionObject(_p)

        collision_object = fcl.CollisionObject(self.box, transform.Transform(transform.Quaternion()))

        # TODO: segfault!
        # ....Warning: distance function between node type 9 and node type 17 is not supported
        # dis, result = fcl.distance(collision_object, p, fcl.DistanceRequest(True))

        ret, result = fcl.collide(collision_object, p, fcl.CollisionRequest())
        # Warning: collision function between node type 9 and node type 17 is not supported
        self.assertTrue(ret == 0)
コード例 #4
0
ファイル: fcl_unit_test.py プロジェクト: neka-nat/python-fcl
    def test_collision_box_sphere(self):
        ret, result = fcl.collide(
            fcl.CollisionObject(fcl.Box(*self._side), transform.Transform(transform.Quaternion())),
            fcl.CollisionObject(fcl.Sphere(self._radius), transform.Transform(transform.Quaternion(), [0.0, 0.0, 0.0])),
            fcl.CollisionRequest(),
        )

        self.assertTrue(len(result.contacts) == 1)
        self.assertTrue(len(result.cost_sources) == 0)

        con = result.contacts[0]
        self.assertAlmostEqual(con.penetration_depth, 0.0)
        self.assertTrue(con.penetration_depth < sys.float_info.min)
        self.assertTrue(isinstance(con.o1, fcl.Box))
        self.assertTrue(isinstance(con.o2, fcl.Sphere))
コード例 #5
0
ファイル: fcl_unit_test.py プロジェクト: kjyv/python-fcl
    def test_collision_box_sphere(self):
        ret, result = fcl.collide(
            fcl.CollisionObject(fcl.Box(*self._side),
                                transform.Transform(transform.Quaternion())),
            fcl.CollisionObject(
                fcl.Sphere(self._radius),
                transform.Transform(transform.Quaternion(), [0.0, 0.0, 0.0])),
            fcl.CollisionRequest())

        self.assertTrue(len(result.contacts) == 1)
        self.assertTrue(len(result.cost_sources) == 0)

        con = result.contacts[0]
        self.assertAlmostEqual(con.penetration_depth, 0.0)
        self.assertTrue(con.penetration_depth < sys.float_info.min)
        self.assertTrue(isinstance(con.o1, fcl.Box))
        self.assertTrue(isinstance(con.o2, fcl.Sphere))
コード例 #6
0
ファイル: fcl_unit_test.py プロジェクト: kjyv/python-fcl
    def test_distance_box_sphere_translated(self):
        ret, result = fcl.collide(
            fcl.CollisionObject(self.box, self.trans1),
            fcl.CollisionObject(self.sphere, self.trans2),
            fcl.CollisionRequest())

        self.assertEqual(result.contacts, [])
        self.assertEqual(result.cost_sources, [])

        dis, result = fcl.distance(
            fcl.CollisionObject(self.box, self.trans1),
            fcl.CollisionObject(self.sphere, self.trans2),
            fcl.DistanceRequest(True))

        self.assertEqual(dis, self._ref_dist)
        self.assertEqual(result.min_distance, self._ref_dist)
        self.assertEqual(result.nearest_points, self._nearest_points)
        self.assertEqual(result.o2.radius, self._radius)
        self.assertEqual(result.o1.side, self._side)
コード例 #7
0
ファイル: fcl_unit_test.py プロジェクト: neka-nat/python-fcl
    def test_distance_box_sphere_translated(self):
        ret, result = fcl.collide(
            fcl.CollisionObject(self.box, self.trans1),
            fcl.CollisionObject(self.sphere, self.trans2),
            fcl.CollisionRequest(),
        )

        self.assertEqual(result.contacts, [])
        self.assertEqual(result.cost_sources, [])

        dis, result = fcl.distance(
            fcl.CollisionObject(self.box, self.trans1),
            fcl.CollisionObject(self.sphere, self.trans2),
            fcl.DistanceRequest(True),
        )

        self.assertEqual(dis, self._ref_dist)
        self.assertEqual(result.min_distance, self._ref_dist)
        self.assertEqual(result.nearest_points, self._nearest_points)
        self.assertEqual(result.o2.radius, self._radius)
        self.assertEqual(result.o1.side, self._side)
コード例 #8
0
ファイル: fcl_unit_test.py プロジェクト: kjyv/python-fcl
 def collide_callback_func(obj1, obj2, res):
     ret, res = fcl.collide(obj1, obj2, fcl.CollisionRequest())
     return ret
コード例 #9
0
    def getLinkDistance(self, l0_name, l1_name, joint_q):
        # type: (str, str, np._ArrayLike[float]) -> float
        '''get shortest distance from link with id l0 to link with id l1 for posture joint_q'''

        from fcl import fcl, collision_data, transform

        #get link rotation and position in world frame
        q = iDynTree.VectorDynSize.fromList(joint_q)
        self.model.dynComp.setRobotState(q, self.dq_zero, self.dq_zero, self.world_gravity)

        if l0_name in self.model.linkNames:    # if robot link
            f0 = self.model.dynComp.getFrameIndex(l0_name)
            t0 = self.model.dynComp.getWorldTransform(f0)
            rot0 = t0.getRotation().toNumPy()
            pos0 = t0.getPosition().toNumPy()
            s0 = self.config['scaleCollisionHull']
        else:   # if world link
            pos0 = self.link_cuboid_hulls[l0_name][1]
            rot0 = eulerAnglesToRotationMatrix(self.link_cuboid_hulls[l0_name][2])
            s0 = 1

        if l1_name in self.model.linkNames:    # if robot link
            f1 = self.model.dynComp.getFrameIndex(l1_name)
            t1 = self.model.dynComp.getWorldTransform(f1)
            rot1 = t1.getRotation().toNumPy()
            pos1 = t1.getPosition().toNumPy()
            s1 = self.config['scaleCollisionHull']
        else:   # if world link
            pos1 = self.link_cuboid_hulls[l1_name][1]
            rot1 = eulerAnglesToRotationMatrix(self.link_cuboid_hulls[l1_name][2])
            s1 = 1

        # TODO: use pos and rot of boxes for vals from geometry tags
        # self.link_cuboid_hulls[l0_name][1], [2]

        b = np.array(self.link_cuboid_hulls[l0_name][0]) * s0
        p = np.array(self.link_cuboid_hulls[l0_name][1])
        b0_center = 0.5*np.array([(b[1][0])+(b[0][0]) + p[0],
                                  (b[1][1])+(b[0][1]) + p[1],
                                  (b[1][2])+(b[0][2]) + p[2]])
        b0 = fcl.Box(b[1][0]-b[0][0], b[1][1]-b[0][1], b[1][2]-b[0][2])

        b = np.array(self.link_cuboid_hulls[l1_name][0]) * s1
        p = np.array(self.link_cuboid_hulls[l1_name][1])
        b1_center = 0.5*np.array([(b[1][0])+(b[0][0]) + p[0],
                                  (b[1][1])+(b[0][1]) + p[1],
                                  (b[1][2])+(b[0][2]) + p[2]])
        b1 = fcl.Box(b[1][0]-b[0][0], b[1][1]-b[0][1], b[1][2]-b[0][2])

        # move box to pos + box center pos (model has pos in link origin, box has zero at center)
        o0 = fcl.CollisionObject(b0, transform.Transform(rot0, pos0+b0_center))
        o1 = fcl.CollisionObject(b1, transform.Transform(rot1, pos1+b1_center))

        distance, d_result = fcl.distance(o0, o1, collision_data.DistanceRequest(True))

        if distance < 0:
            if self.config['verbose'] > 1:
                print("Collision of {} and {}".format(l0_name, l1_name))

            # get proper collision and depth since optimization should also know how much constraint is violated
            cr = collision_data.CollisionRequest()
            cr.enable_contact = True
            cr.enable_cost = True
            collision, c_result = fcl.collide(o0, o1, cr)

            # sometimes no contact is found even though distance is less than 0?
            if len(c_result.contacts):
                distance = c_result.contacts[0].penetration_depth

        return distance
コード例 #10
0
ファイル: example.py プロジェクト: jf---/python-fcl
def cb_func(obj1, obj2, res):
    print "cb_func start"
    ret, res = fcl.collide(obj1, obj2, fcl.CollisionRequest())
    print "result: ", ret
    return ret
コード例 #11
0
ファイル: example.py プロジェクト: jf---/python-fcl
manager.registerObject(fcl.CollisionObject(fcl.Cylinder(7.0, 8.0)))
print "After register 2 : ", manager.size()

# Use Callback function
def cb_func(obj1, obj2, res):
    print "cb_func start"
    ret, res = fcl.collide(obj1, obj2, fcl.CollisionRequest())
    print "result: ", ret
    return ret
res = fcl.CollisionResult()
manager.collide(res, cb_func)

# Collision calcuration
ret, result = fcl.collide(fcl.CollisionObject(fcl.Box(1.0, 2.0, 3.0),
                                              transform.Transform(transform.Quaternion(), [10.0, 0.0, 0.0])),
                          fcl.CollisionObject(fcl.Sphere(4.0),
                                              transform.Transform(transform.Quaternion(), [-10.0, 0.0, 0.0])),
                          fcl.CollisionRequest())

print "-- Collision result: ", ret
for contact in result.contacts:
    print contact.o1
    print contact.o2
for cost_source in result.cost_sources:
    print cost_source

dis, result = fcl.distance(fcl.CollisionObject(fcl.Box(1.0, 2.0, 3.0),
                                               transform.Transform(transform.Quaternion(), [10.0, 0.0, 0.0])),
                           fcl.CollisionObject(fcl.Sphere(4.0),
                                               transform.Transform(transform.Quaternion(), [-10.0, 0.0, 0.0])),
                           fcl.DistanceRequest(True))
コード例 #12
0
ファイル: fcl_unit_test.py プロジェクト: neka-nat/python-fcl
 def collide_callback_func(obj1, obj2, res):
     ret, res = fcl.collide(obj1, obj2, fcl.CollisionRequest())
     return ret
コード例 #13
0
ファイル: example.py プロジェクト: kiwiwan/python-fcl
def cb_func(obj1, obj2, res):
    print "cb_func start"
    ret, res = fcl.collide(obj1, obj2, fcl.CollisionRequest())
    print "result: ", ret
    return ret
コード例 #14
0
ファイル: example.py プロジェクト: kiwiwan/python-fcl
# Use Callback function
def cb_func(obj1, obj2, res):
    print "cb_func start"
    ret, res = fcl.collide(obj1, obj2, fcl.CollisionRequest())
    print "result: ", ret
    return ret


res = fcl.CollisionResult()
manager.collide(res, cb_func)

# Collision calcuration
ret, result = fcl.collide(
    fcl.CollisionObject(
        fcl.Box(1.0, 2.0, 3.0),
        transform.Transform(transform.Quaternion(), [10.0, 0.0, 0.0])),
    fcl.CollisionObject(
        fcl.Sphere(4.0),
        transform.Transform(transform.Quaternion(), [-10.0, 0.0, 0.0])),
    fcl.CollisionRequest())

print "-- Collision result: ", ret
for contact in result.contacts:
    print contact.o1
    print contact.o2
for cost_source in result.cost_sources:
    print cost_source

dis, result = fcl.distance(
    fcl.CollisionObject(
        fcl.Box(1.0, 2.0, 3.0),
        transform.Transform(transform.Quaternion(), [10.0, 0.0, 0.0])),