动手学运动规划:1.4.c 碰撞检测算法:GJK代码解析

最新推荐文章于 2024-10-16 00:00:43 发布

自动驾驶小白说

最新推荐文章于 2024-10-16 00:00:43 发布

阅读量791

点赞数 21

分类专栏：动手学运动规划文章标签：算法自动驾驶

本文链接：https://blog.csdn.net/2403_86993842/article/details/142376122

版权

动手学运动规划专栏收录该内容

20 篇文章 0 订阅

订阅专栏

你我来自湖北四川广西宁夏河南山东贵州云南的小镇乡村, 曾经发誓要做了不起的人.

*—你曾是少年 S.H.E*

🏰代码及环境配置：请参考0.2 环境配置和代码运行 | 动手学运动规划!

本节提供了GJK算法的代码测试.

python3 tests/basic/gjk_collision_check_test.py

1.4.c.1 GJK代码实现

tests/basic/gjk.py 提供了GJK算法的实现,该实现不仅仅返回碰撞结果,还计算了最小距离.

这里我们介绍一下主流程run_gjk() ,他接受两个二维凸集和一个初始方向作为输入.

首先基于初始方向计算了shape1, shape2两相反方向上的边界点, 然后相减得到Minoski差集合的边界点的向量v( $\overrightarrow{OP_p}$ ).
利用向量v,计算Minoski差集合边界上的第一个单纯形点w
初始化单纯形S, S1和S2分别是单纯形S对应的 shape1和shape2上的原始点,因为算法需要计算最小距离
进入循环,停止条件代表当前最小距离的v.dot(v) > 代表新的最小距离的v.dot(w), 也就是无法找到更小距离的点了
循环内find_first_unset使用了数学技巧来找b中第一个0的位置索引
我们可视化了每一次迭代的w,a,b点

def run_gjk(shape1, shape2, a0=np.array([1, 0]), eps=0.001):
    """Gilbert-Johnson-Keerthi distance algorithm.
    Parameters:
        shape1, shape2: shapes to calculate distance between
        a0: arbitrary initial direction
    Returns:
        Distance between the shapes (0 if they intersect),
        points v1, v2 representing the closest points on the first and
        second shapes, respectively."""
    # arbitrary point in Minoski diff
    v = shape1.support(a0) - shape2.support(-a0)

    # direction to origin from closest point
    a = shape1.support(-v)
    b = shape2.support(v)
    w = a - b

    # initialize simplex
    S = np.zeros((2, 3))
    bitset = 0

    # to compute the closest points on the shapes, in addition to the distance,
    # we need to keep track of the points from each shape used to compute the
    # simplex
    S1 = np.zeros((2, 3))
    S2 = np.zeros((2, 3))

    # iterate until w is no more extreme in the direction v than v itself
    while v.dot(v) - v.dot(w) > eps:
        # add new point to simplex
        i = find_first_unset(bitset)
        bitset = set_bit(bitset, i)
        S[:, i] = w
        S1[:, i] = a
        S2[:, i] = b

        # compute closest point to the origin in the simplex, as well the
        # smallest simplex that supports that point
        v, bitset, contains_origin, coeffs = johnson(S, bitset)

        if contains_origin:
            break

        # recompute direction
        a = shape1.support(-v)
        b = shape2.support(v)
        w = a - b

    # compute closest points using the same coefficients as the closest point
    # of the Minowski diff to the origin v
    idx = bit_indices(bitset)
    v1 = coeffs.dot(S1[:, idx].T)
    v2 = coeffs.dot(S2[:, idx].T)

    distance = np.sqrt(v.dot(v))

    return distance, v1, v2

循环中调用了johnson算法来计算单纯形上与原点的最近距离的点,代码如下:

def johnson(points, this_bitset):
    """Johnson distance algorithm
    Parameters:
        points: 2x3 matrix of points in the simplex
        this_bitset: 3-bit number representing the active members of the
            simplex
    Returns:
        v: the closest point of this simplex to the origin
        bitset: the bitset of the smallest sub-simplex supporting v
        contains_origin: True if the sub-simplex contains the origin
        coeffs: Coefficients for the points in the sub-simplex to v via
            convex combination"""
    # store deltas: row is indexed by the points in the set X; column indexed
    # by delta index
    # {y1} represented by 001, {y2} by 010, etc
    D = np.zeros((8, 3))

    # singleton sets are unity
    D[0b001, 0] = 1
    D[0b010, 1] = 1
    D[0b100, 2] = 1

    # calculate deltas
    # this implementation requires bitset array to be ordered in increasing
    # number of bits
    for bitset in BITSETS:
        if bitset & this_bitset != bitset:
            continue

        for i in range(3):
            bitset_no_i = unset_bit(bitset, i)
            if bitset_no_i != 0 and get_bit(bitset, i):
                k = find_first_set(bitset_no_i)
                yk = points[:, k]
                yi = points[:, i]
                D[bitset, i] = np.sum(
                    [
                        D[bitset_no_i, j] * (yk - yi).dot(points[:, j])
                        for j in bit_indices(bitset_no_i)
                    ]
                )

    for bitset in BITSETS:
        # only iterate bit sets that contain members of the current bitset
        if bitset & this_bitset != bitset:
            continue

        # check conditions for this bit set to contain the closest point
        contains_closest_pt = True
        for j in range(3):
            if get_bit(bitset, j) and D[bitset, j] <= 0:
                contains_closest_pt = False
            elif not get_bit(bitset, j) and D[set_bit(bitset, j), j] > 0:
                contains_closest_pt = False

        if contains_closest_pt:
            idx = bit_indices(bitset)
            coeffs = D[bitset, idx] / np.sum(D[bitset, idx])
            v = coeffs.dot(points[:, idx].T)
            contains_origin = bitset == 0b111
            return v, bitset, contains_origin, coeffs

1.4.c.1 GJK代码测试

在tests/basic/gjk_collision_check_test.py 中,生成了随机的矩形和圆,调用gjk算法来计算最小距离和对应的点

    for i in range(0, 10):
        plt.cla()
        # test shapes
        x1 = random.uniform(0.0, 10.0)
        y1 = random.uniform(0.0, 10.0)
        theta1 = random.uniform(-pi, pi)
        length1 = random.uniform(2.0, 4.0)
        width1 = random.uniform(1.0, 3.0)
        box1 = Box(x1, y1, theta1, length1, width1)
        shape1 = gjk.Polygon(get_xy_matrix(box1.corners))

        x2 = random.uniform(0.0, 10.0)
        y2 = random.uniform(0.0, 10.0)
        r2 = random.uniform(1.0, 3.0)
        shape2 = gjk.Circle(c=(x2, y2), r=r2)

        # calculate distance and closest points on shapes
        d, v1, v2 = gjk.run_gjk(shape1, shape2)
        print(d)