《机器人学一(Robotics(1))》_台大林沛群 第 3 周 【机械手臂 顺运动学】Quiz 3

coursera课程链接

高手在课程论坛
课程论坛链接

理论知识部分

Denavit-Hartenberg

D-H 建模 (Craig version) !!!

！！！重要： 两个右手定则

两个右手定则：
1、确定坐标系方向的右手定则

2、确定 旋转角 α 正负的右手定则

• 右手拇指 指向 $X_{i-1}$，四指 弯曲 方向为 正

Denavit-Hartenberg 参数表

！！！重要： d 是有正负的

注意： d也要根据${\hat{Z}}_i的方向判定正负【{\hat{X}}_i相对于{\hat{X}}_{i-1}】$
点击：页面内跳转到例子

B站链接： DH参数定义 3D演示
B 站链接： 参考后面的例题部分

D-H 建模 (Standard) 了解

标准部分， 前面的更常用，这种仅作为 了解

Quiz 3

1、
A . ${\hat{X}}_2$没有沿着 $a_2$方向
B. 第2个 坐标系 的原点位置 不便于 计算； ${\hat{X}}_2$没有沿着 $a_2$方向
C. ✔
D. ${\hat{X}}_2$没有沿着 $a_2$方向

2、
系统有bug。。。。答案的要求应该是下面这个：

i = 4
A： 右手拇指指向${\hat{X}}_3$，将${\hat{Z}}_3$旋转到${\hat{Z}}_4$的方向，旋转方向与四指弯曲方向相反，为负， α为 -90。
B： ${\hat{Z}}_3$与 ${\hat{Z}}_4$ 可相交，距离为 0。
C： 沿着 ${\hat{Z}}_4$的方向，${\hat{X}}_3$相对于${\hat{X}}_4$的距离为 430。
i = 6
D： 右手拇指指向${\hat{X}}_5$，将${\hat{Z}}_5$旋转到${\hat{Z}}_6$的方向，旋转方向与四指弯曲方向相反，为负， α为 -90 。
E： ${\hat{Z}}_5$与 ${\hat{Z}}_6$ 可相交，距离为0。
F： 沿着 ${\hat{Z}}_6$的方向，${\hat{X}}_5$与${\hat{X}}_6$的距离为 0。

答案： -90//0//430//-90//0//0

===================================
bug 的图 要求解的 i 位置有误 两套 题目？？

• 要是遇到这个 ， 刷新下

i = 3
A： ${\hat{Z}}_2$与${\hat{Z}}_3$方向相同，无需旋转， α为 0。
B：沿着 ${\hat{X}}_2$的方向，${\hat{Z}}_2$与${\hat{Z}}_3$的距离为 430。
C： 沿着 ${\hat{Z}}_3$的方向，${\hat{X}}_3$相对于${\hat{X}}_2$的距离为 -90。

例子： 上面C的求解。点击返回刚才的地方

注意： d也要根据${\hat{Z}}_i的方向判定正负【大的相对于小的】$
i = 5
D： 右手拇指指向${\hat{X}}_4$，将${\hat{Z}}_4$旋转到${\hat{Z}}_5$的方向，旋转方向与四指弯曲方向一致，为正， α为 90 。
E： ${\hat{Z}}_4$与 ${\hat{Z}}_5$ 相交，距离为0。
F： 沿着 ${\hat{Z}}_5$的方向，${\hat{X}}_4$与${\hat{X}}_5$的距离为 0。

答案： 0//430//-90//90//0//0
。。。。。

======================

3、

i = 2
${\alpha }_{i-1}=-90$
$d_i$ = 220
${\theta }_i$ = - 40°
A = $a_{i-1}$ = 0
B = $-sin({\alpha }_{i-1})\cdot d_i$ = 220
C = $sin({\theta }_i)\cdot sin({\alpha }_{i-1})$ = 0.64
D = $cos({\theta }_i)\cdot sin({\alpha }_{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)