demo_pendulum.py 文件源码

def erf(qgoal, q):
    """
    Returns error e given two states qgoal and xq.

    """
    e = qgoal - q
    for i in [0, 1]:
        c = np.cos(q[i])
        s = np.sin(q[i])
        cg = np.cos(qgoal[i])
        sg = np.sin(qgoal[i])
        e[i] = np.arctan2(sg*c - cg*s, cg*c + sg*s)
    return e

################################################# OBJECTIVES AND CONSTRAINTS

# What is goal