demo_pendulum.py 文件源码

python
阅读 44 收藏 0 点赞 0 评论 0

项目:lqRRT 作者: jnez71 项目源码 文件源码
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
评论列表
文章目录


问题


面经


文章

微信
公众号

扫码关注公众号