《机器人学一(Robotics(1))》_台大林沛群 第4周 Quiz4

前两题主要是细心观察即可，第三题主要是使用勾股定理以及简单的反三角函数求解即可，长度与角度答案分别为80和30，不作赘述，主要阐述从第四题开始的解题过程。

P4.

根据上述推导过程编写代码如下:

import numpy as np
theta1 = np.pi * (-60) / 180
r1 = np.array([[np.cos(theta1), 0, np.sin(theta1)],
               [0, 1, 0],
               [-np.sin(theta1), 0, np.cos(theta1)]])
p1 = np.array([-500, 452, 410])
t1 = RP_Transformation_Matrix(r1, p1.reshape(3, 1))

r2 = np.array([[1, 0, 0],
               [0, 1, 0],
               [0, 0, 1]])
p2 = np.array([830, 20, 330])
t2 = RP_Transformation_Matrix(r2, p2.reshape(3, 1))

t = np.dot(t2, t1)
t = [[float(format(x, '.3g')) for x in t[i]] for i in range(len(t))]  # 保留 3位有效数字`

得到答案为 : 0.5//-0.866//0.866//330//472//740

P5 - P7.

解题代码如下：

# pieper's solution
# formula : p_04 = p_06 = t_01 * t_12 * t_23 * p _34
# p_04 = p_06 = t_06[:,-1]
t_wc = t
t_cw = np.linalg.inv(t_wc)
p_cw = t_cw[:, -1]
t_6c = np.array([[0, 0, 1, 0],
                 [0, -1, 0, 0],
                 [1, 0, 0, 206],
                 [0, 0, 0, 1]])
t_0w = np.array([[1, 0, 0, 0],
                 [0, 1, 0, 0],
                 [0, 0, 1, 373],
                 [0, 0, 0, 1]])
t_06 = np.dot(np.dot(np.linalg.inv(t_0w), t_wc), np.linalg.inv(t_6c))
print(t_06)
p_04 = t_06[:, -1]
print(p_04)

到这里求出了p_04，即pieper’s solution核心公式左侧的已知量，接下来依次计算theta角即可：
代码运行结果如下：

接下来利用sympy包求解theta3：

from sympy import *
# 计算theta3
theta3 = Symbol('theta3')
x, y, z = p_04[0], p_04[1], p_04[2]
r = x**2 + y**2 + z**2

k1 = a3 * cos(theta3) + d4 * sin(alpha3) * sin(theta3) + a2
k2 = -(a3 * cos(alpha2) * sin(theta3) - d4 * sin(alpha3) * cos(alpha2) * cos(theta3)
       - d4 * sin(alpha2) * cos(alpha3) - d3 * sin(alpha2))
f3 = a3 * sin(alpha2) * sin(theta3) - d4 * sin(alpha3) * sin(alpha2) * cos(theta3)\
     + d4 * cos(alpha2) * cos(alpha3) + d3 * cos(alpha2)
k3 = k1**2 + k2**2 + f3**2 + a1**2 + d2**2 + 2 * d2 * f3
k4 = f3 * cos(alpha1) + d2 * cos(alpha1)

f1 = ((r - k3)/(2 * a1)) ** 2 + ((z - k4) /sin(alpha1))**2 - k1**2 - k2**2
ans = solve([f1], ['theta3'])
ans = [ans[i][0] for i in range(len(ans))]
ans_angle = [180 * ans[i]/ np.pi for i in range(len(ans)) if ans[i]>= -180.0 and ans[i]<= 180.0]
# 根据题目对theta3的范围筛选出角度
print("可能的theta3值有:",  ans_angle)
print(ans)

代码运行结果如下：

题干提醒是双解，说明后续角度对theta3也会有约束，所以这里先不进行角度的选择，而是继续根据公式求解theta2角的求解。可以看到theta2需要同时满足r,z两个条件的要求，为了快速计算，分别对r, 和z约束条件下的theta2进行求解，最后判断其解。

# 计算theta2
theta3 = -2.76034628749053
k1 = a3 * cos(theta3) + d4 * sin(alpha3) * sin(theta3) + a2
k2 = -(a3 * cos(alpha2) * sin(theta3) - d4 * sin(alpha3) * cos(alpha2) * cos(theta3)
       - d4 * sin(alpha2) * cos(alpha3) - d3 * sin(alpha2))
f3 = a3 * sin(alpha2) * sin(theta3) - d4 * sin(alpha3) * sin(alpha2) * cos(theta3)\
     + d4 * cos(alpha2) * cos(alpha3) + d3 * cos(alpha2)
