demo_boat_advanced.py 文件源码

def erf(xgoal, x):
    """
    Returns error e given two states xgoal and x.
    Angle differences are taken properly on SO3.

    """
    e = xgoal - x
    c = np.cos(x[2])
    s = np.sin(x[2])
    cg = np.cos(xgoal[2])
    sg = np.sin(xgoal[2])
    e[2] = np.arctan2(sg*c - cg*s, cg*c + sg*s)
    return e

################################################# OBJECTIVES

# Initial condition and goal