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
评论列表
文章目录