《机器人学一(Robotics(1))》_台大林沛群 第 3 周 【机械手臂 顺运动学】Quiz 3
coursera课程链接
高手在课程论坛
课程论坛链接
理论知识部分
Denavit-Hartenberg
D-H 建模 (Craig version) !!!
！！！重要： 两个右手定则
两个右手定则：
1、确定坐标系方向的右手定则
2、确定 旋转角 α 正负的右手定则
- 右手拇指 指向 X i − 1 X_{i-1} Xi−1，四指 弯曲 方向为 正
Denavit-Hartenberg 参数表
！！！重要： d 是有正负的
注意： d也要根据 Z ^ i 的方向判定正负【 X ^ i 相对于 X ^ i − 1 】 \hat{Z}_{i}的方向判定正负【\hat{X}_{i}相对于\hat{X}_{i-1}】 Z^i的方向判定正负【X^i相对于X^i−1】
点击：页面内跳转到例子
B站链接： DH参数定义 3D演示
B 站链接： 参考后面的例题部分
D-H 建模 (Standard) 了解
标准部分， 前面的更常用，这种仅作为 了解
Quiz 3
1、
A . X ^ 2 \hat{X}_{2} X^2没有沿着 a 2 a_{2} a2方向
B. 第2个 坐标系 的原点位置 不便于 计算； X ^ 2 \hat{X}_{2} X^2没有沿着 a 2 a_{2} a2方向
C. ✔
D. X ^ 2 \hat{X}_{2} X^2没有沿着 a 2 a_{2} a2方向
2、
系统有bug。。。。答案的要求应该是下面这个：
i = 4
A： 右手拇指指向 X ^ 3 \hat{X}_{3} X^3，将 Z ^ 3 \hat{Z}_{3} Z^3旋转到 Z ^ 4 \hat{Z}_{4} Z^4的方向，旋转方向与四指弯曲方向相反，为负， α为 -90。
B： Z ^ 3 \hat{Z}_{3} Z^3与 Z ^ 4 \hat{Z}_{4} Z^4 可相交，距离为 0。
C： 沿着 Z ^ 4 \hat{Z}_{4} Z^4的方向， X ^ 3 \hat{X}_{3} X^3相对于 X ^ 4 \hat{X}_{4} X^4的距离为 430。
i = 6
D： 右手拇指指向 X ^ 5 \hat{X}_{5} X^5，将 Z ^ 5 \hat{Z}_{5} Z^5旋转到 Z ^ 6 \hat{Z}_{6} Z^6的方向，旋转方向与四指弯曲方向相反，为负， α为 -90 。
E： Z ^ 5 \hat{Z}_{5} Z^5与 Z ^ 6 \hat{Z}_{6} Z^6 可相交，距离为0。
F： 沿着 Z ^ 6 \hat{Z}_{6} Z^6的方向， X ^ 5 \hat{X}_{5} X^5与 X ^ 6 \hat{X}_{6} X^6的距离为 0。
答案： -90//0//430//-90//0//0
===================================
bug 的图 要求解的 i 位置有误 两套 题目？？
- 要是遇到这个 ， 刷新下
i = 3
A： Z ^ 2 \hat{Z}_{2} Z^2与 Z ^ 3 \hat{Z}_{3} Z^3方向相同，无需旋转， α为 0。
B：沿着 X ^ 2 \hat{X}_{2} X^2的方向， Z ^ 2 \hat{Z}_{2} Z^2与 Z ^ 3 \hat{Z}_{3} Z^3的距离为 430。
C： 沿着 Z ^ 3 \hat{Z}_{3} Z^3的方向， X ^ 3 \hat{X}_{3} X^3相对于 X ^ 2 \hat{X}_{2} X^2的距离为 -90。
例子： 上面C的求解。点击返回刚才的地方
注意： d也要根据 Z ^ i 的方向判定正负【大的相对于小的】 \hat{Z}_{i}的方向判定正负【大的相对于小的】 Z^i的方向判定正负【大的相对于小的】
i = 5
D： 右手拇指指向 X ^ 4 \hat{X}_{4} X^4，将 Z ^ 4 \hat{Z}_{4} Z^4旋转到 Z ^ 5 \hat{Z}_{5} Z^5的方向，旋转方向与四指弯曲方向一致，为正， α为 90 。
E： Z ^ 4 \hat{Z}_{4} Z^4与 Z ^ 5 \hat{Z}_{5} Z^5 相交，距离为0。
F： 沿着 Z ^ 5 \hat{Z}_{5} Z^5的方向， X ^ 4 \hat{X}_{4} X^4与 X ^ 5 \hat{X}_{5} X^5的距离为 0。
答案： 0//430//-90//90//0//0
。。。。。
======================
3、
i = 2
α i − 1 = − 90 α_{i-1} = -90 αi−1=−90
d i d_i di = 220
θ i θ_i θi = - 40°
A = a i − 1 a_{i-1} ai−1 = 0
B = − s i n ( α i − 1 ) ⋅ d i -sin(α_{i-1})·d_i −sin(αi−1)⋅di = 220
C = s i n ( θ i ) ⋅ s i n ( α i − 1 ) sin(θ_i)·sin(α_{i-1}) sin(θi)⋅sin(αi−1) = 0.64
D = c o s ( θ i ) ⋅ s i n ( α i − 1 ) cos(θ_i)·sin(α_{i-1}) cos(θi)⋅sin(αi−1) = - 0.77
答案： 0//220//0.64//-0.77
4、
- 有旋转，不能直接 看。。。
答案： 700//320//280
5、
答案： 700//-0.28//0.54//320
附录： NumPy计算代码
最终版本
- 将 参数 顺序 改成 α, a, d, θ 可能舒服些
- 转换矩阵的命名可 参考 T_01, T_12
import numpy as np
α, a, θ, d = 0, 0, np.pi*15/180, 0
T1 = [[np.cos(θ), -np.sin(θ), 0, a],\
[np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α), -np.sin(α)*d],\
[np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α), np.cos(α)*d],\
[0, 0, 0, 1]]
α, a, θ, d = np.pi*(-90)/180, 0, np.pi*(-40)/180, 220
T2 = [[np.cos(θ), -np.sin(θ), 0, a],\
[np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α), -np.sin(α)*d],\
[np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α), np.cos(α)*d],\
[0, 0, 0, 1]]
T = np.dot(T1, T2)
α, a, θ, d = 0, 430, np.pi*(-50)/180, -90
T3 = [[np.cos(θ), -np.sin(θ), 0, a],\
[np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α), -np.sin(α)*d],\
[np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α), np.cos(α)*d],\
[0, 0, 0, 1]]
T = np.dot(T, T3)
α, a, θ, d = np.pi*(-90)/180, 0, np.pi*(30)/180, 430
T4 = [[np.cos(θ), -np.sin(θ), 0, a],\
[np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α), -np.sin(α)*d],\
[np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α), np.cos(α)*d],\
[0, 0, 0, 1]]
T = np.dot(T, T4)
T_04 = [[float(format(x, '.2g')) for x in T[i]] for i in range(len(T))]
print(T_04) ## 第4题 答案
α, a, θ, d = np.pi*(90)/180, 0, np.pi*(70)/180, 0
T5 = [[np.cos(θ), -np.sin(θ), 0, a],\
[np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α), -np.sin(α)*d],\
[np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α), np.cos(α)*d],\
[0, 0, 0, 1]]
T = np.dot(T, T5)
α, a, θ, d = np.pi*(-90)/180, 0, np.pi*(25)/180, 0
T6 = [[np.cos(θ), -np.sin(θ), 0, a],\
[np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α), -np.sin(α)*d],\
[np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α), np.cos(α)*d],\
[0, 0, 0, 1]]
T = np.dot(T, T6)
T_06 = [[float(format(x, '.2g')) for x in T[i]] for i in range(len(T))]
print(T_06) ## 第5题答案
版本0
import numpy as np
## 格式设置
# 不使用省略号表示
np.set_printoptions(threshold = np.inf)
# 不使用科学计数显示:
np.set_printoptions(suppress = True)
# np.set_printoptions(precision=2) ## 只设置 打印 精度 这是保留到 小数点 后两位，不是题目要求的
# T1 = np.around(T1, decimals=2) ##
α, a, θ, d = 0, 0, np.pi*15/180, 0
T1 = [[np.cos(θ), -np.sin(θ), 0, a],\
[np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α), -np.sin(α)*d],\
[np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α), np.cos(α)*d],\
[0, 0, 0, 1]]
# T1 = [[float(format(x, '.3g')) for x in T1[i]] for i in range(len(T1))] ## 保留3位有效数字
# T1 = np.around(T1, decimals=2)
# print(T1)
α, a, θ, d = np.pi*(-90)/180, 0, np.pi*(-40)/180, 220
T2 = [[np.cos(θ), -np.sin(θ), 0, a],\
[np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α), -np.sin(α)*d],\
[np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α), np.cos(α)*d],\
[0, 0, 0, 1]]
# T2 = [[float(format(x, '.3g')) for x in T2[i]] for i in range(len(T2))]
# T2 = np.around(T2, decimals=2)
T = np.dot(T1, T2)
# T = [[float(format(x, '.3g')) for x in T[i]] for i in range(len(T))]
# T = np.around(T, decimals=2)
# print(T)
# T2 = np.around(T2, decimals=2)
# print(T2) ## 第3题 答案
α, a, θ, d = 0, 430, np.pi*(-50)/180, -90
T3 = [[np.cos(θ), -np.sin(θ), 0, a],\
[np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α), -np.sin(α)*d],\
[np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α), np.cos(α)*d],\
[0, 0, 0, 1]]
# T3 = [[float(format(x, '.3g')) for x in T3[i]] for i in range(len(T3))] ## 保留3位有效数字
# T3 = np.around(T3, decimals=2)
T = np.dot(T, T3)
# T = [[float(format(x, '.3g')) for x in T[i]] for i in range(len(T))]
# T = np.around(T, decimals=2)
α, a, θ, d = np.pi*(-90)/180, 0, np.pi*(30)/180, 430
T4 = [[np.cos(θ), -np.sin(θ), 0, a],\
[np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α), -np.sin(α)*d],\
[np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α), np.cos(α)*d],\
[0, 0, 0, 1]]
# T4 = [[float(format(x, '.3g')) for x in T4[i]] for i in range(len(T4))]
# T4 = np.around(T4, decimals=2)
T = np.dot(T, T4)
# T = [[float(format(x, '.3g')) for x in T[i]] for i in range(len(T))]
# T = np.around(T, decimals=2)
T_04 = [[float(format(x, '.2g')) for x in T[i]] for i in range(len(T))]
print(T_04) ## 第4题 答案
α, a, θ, d = np.pi*(90)/180, 0, np.pi*(70)/180, 0
T5 = [[np.cos(θ), -np.sin(θ), 0, a],\
[np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α), -np.sin(α)*d],\
[np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α), np.cos(α)*d],\
[0, 0, 0, 1]]
# T5 = [[float(format(x, '.3g')) for x in T5[i]] for i in range(len(T5))]
# T5 = np.around(T5, decimals=2)
# T5 = np.around(T5, decimals=2)
T = np.dot(T, T5)
# T = [[float(format(x, '.3g')) for x in T[i]] for i in range(len(T))]
# T = np.around(T, decimals=2)
α, a, θ, d = np.pi*(-90)/180, 0, np.pi*(25)/180, 0
T6 = [[np.cos(θ), -np.sin(θ), 0, a],\
[np.sin(θ)*np.cos(α), np.cos(θ)*np.cos(α), -np.sin(α), -np.sin(α)*d],\
[np.sin(θ)*np.sin(α), np.cos(θ)*np.sin(α), np.cos(α), np.cos(α)*d],\
[0, 0, 0, 1]]
# T6 = [[float(format(x, '.3g')) for x in T6[i]] for i in range(len(T6))]
# T6 = np.around(T6, decimals=2)
# T6 = np.around(T6, decimals=2)
T = np.dot(T, T6)
# T = np.around(T, decimals=2)
# T = [[float(format(x, '.3g')) for x in T[i]] for i in range(len(T))]
T_06 = [[float(format(x, '.2g')) for x in T[i]] for i in range(len(T))]
print(T_06) ## 第5题答案
课程论坛的其它 版本
import numpy as np
import math
a0 = 0
a1 = 0
d1 = 0
a2 = 430
d2 = 220
a3 = 0 # z2 z3 垂直
d3 = -90
a4 = 0 # 因为 Z4 Z5 垂直
d4 = 430
a5 = 0
d5 = 0
d6 = 0
theta1=math.radians(15)
theta2=math.radians(-40)
theta3=math.radians(-50)
theta4=math.radians(30)
theta5=math.radians(70)
theta6=math.radians(25)
T01 = np.mat([[math.cos(theta1), -math.sin(theta1), 0, a0],
[math.sin(theta1), math.cos(theta1), 0, 0],
[0, 0, 1, d1],
[0, 0, 0, 1]])
T12 = np.mat([[math.cos(theta2), -math.sin(theta2), 0, a1],
[0, 0, 1, d2],
[-math.sin(theta2), -math.cos(theta2), 0, 0],
[0, 0, 0, 1]])
T23 = np.mat([[math.cos(theta3), -math.sin(theta3), 0, a2],
[math.sin(theta3), math.cos(theta3), 0, 0],
[0, 0, 1, d3],
[0, 0, 0, 1]])
T34 = np.mat([[math.cos(theta4), -math.sin(theta4), 0, a3],
[0, 0, 1, d4],
[-math.sin(theta4), -math.cos(theta4), 0, 0],
[0, 0, 0, 1]])
T45 = np.mat([[math.cos(theta5), -math.sin(theta5), 0, a4],
[0, 0, -1, -d5],
[math.sin(theta5), math.cos(theta5), 0, 0],
[0, 0, 0, 1]])
T56 = np.mat([[math.cos(theta6), -math.sin(theta6), 0, a5],
[0, 0, 1, d6],
[-math.sin(theta6), -math.cos(theta6), 0, 0],
[0, 0, 0, 1]])
# NO.4
P0 = np.mat([[0],[0],[0],[1]])
print("NO.4\n")
print(T01 * T12 * T23 * T34 * P0)
# NO.5
print("NO.5\n")
print(T01 * T12 * T23 * T34 * T45 * T56)