k3 = k1**2 + k2**2 + f3**2 + a1**2 + d2**2 + 2 * d2 * f3
k4 = f3 * cos(alpha1) + d2 * cos(alpha1)

theta2 = Symbol('theta2')
# f2 = r - (2 * a1 * (k1 * cos(theta2) + k2 * sin(theta2)) + k3)
f2 = z - ((k1 * sin(theta2) - k2 * cos(theta2)) * sin(alpha1) + k4)
ans2 = solve([f2], ['theta2'])
ans2 = [ans2[i][0] for i in range(len(ans2))]
ans2_angle = [180 * ans2[i] / np.pi for i in range(len(ans2)) if ans2[i]>= -180.0 and ans2[i]<= 180.0]
print("可能的theta2值有：", ans2_angle)
print(ans2)

改换四个theta3的值，代码运行结果归纳如下：

theta2的可能值

• theta3 == -3.05085107023123 (-174)

# 使用r条件
[-161.855697915037, -120.043627174960]  # angle
[-2.82491484173072, -2.09515654023959]  # radius

# 使用z条件
[-120.043627174960, 18.1443020849632]
[-2.09515654023959, 0.316677811859079]

所以该条件下，theta2 = -120.043627174960 (-2.09515654023959)

• theta3 == -2.76034628749053 (-158)

# 使用r条件
[11.9128703903189, 49.5300174395328]
[0.207918811674406, 0.864461882890060]

# 使用z条件
[-130.469982560467, 11.9128703903189]
[-2.27713077069973, 0.207918811674406]

所以该条件下，theta2 = 11.9128703903189 (0.207918811674406)

• theta3 == -0.616837009840437 (-35)

# 使用r条件
[-49.5300174395328, -11.9128703903189]
[-0.864461882890060, -0.207918811674406]

# 使用z条件
[-49.5300174395327, 168.087129609681]
[-0.864461882890058, 2.93367384191539]

所以该条件下，theta2 = -49.5300174395327 (-0.864461882890058)

• theta3 == -0.326332227099740 (-18)

# 使用r条件
[120.043627174961, 161.855697915037]
[2.09515654023960, 2.82491484173071]

# 使用z条件
[-59.9563728250397, 161.855697915037]
[-1.04643611335020, 2.82491484173072]

所以该条件下，theta2 = 161.855697915037 (2.82491484173072)。
通过上述分析，将theta3和theta2的可能值都缩小到四个可能角度，其结果如下：

theta3和theta2的可能值

theta3 = [-3.05085107023123, -2.76034628749053, -0.616837009840436, -0.326332227099739]
theta2 = [-2.09515654023959, 0.207918811674406,  -0.864461882890058, 2.82491484173072]

接下来继续计算theta1值，与theta2情况相同，theta1受x, y 两个条件的限制，为了提高计算效率，分别在x和y条件下进行求解，最后归纳可能得theta1, theta2, theta3值。

# 计算theta1
ans = [-3.05085107023123, -2.76034628749053, -0.616837009840436, -0.326332227099739]
ans2 = [-2.09515654023959, 0.207918811674406,  -0.864461882890058, 2.82491484173072]

theta3 = ans[2]
theta2 = ans2[2]
f1 = a3 * cos(theta3) + d4 * sin(alpha3) * sin(theta3) + a2
f2 = a3 * cos(alpha2) * sin(theta3) - d4 * sin(alpha3) * cos(alpha2) * cos(theta3) \
     - d4 * sin(alpha2) * cos(alpha3) - d3 * sin(alpha2)
f3 = a3 * sin(alpha2) * sin(theta3) - d4 * sin(alpha3) * sin(alpha2) * cos(theta3) \
     + d4 * cos(alpha2) * cos(alpha3) + d3 * cos(alpha2)
# f1 = f1(theta3, a2, a3, alpha3, d4)
# print(f1)
g1 = cos(theta2) * f1 - sin(theta2) * f2 + a1
g2 = sin(theta2) * cos(alpha1) * f1 + cos(theta2) * cos(alpha1) * f2 - sin(alpha1) * f3 - d2 * sin(alpha1)


theta1 = Symbol('theta1')
# f3 = x - (cos(theta1) * g1 - sin(theta1) * g2)
f3 = y - (sin(theta1) * g1 + cos(theta1) * g2)
ans3 = solve([f3], [theta1])
ans3 = [ans3[i][0] for i in range(len(ans3))]
ans3_angle = [180 * ans3[i] / np.pi for i in range(len(ans3))]
print("可能的theta1值有：", ans3_angle)
print(ans3)

