Baxter的正向运动学
Mas*_*nya 7 python robotics kinematics
我根据其硬件规格和以下关节轴将Baxter臂机器人的正向运动学功能放在一起:
以下正向运动学的关节位置与相应的笛卡尔坐标不匹配,我在这里做错了什么？
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]
好的，所以我一直在看这个并检查了你的代码。该代码很好，可以与您定义的运动链配合使用，实现从机械臂底座到末端的转换。
(H0 * H1 * H2 * H3 * H4 * H5 * H6) 是正确的运动链，其中每个运动链代表从手臂基部开始的链中从一个关节到下一个关节的转换。
问题是你的转换是错误的。您对 H0 到 H6 的表示不正确，正是这些矩阵中的数字导致您的转换与实际发生的转换不匹配。您需要从底部一直到手臂末端进行正确的转换。除此之外，你的方法是正确的。
看起来您正在为变换矩阵使用正常的 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）。一旦将它们全部组合在一起，您应该获得正确的正向运动学变换（请参阅论文以获取其他资源）。左臂和右臂具有相同的运动学。
When you multiply it all together then you get the expressions for the coordinates of x7, y7, and z7 from the base of the arm that are functions of the rotations of the joints and the geometry of the robot arm. See the paper on page 17 for the expressions for x7,y7, and z7. Also see page 14 for the individual transformations.
Also don't forget to express the angles in radians since your code uses regular trig functions.
One last update: I just remembered that it is easier for me to think of the intermediate translation and rotational steps one-by-one (instead of jumping straight to the DH matrix). The two approaches will be equivalent, but I like to think of each individual step that it takes to get from one rotation frame to the next.
For this you can use these building blocks.
Pure Translation:
[1 0 0 u;
0 1 0 v;
0 0 1 w;
0 0 0 1]
Where u is the distance from the previous frame to the new frame measured from the previous x frame axis.
Where v is the distance from the previous frame to the new frame measured from the previous y frame axis.
Where w is the distance from the previous frame to the new frame measured from the previous z frame axis.
Rotation about the z-axis by arbitrary theta: This represent the robot joint rotation to an arbitrary theta.
[cos(theta) -sin(theta) 0 0;
sin(theta) cos(theta) 0 0;
0 0 1 0;
0 0 0 1]
Combination of rotations around intermediate frames to get to final frame position: (these angles will usually be in increments of pi/2 or pi to be able to get to the final orientation) Can use a rotation about the intermediate x axis, y axis, or z axis shown below.
(Rotation about x axis by alpha)
R_x(alpha) = [1 0 0 0;
0 cos(alpha) -sin(alpha) 0;
0 sin(alpha) cos(alpha) 0;
0 0 0 1];
(Rotation about y axis by beta)
R_y(beta) = [ cos(beta) 0 sin(beta) 0;
0 1 0 0;
-sin(beta) 0 cos(beta) 0;
0 0 0 1];
(Rotation about z axis by gamma):
[cos(gamma) -sin(gamma) 0 0;
sin(gamma) cos(gamma) 0 0;
0 0 1 0;
0 0 0 1]
So with these building blocks you can build the sequence of steps to go from one frame to another (essentially any H matrix can be decomposed into these steps). The chain would be something like:
[H](transformation from previous frame to the next frame) = [Pure Translation from previous joint to new joint expressed in the previous joint's frame] * [Rotation about previous frame's z-axis by theta (for the joint) (since the joint has many positions, theta is left as symbolic)] * [All other intermediate rotations to arrive at the new joint frame orientation expressed as rotations about intermediate axis frames]
That is essentially what the DH parameters helps you do, but I like to think of the individual steps to get from one frame to the next instead of jumping there with the DH parameters.
With the corrected H0 through H6 transformations, your approach is correct. Just change the definitions of H0 through H6 in your code.