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

Quiz 3

解决代码如下：

import numpy as np

# continual Link transformations params
# alpha, a, theta, d

def link_transformation_function (alpha, a, theta, d):
    alpha = np.pi * alpha / 180
    theta = np.pi * theta / 180
    T = [[np.cos(theta), -np.sin(theta), 0, a],
         [np.sin(theta) * np.cos(alpha), np.cos(theta) * np.cos(alpha), -np.sin(alpha), -np.sin(alpha) * d],
         [np.sin(theta) * np.sin(alpha), np.cos(theta) * np.sin(alpha), np.cos(alpha), np.cos(alpha) * d],
         [0, 0, 0, 1]]
    return T
    pass

# start to compute
# T_0_1
T_01 = link_transformation_function(0, 0, 15, 0)
# T_1_2
T_12 = link_transformation_function(-90, 0, -40, 220)
# T_2_3
T_23 = link_transformation_function(0, 430, -50, -90)

# p4.
T = np.dot(T_01, T_12)
T = np.dot(T, T_23)
p = [0, 430, 0, 1]
p = np.dot(T, p)
# print(T)
print(p)

# p5.
T_34 = link_transformation_function(-90, 0, 30, 430)
T_45 = link_transformation_function(90, 0, 70, 0)
T_56 = link_transformation_function(-90, 0, 25, 0)
T = np.dot(T, T_34)
T = np.dot(T, T_45)
T = np.dot(T, T_56)
print(T)

答案如下，注意四舍五入保留两位有效数字：