python - Baxter 的正向运动学

Question

我已经根据它的 hardware specs 为 Baxter ARM 机器人组合了这个正向运动学函数。以及以下关节轴:
以下正向运动学的关节位置与相应的笛卡尔坐标不匹配，我在这里做错了什么？

def FK_function_2(joints):
    def yaw(theta): #(rotation around z)
        y = np.array([[np.cos(theta), -np.sin(theta), 0],
                      [np.sin(theta), np.cos(theta), 0],
                      [0, 0, 1] ])
        return y

    R01 = yaw(joints[0]).dot(np.array([[-1,     0,   0],
                                       [0,      0,   1],
                                       [0,      1,   0]]))
    R12 = yaw(joints[1]).dot(np.array([[0,      0,   -1],
                                       [-1,     0,   0],
                                       [0,      1,   0]]))
    R23 = yaw(joints[2]).dot(np.array([[-1,     0,   0],
                                       [0,      0,   1],
                                       [0,      1,   0]]))
    R34 = yaw(joints[3]).dot(np.array([[-1,     0,   0],
                                       [0,      0,   1],
                                       [0,      1,   0]]))
    R45 = yaw(joints[4]).dot(np.array([[-1,     0,   0],
                                       [0,      0,   1],
                                       [0,      1,   0]]))
    R56 = yaw(joints[5]).dot(np.array([[-1,     0,   0],
                                       [0,      0,   1],
                                       [0,      1,   0]]))
    R67 = yaw(joints[6]).dot(np.array([[1,      0,   0],
                                       [0,      1,   0],
                                       [0,      0,   1]]))

    d = np.array([0.27035, 0, 0.36435, 0, 0.37429, 0, 0.229525])
    a = np.array([0.069, 0, 0.069, 0, 0.010, 0, 0])

    l1 = np.array([a[0]*np.cos(joints[0]), a[0]*np.sin(joints[0]), d[0]]);
    l2 = np.array([a[1]*np.cos(joints[1]), a[1]*np.sin(joints[1]), d[1]]); 
    l3 = np.array([a[2]*np.cos(joints[2]), a[2]*np.sin(joints[2]), d[2]]); 
    l4 = np.array([a[3]*np.cos(joints[3]), a[3]*np.sin(joints[3]), d[3]]); 
    l5 = np.array([a[4]*np.cos(joints[4]), a[4]*np.sin(joints[4]), d[4]]);
    l6 = np.array([a[5]*np.cos(joints[5]), a[5]*np.sin(joints[5]), d[5]]);
    l7 = np.array([a[6]*np.cos(joints[6]), a[6]*np.sin(joints[6]), d[6]]);

    unit = np.array([0, 0, 0, 1])
    H0 = np.concatenate((np.concatenate((R01, l1.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
    H1 = np.concatenate((np.concatenate((R12, l2.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
    H2 = np.concatenate((np.concatenate((R23, l3.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
    H3 = np.concatenate((np.concatenate((R34, l4.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
    H4 = np.concatenate((np.concatenate((R45, l5.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
    H5 = np.concatenate((np.concatenate((R56, l6.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
    H6 = np.concatenate((np.concatenate((R67, l7.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)


    T = H0.dot(H1).dot(H2).dot(H3).dot(H4).dot(H5).dot(H6)

    return T[0:3, 3]

Answer 1

好的，所以我一直在看这个并检查了你的代码。该代码很好，可与您定义的运动链配合使用，并从机械臂的底部到末端进行转换。

(H0 * H1 * H2 * H3 * H4 * H5 * H6) 是正确的运动链，其中每个都代表从 ARM 底部开始的链中从一个关节到下一个关节的转换。

问题是你的转换是错误的。您对 H0 到 H6 的表示不正确，正是这些矩阵中的数字导致您的转换与发生的实际转换不匹配。您需要从底部一直到 ARM 末端进行正确的转换。除此之外，你的方法是正确的。

看起来您正在为转换矩阵使用正常的 DH 参数。您的 a 和 d(以及未在您的代码中显示的 alpha)的值已关闭并导致转换的表达不正确。 DH 参数见 https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters .

在浏览修改后的 DH 表以设置转换后，我找到了 Baxter 正向运动学的精确指南以提供帮助。我会在上面的 wiki 文章末尾查看修改后的 DH 参数，因为该指南使用了该参数。

百特正向运动学指南: https://www.ohio.edu/mechanical-faculty/williams/html/pdf/BaxterKinematics.pdf

在本文中，作者 Robert Williams 为 Baxter 机械臂设置了 DH 参数，并获得了与您所拥有的值不同的值(我知道您使用的是正常的 DH 参数，但我会考虑使用修改后的参数)。他的表是:



长度为:



并使用修改后的 DH 矩阵:



现在您可以计算矩阵 H0 到 H6，如果需要，您还可以添加末端效应器几何形状，如果您有额外的 H7。一旦将它们全部组合在一起，您应该获得正确的正向运动学转换(有关其他资源，请参阅论文)。左右臂具有相同的运动学。

当你将它们相乘时，你会得到 x7、y7 和 z7 坐标的表达式，从 ARM 的底部开始，这些表达式是关节旋转和机器人 ARM 几何形状的函数。有关 x7、y7 和 z7 的表达式，请参阅第 17 页上的论文。另请参阅第 14 页了解各个转换。

另外不要忘记以弧度表示角度，因为您的代码使用常规三角函数。

最后更新:
我只记得我更容易一个一个地思考中间平移和旋转步骤(而不是直接跳到 DH 矩阵)。这两种方法是等效的，但我喜欢考虑从一个旋转框架到下一个旋转框架所需的每个单独步骤。

为此，您可以使用这些构建块。

纯译:

[1   0   0   u;
0    1   0   v;
0    0   1   w;
0    0   0    1]

其中 u 是从前一个 x 帧轴测量的从前一帧到新帧的距离。

其中 v 是从前一个 y 帧轴测量的从前一帧到新帧的距离。

其中 w 是从前一帧到从前一个 z 帧轴测量的新帧的距离。

以任意 theta 绕 z 轴旋转:
这表示机器人关节旋转到任意θ。

[cos(theta)    -sin(theta)        0 0;
sin(theta)     cos(theta)         0 0;
0                   0             1 0;
0                   0             0 1]

围绕中间帧的旋转组合以获得最终帧位置:(这些角度通常以 pi/2 或 pi 为增量，以便能够到达最终方向)
可以使用围绕中间 x 轴、y 轴或 z 轴的旋转，如下所示。

(按 alpha 绕 x 轴旋转)

R_x(alpha) =         [1             0           0              0;
                      0         cos(alpha)  -sin(alpha)        0;
                      0         sin(alpha)  cos(alpha)         0;
                      0            0            0              1];

(通过 beta 绕 y 轴旋转)

R_y(beta) =   [  cos(beta)     0      sin(beta)    0;
                     0         1          0        0;
                 -sin(beta)    0      cos(beta)    0;
                     0         0          0        1];

(通过 Gamma 绕 z 轴旋转):

[cos(gamma)  -sin(gamma)     0      0;
sin(gamma)    cos(gamma)     0      0;
       0          0          1      0;
       0          0          0      1]

因此，通过这些构建块，您可以构建从一帧到另一帧的步骤序列(基本上任何 H 矩阵都可以分解为这些步骤)。该链将类似于:

[H](从前一帧到下一帧的变换)= [从前一个关节到前一个关节帧中表达的新关节的纯平移] * [关于前一帧的 z 轴旋转 theta(对于关节)(由于关节有许多位置，theta 保留为符号)] * [所有其他中间旋转到达新的关节框架方向，表示为围绕中间轴框架的旋转]

这基本上是 DH 参数可以帮助您做的事情，但我喜欢考虑从一帧到下一帧的各个步骤，而不是使用 DH 参数跳到那里。

使用更正的 H0 到 H6 转换，您的方法是正确的。只需在代码中更改 H0 到 H6 的定义即可。