Inverse_Kinematics module

Inverse_Kinematics.disp(expr)

Displays a simplified Sympy expression.

Inverse_Kinematics.h_T(alpha, a, theta, d)

Returns a general homogeneous transform.

Inverse_Kinematics.rot_x(alpha)

Returns a homogeneous transform for just a rotation about the X axis by alpha.

Inverse_Kinematics.rot_z(theta)

Returns a homogeneous transform for just a rotation about the Z axis by theta.

Inverse_Kinematics.trans_x(a)

Returns a homogeneous transform for just a translation along the X axis by a.

Inverse_Kinematics.trans_z(d)

Returns a homogeneous transform for just a rotation along the Z axis by d.

Python Program

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
import numpy as np
import sympy

np.set_printoptions(precision=3, suppress=True)

sympy.init_printing(use_unicode=True, num_columns=400)


def disp(expr):
    """Displays a simplified Sympy expression."""

    e = sympy.simplify(expr)
    e = sympy.expand(e)
    e = e.evalf()

    print(sympy.pretty(e))

    return


def rot_x(alpha):
    """Returns a homogeneous transform for just a rotation about the X axis by alpha."""

    T = sympy.Matrix([[1, 0, 0, 0],
                      [0, sympy.cos(alpha), -sympy.sin(alpha), 0],
                      [0, sympy.sin(alpha), sympy.cos(alpha), 0],
                      [0, 0, 0, 1]])

    return T


def trans_x(a):
    """Returns a homogeneous transform for just a translation along the X axis by a."""

    T = sympy.Matrix([[1, 0, 0, a],
                      [0, 1, 0, 0],
                      [0, 0, 1, 0],
                      [0, 0, 0, 1]])

    return T


def rot_z(theta):
    """Returns a homogeneous transform for just a rotation about the Z axis by theta."""

    T = sympy.Matrix([[sympy.cos(theta), -sympy.sin(theta), 0, 0],
                      [sympy.sin(theta), sympy.cos(theta), 0, 0],
                      [0, 0, 1, 0],
                      [0, 0, 0, 1]])

    return T


def trans_z(d):
    """Returns a homogeneous transform for just a rotation along the Z axis by d."""

    T = sympy.Matrix([[1, 0, 0, 0],
                      [0, 1, 0, 0],
                      [0, 0, 1, d],
                      [0, 0, 0, 1]])

    return T


def h_T(alpha, a, theta, d):
    """Returns a general homogeneous transform."""

    T = rot_x(alpha) @ trans_x(a) @ rot_z(theta) @ trans_z(d)

    return T


# Forward kinematics for the given problem.

theta_1 = np.deg2rad(0.0)
T01 = h_T(0, 0, theta_1, 0)
print("Matrix T01:")
disp(T01)

alpha_1 = np.deg2rad(90)
theta_2 = np.deg2rad(0.0)
T12 = h_T(alpha_1, 0, theta_2, 0)
print("\nMatrix T12:")
disp(T12)

l1 = 500
theta_3 = np.deg2rad(45.0)
T23 = h_T(0, l1, theta_3, 0)
print("\nMatrix T23:")
disp(T23)

l2 = 500
theta_4 = np.deg2rad(45.0)
T34 = h_T(0, l2, theta_4, 0)
print("\nMatrix T34:")
disp(T34)

l3 = 230
theta_5 = np.deg2rad(0)
T45 = h_T(0, l3, theta_5, 0)
print("\nMatrix T45:")
disp(T45)

T05 = T01 @ T12 @ T23 @ T34 @ T45
print("\nMatrix T05:")
disp(T05)

print("\nProblem: location x, y, z")
x05 = float(T05[0, 3])
y05 = float(T05[1, 3])
z05 = float(T05[2, 3])

print("x05: {}".format(x05))
print("y05: {}".format(y05))
print("z05: {}".format(z05))

# Inverse kinematics for the given problem.

x = 853.553
y = 0.0
z = 583.553

theta_1 = np.arctan2(y, z)
T01 = h_T(0, 0, theta_1, 0)
print("\nMatrix T01 for given problem:")
disp(T01)

T45 = h_T(0, 230, 0, 0)
print("\nMatrix T45 for given problem:")
disp(T45)

T15 = T05 @ T01.inv()
print("\nMatrix T15 for given problem:")
disp(T15)

T14 = T15 @ T45.inv()
print("\nMatrix T14 for given problem:")
disp(T14)

x14 = float(T14[0, 3])
y14 = float(T14[1, 3])
z14 = float(T14[2, 3])

print("x14: {}".format(x14))
print("y14: {}".format(y14))
print("z14: {}".format(z14))

B = np.arctan2(z14, x14)
c2 = (l2**2 - l1**2 - x14**2 - z14**2) / (-2*l1*np.sqrt(x14**2 + z14**2))
s2 = np.sqrt(1 - c2**2)
w = np.arctan2(s2, c2)
theta_2 = B - w

c3 = (x14**2 + z14**2 - l1**2 - l2**2)/(2*l1*l2)
s3 = np.sqrt(1 - c3**2)
theta_3 = np.arctan2(s3, c3)

T35 = T34 @ T45
disp(T35)

x35 = float(T35[0, 3])
y35 = float(T35[1, 3])
z35 = float(T35[2, 3])

print("x35: {}".format(x35))
print("y35: {}".format(y35))
print("z35: {}".format(z35))