最后求得的结果归纳如下：

• theta3 = -174, theta2 = -120

# 使用x条件
[-115.684399755326, 115.684399755326]
[-2.01907366892376, 2.01907366892376]

# 使用y条件

• theta3 = -158, theta2 = 12

# 使用x条件
[-64.3156002446744, 64.3156002446744]
[-1.12251898466604, 1.12251898466604]

# 使用y条件
[64.3156002446746, 115.684399755325]
[1.12251898466604, 2.01907366892375]

得出可能的theta1 = 64

• theta3 = -35, theta2 = -50

# 使用x条件
[-64.3156002446745, 64.3156002446745]
[-1.12251898466604, 1.12251898466604]

# 使用y条件
[64.3156002446744, 115.684399755326]
[1.12251898466604, 2.01907366892375]

得出的可能theta1 = 64

• theta3 = -18, theta2 = 162

# 使用x条件
[-115.684399755326, 115.684399755326]
[-2.01907366892375, 2.01907366892375]

# 使用y条件

这里得出，可能的(theta3, theta2) = (-158, 12) / (-35, -50) 可能的theta1值为64，求解完毕。

p5	p6	p7
-158//-35	12//-50	64

P8.

代码求解如下：

from week03 import link_transformations as lt
import numpy as np
import math as mt

# link_transformation_function (alpha, a, theta, d)

# 先求出R_04 = R_01 * R_12 * R_23 * R_34
# 这里使用(theta3, theta2) = (-158, 12) / (-35, -50)
R_01 = lt.link_transformation_function(0, 0, 64, 0)
R_12 = lt.link_transformation_function(-90, -30, -50, 0)
R_23 = lt.link_transformation_function(0, 340, -35, 0)
alpha = np.pi * (-90) / 180
R_3_rotation_x = [[1, 0, 0, 0],
                  [0, np.cos(alpha), -np.sin(alpha), 0],
                  [0, np.sin(alpha), np.cos(alpha), 0],
                  [0, 0, 0, 1]]
R = np.dot(R_01, R_12)
R = np.dot(R, R_23)
R = np.dot(R, R_3_rotation_x)
R_03 = R[:3, :3]
print("R_03:\n", R_03)

# R_06已知
R_06 = [[-0.866, 0, 0.5],
        [0, -1, 0],
        [0.5, 0, 0.866]]

# 求取R_36
R_36 = np.dot(np.linalg.inv(R_03), R_06)
print(R_36)

# 利用Z-Y-Z求取theta4, theta5, theta6
r13 = R_36[0][2]
r23 = R_36[1][2]
r31 = R_36[2][0]
r32 = R_36[2][1]
r33 = R_36[2][2]

# 已知theta5可以有正负两解，分别求出两组解
theta5 = -mt.atan2(mt.sqrt(r31**2 + r32**2), r33)
theta4 = mt.atan2(r23 / mt.sin(theta5), r13 / mt.sin(theta5))
theta6 = mt.atan2(r32 / mt.sin(theta5), -r31 / mt.sin(theta5))

theta4_angle = theta4 * 180 / np.pi + 180
theta5_angle = theta5 * 180 / np.pi
theta6_angle = theta6 * 180 / np.pi + 180
print("第一组解：", theta4_angle, theta5_angle, theta6_angle)
print("第一组解：", theta4_angle, theta5_angle, theta6_angle-360)


theta5 = mt.atan2(mt.sqrt(r31**2 + r32**2), r33)
theta4 = mt.atan2(r23 / mt.sin(theta5), r13 / mt.sin(theta5))
theta6 = mt.atan2(r32 / mt.sin(theta5), -r31 / mt.sin(theta5))

theta4_angle = theta4 * 180 / np.pi + 180
theta5_angle = theta5 * 180 / np.pi
theta6_angle = theta6 * 180 / np.pi + 180
print("第二组解：", theta4_angle, theta5_angle, theta6_angle)
print("第二组解：", theta4_angle - 360, theta5_angle, theta6_angle - 360)

在求解时，T_03是按照D-H表达法的逻辑去求得的，而最后的T_34并不是实际的T_34，而是经过处理后符合“Z-Y-Z”旋转规范的T_34。
代码运行结果如下：

实际上最后可以看到，最后只求得一组有效的theta解，即
(theta1, theta2, theta3, theta4, theta5, theta6) = (64, -50, -35, 27, -82, -65)，求解完成。