机械臂雅可比矩阵导数（未验证）

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

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

总结

没啥总结的，不保证对就是了。