提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档
前言
GitHub上边抄的,看了几个算法是一样的,自己没有验证就拿来用了,不保证正确。
一、求解方法
先求hessian矩阵,然后拿速度迭代,就算好了orz
二、代码
代码如下:
def hessian0(self, q, J0):
n=6
# q=None J0=None
def cross(a,b):
x = a[1]*b[2]-a[2]*b[1]
y = a[2]*b[0]-a[0]*b[2]
z = a[0]*b[1]-a[1]*b[0]
return np.array([x, y, z])
H=np.zeros((6, n, n))
for j in range(n):
for i in range(j,n):
H[: 3, i, j] = np.cross(J0[3:, j],J0[: 3, i])
H[3:, i, j] = np.cross(J0[3:, j],J0[3:, i])
if i!=j:
H[:3, j, i] = H[:3, i, j]
return(H)
def J_dot(self, dq, H):
n = 6
Jd = np.zeros((6,n))
for i in range(n):
Jd += H[:, :, i]*dq
return Jd
总结
没啥总结的,不保证对就是了。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)