c4 = (x35 - l2) / l3
s4 = np.sqrt(1 - c4**2)
theta_4 = np.arctan2(s4, c4)

print("\ntheta_2: {}".format(np.rad2deg(theta_2)))
print("\ntheta_3: {}".format(np.rad2deg(theta_3)))
print("\ntheta_4: {}".format(np.rad2deg(theta_4)))

Output

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
  Matrix T01:
  ⎡1.0   0    0    0 ⎤
  ⎢                  ⎥
  ⎢ 0   1.0   0    0 ⎥
  ⎢                  ⎥
  ⎢ 0    0   1.0   0 ⎥
  ⎢                  ⎥
  ⎣ 0    0    0   1.0⎦

  Matrix T12:
  ⎡1.0           0                     0             0 ⎤
  ⎢                                                    ⎥
  ⎢ 0   6.12323399573677e-17          -1.0           0 ⎥
  ⎢                                                    ⎥
  ⎢ 0           1.0           6.12323399573677e-17   0 ⎥
  ⎢                                                    ⎥
  ⎣ 0            0                     0            1.0⎦

  Matrix T23:
  ⎡0.707106781186548  -0.707106781186547   0   500.0⎤
  ⎢                                                 ⎥
  ⎢0.707106781186547  0.707106781186548    0     0  ⎥
  ⎢                                                 ⎥
  ⎢        0                  0           1.0    0  ⎥
  ⎢                                                 ⎥
  ⎣        0                  0            0    1.0 ⎦

  Matrix T34:
  ⎡0.707106781186548  -0.707106781186547   0   500.0⎤
  ⎢                                                 ⎥
  ⎢0.707106781186547  0.707106781186548    0     0  ⎥
  ⎢                                                 ⎥
  ⎢        0                  0           1.0    0  ⎥
  ⎢                                                 ⎥
  ⎣        0                  0            0    1.0 ⎦

  Matrix T45:
  ⎡1.0   0    0   230.0⎤
  ⎢                    ⎥
  ⎢ 0   1.0   0     0  ⎥
  ⎢                    ⎥
  ⎢ 0    0   1.0    0  ⎥
  ⎢                    ⎥
  ⎣ 0    0    0    1.0 ⎦

  Matrix T05:
  ⎡2.22044604925031e-16          -1.0                   0              853.553390593274  ⎤
  ⎢                                                                                      ⎥
  ⎢6.12323399573677e-17  1.23259516440783e-32          -1.0          3.57323395960819e-14⎥
  ⎢                                                                                      ⎥
  ⎢        1.0           2.22044604925031e-16  6.12323399573677e-17    583.553390593274  ⎥
  ⎢                                                                                      ⎥
  ⎣         0                     0                     0                    1.0         ⎦


  Problem: location x, y, z

  x05: 853.5533905932738
  y05: 3.573233959608189e-14
  z05: 583.5533905932737

  Matrix T01 for given problem:
  ⎡1.0   0    0    0 ⎤
  ⎢                  ⎥
  ⎢ 0   1.0   0    0 ⎥
  ⎢                  ⎥
  ⎢ 0    0   1.0   0 ⎥
  ⎢                  ⎥
  ⎣ 0    0    0   1.0⎦

  Matrix T45 for given problem:
  ⎡1.0   0    0   230.0⎤
  ⎢                    ⎥
  ⎢ 0   1.0   0     0  ⎥
  ⎢                    ⎥
  ⎢ 0    0   1.0    0  ⎥
  ⎢                    ⎥
  ⎣ 0    0    0    1.0 ⎦

  Matrix T15 for given problem:
  ⎡2.22044604925031e-16          -1.0                   0              853.553390593274  ⎤
  ⎢                                                                                      ⎥
  ⎢6.12323399573677e-17  1.23259516440783e-32          -1.0          3.57323395960819e-14⎥
  ⎢                                                                                      ⎥
  ⎢        1.0           2.22044604925031e-16  6.12323399573677e-17    583.553390593274  ⎥
  ⎢                                                                                      ⎥
  ⎣         0                     0                     0                    1.0         ⎦

  Matrix T14 for given problem:
  ⎡2.22044604925031e-16          -1.0                   0              853.553390593274  ⎤
  ⎢                                                                                      ⎥
  ⎢6.12323399573677e-17  1.23259516440783e-32          -1.0          2.16489014058873e-14⎥
  ⎢                                                                                      ⎥
  ⎢        1.0           2.22044604925031e-16  6.12323399573677e-17    353.553390593274  ⎥
  ⎢                                                                                      ⎥
  ⎣         0                     0                     0                    1.0         ⎦

  x14: 853.5533905932738
  y14: 2.164890140588733e-14
  z14: 353.55339059327366

  ⎡0.707106781186548  -0.707106781186547   0   662.634559672906⎤
  ⎢                                                            ⎥
  ⎢0.707106781186547  0.707106781186548    0   162.634559672906⎥
  ⎢                                                            ⎥
  ⎢        0                  0           1.0         0        ⎥
  ⎢                                                            ⎥
  ⎣        0                  0            0         1.0       ⎦

  x35: 662.6345596729059
  y35: 162.6345596729059
  z35: 0.0


  theta_2: -9.54166404439055e-15
  theta_3: 45.000000000000014
  theta_4: 45.000000000000014