demo_boat_advanced.py 文件源码

def is_feasible(x, u):
    # Reject going too fast
    for i, v in enumerate(x[3:]):
        if v > velmax_pos_plan[i] or v < velmax_neg_plan[i]:
            return False
    # Body to world
    R = np.array([
                  [np.cos(x[2]), -np.sin(x[2])],
                  [np.sin(x[2]),  np.cos(x[2])],
                ])
    # Boat vertices in world frame
    verts = x[:2] + R.dot(vps).T
    # Check for collisions over all obstacles
    for ob in obs:
        if np.any(npl.norm(verts - ob[:2], axis=1) <= ob[2]):
            return False
    return True

################################################# HEURISTICS