Correct quadrotor collision distance calculation.

[cbuglino]

There appears to be a typo in the calculation of the quadrotor distance in check_collision(). The square root seems to be missing. I don't have permissions to push a branch / pull request, but I think the function definition can be changed to use np.linalg.norm with slightly more pragmatic indexing.

def check_collision(state):
    """
    This function checks if the state is colliding with the obstacles
    
    Inputs:
    state: the current state of the quadrotor as a numpy array (x,vx,y,vy,theta,omega)
    
    Output:
    a boolean value. True if there is contact and False otherwise.
    """
    dist1 = np.linalg.norm(
        state[[0, 2]] - OBSTACLE_CENTERS[0][[0, 1]], ord=2
    )
    dist2 = np.linalg.norm(
        state[[0, 2]] - OBSTACLE_CENTERS[1][[0, 1]], ord=2
    )
    dist3 = np.linalg.norm(
        state[[0, 2]] - OBSTACLE_CENTERS[2][[0, 1]], ord=2
    )
    if dist1 < OBSTACLE_RADIUS + LENGTH:
        return True
    elif dist2 < OBSTACLE_RADIUS + LENGTH:
        return True
    elif dist3 < OBSTACLE_RADIUS + LENGTH:
        return True
    else:
        return False