正向运动学

串联机械手的正向运动学：

这里写图片描述

这里写图片描述
若对以上有疑问，建议阅读<前面文章>

以前文提到的SCARA机器人为例，正向运动学代码实现如下：

from sympy import symbols, cos, sin, pi, simplify
from sympy.matrices import Matrix

### Create symbols for joint variables
q1, q2, q3, q4 = symbols('q1:5')
d1, d2, d3, d4 = symbols('d1:5')
a0, a1, a2, a3 = symbols('a0:4')
alpha0, alpha1, alpha2, alpha3 = symbols('alpha0:4')

a12 = 0.4500 # meters
a23 = 0.3000 # meters

# DH Parameters
s = {alpha0: 0,  a0:   0, d1: 0, 
     alpha1: 0,  a1: a12, d2: 0,  
     alpha2: 0,  a2: a23,        q3: 0,
     alpha3: 0,  a3:   0, d4: 0}

#### Homogeneous Transforms
T0_1 = Matrix([[             cos(q1),            -sin(q1),            0,              a0],
               [ sin(q1)*cos(alpha0), cos(q1)*cos(alpha0), -sin(alpha0), -sin(alpha0)*d1],
               [ sin(q1)*sin(alpha0), cos(q1)*sin(alpha0),  cos(alpha0),  cos(alpha0)*d1],
               [                   0,                   0,            0,               1]])
T0_1 = T0_1.subs(s)

T1_2 = Matrix([[             cos(q2),            -sin(q2),            0,              a1],
               [ sin(q2)*cos(alpha1), cos(q2)*cos(alpha1), -sin(alpha1), -sin(alpha1)*d2],
               [ sin(q2)*sin(alpha1), cos(q2)*sin(alpha1),  cos(alpha1),  cos(alpha1)*d2],
               [                   0,                   0,            0,               1]])
T1_2 = T1_2.subs(s)

T2_3 = Matrix([[             cos(q3),            -sin(q3),            0,              a2],
               [ sin(q3)*cos(alpha2), cos(q3)*cos(alpha2), -sin(alpha2), -sin(alpha2)*d3],
               [ sin(q3)*sin(alpha2), cos(q3)*sin(alpha2),  cos(alpha2),  cos(alpha2)*d3],
               [                   0,                   0,            0,               1]])
T2_3 = T2_3.subs(s)

T3_4 = Matrix([[             cos(q4),            -sin(q4),            0,              a3],
               [ sin(q4)*cos(alpha3), cos(q4)*cos(alpha3), -sin(alpha3), -sin(alpha3)*d4],
               [ sin(q4)*sin(alpha3), cos(q4)*sin(alpha3),  cos(alpha3),  cos(alpha3)*d4],
               [                   0,                   0,            0,               1]])
T3_4 = T3_4.subs(s)

# Transform from base link to end effector
T0_4 = simplify(T0_1 * T1_2 * T2_3 * T3_4)

print(T0_4)
print(T0_4.evalf(subs={q1: 0, q2: 0, d3: 0, q4: 0}))

猜你喜欢