Forward_Kinematics module¶
-
class
Forward_Kinematics.ForwardKinematics(**kwargs)¶ Bases:
objectDefinition: This class generates Homogeneous transform matrices, although it uses a symbolic approach that can be used to multiply any matrix and obtain the translation or rotation.
sympy.cos and sympy.sin: cos and sin for sympy
sympy.simplify: SymPy has dozens of functions to perform various kinds of simplification. simplify() attempts to apply all of these functions in an intelligent way to arrive at the simplest form of an expression.
Returns: It returns Rotation and translation matrices.
Obs: **kwargs (keyword arguments) are used to facilitate the identification of the parameters, so initiate the object
-
rot_x(alpha)¶ Definition: Receives an alpha angle and returns the rotation matrix for the given angle at the X axis.
Parameters: alpha (string) – Rotation Angle around the X axis Returns: The Rotational Matrix at the X axis by an given angle
-
rot_z(theta)¶ Definition: Receives an theta angle and returns the rotation matrix for the given angle at the Z axis.
Parameters: theta (string) – Rotation Angle around the Z axis Returns: The Rotational Matrix at the Z axis by an given angle
-
trans_x(a)¶ Definition: Translates the matrix a given amount a on the X axis by Defining a 4x4 identity matrix with a as the (1,4) element.
Parameters: a (string) – Distance translated on the X-axis Returns: The Translation Matrix on the X axis by a given distance
-
trans_z(d)¶ Definition: Translate the matrix a given amount d on the Z axis. by Defining a matrix T 4x4 identity matrix with d (3,4) element position.
Parameters: d (string) – Distance translated on the Z-axis Returns: The Translation Matrix on the Z axis by a given distance
-
-
Forward_Kinematics.main()¶ Assessment 02 Robotic manipulator design - Forward Kinematics.
-
Forward_Kinematics.printM(expr, num_digits)¶
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 176 177 178 179 180 181 182 183 184 | import numpy as np
import sympy as sympy
from sympy import *
class ForwardKinematics:
"""
Definition: This class generates Homogeneous transform matrices, although it uses a symbolic approach
that can be used to multiply any matrix and obtain the translation or rotation.
sympy.cos and sympy.sin: cos and sin for sympy
sympy.simplify: SymPy has dozens of functions to perform various kinds of simplification.
simplify() attempts to apply all of these functions
in an intelligent way to arrive at the simplest form of an expression.
Returns: It returns Rotation and translation matrices.
Obs: **kwargs (keyword arguments) are used to facilitate the identification of the parameters, so initiate the
object
"""
np.set_printoptions(precision=3, suppress=True)
sympy.init_printing(use_unicode=True, num_columns=400)
def __init__(self, **kwargs):
"""
Initializes the Object.
"""
self._x_angle = kwargs['x_angle'] if 'x_angle' in kwargs else 'alpha_i-1'
self._x_dist = kwargs['x_dist'] if 'x_dist' in kwargs else 'a_i-1'
self._y_angle = kwargs['y_angle'] if 'y_angle' in kwargs else '0'
self._y_dist = kwargs['y_dist'] if 'y_dist' in kwargs else '0'
self._z_angle = kwargs['z_angle'] if 'z_angle' in kwargs else 'theta_i'
self._z_dist = kwargs['z_dist'] if 'z_dist' in kwargs else 'd_i'
def trans_x(self, a):
"""
Definition: Translates the matrix a given amount `a` on the *X* axis by Defining a 4x4 identity
matrix with `a` as the (1,4) element.
:type a: string
:param a: Distance translated on the X-axis
Returns: The Translation Matrix on the *X* axis by a given distance
"""
self._x_dist = a
t_x = sympy.Matrix([[1, 0, 0, self._x_dist],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
t_x = t_x.evalf()
return t_x
def trans_z(self, d):
"""
Definition: Translate the matrix a given amount `d` on the *Z* axis. by Defining a matrix T 4x4 identity
matrix with *d* (3,4) element position.
:type d: string
:param d: Distance translated on the Z-axis
Returns: The Translation Matrix on the *Z* axis by a given distance
"""
self._z_dist = d
t_z = sympy.Matrix([[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, self._z_dist],
[0, 0, 0, 1]])
t_z = t_z.evalf()
return t_z
def rot_x(self, alpha):
"""
Definition: Receives an alpha angle and returns the rotation matrix for the given angle at the *X* axis.
:type alpha: string
:param alpha: Rotation Angle around the X axis
Returns: The Rotational Matrix at the X axis by an *given* angle
"""
self._x_angle = alpha
r_x = sympy.Matrix([[1, 0, 0, 0],
[0, sympy.cos(self._x_angle), -sympy.sin(self._x_angle), 0],
[0, sympy.sin(self._x_angle), sympy.cos(self._x_angle), 0],
[0, 0, 0, 1]])
r_x = r_x.evalf()
return r_x
def rot_z(self, theta):
"""
Definition: Receives an theta angle and returns the rotation matrix for the given angle at the *Z* axis.
:type theta: string
:param theta: Rotation Angle around the Z axis
Returns: The Rotational Matrix at the Z axis by an *given* angle
"""
self._z_angle = theta
r_z = sympy.Matrix([[sympy.cos(self._z_angle), -sympy.sin(self._z_angle), 0, 0],
[sympy.sin(self._z_angle), sympy.cos(self._z_angle), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
r_z = r_z.evalf()
return r_z
# def printM(expr, num_digits):
# return expr.xreplace({n.evalf(): n if type(n) == int else Float(n, num_digits) for n in expr.atoms(Number)})
def printM(expr, num_digits):
return expr.xreplace({n.evalf(): round(n, num_digits) for n in expr.atoms(Number)})
def main():
"""
Assessment 02 Robotic manipulator design - Forward Kinematics.
"""
a1 = ForwardKinematics() # Rx(a_i-1)
a2 = ForwardKinematics() # Dx(a_i-1)
a3 = ForwardKinematics() # Dz(d_i)
a4 = ForwardKinematics() # Rz(theta_i)
print('Matrix t_0_1:')
t_0_1 = (a1.rot_x('0')) * (a2.trans_x('0')) * (a3.trans_z('l1')) * (a4.rot_z('theta_1'))
print(sympy.pretty(t_0_1))
print('\nMatrix t_1_2:')
t_1_2 = (a1.rot_x('alpha_1')) * (a2.trans_x('0')) * (a3.trans_z('0')) * (a4.rot_z('theta_2'))
t_1_2_subs = t_1_2.subs('alpha_1', np.deg2rad(90.00))
print(sympy.pretty(printM(t_1_2_subs, 3)))
print('\nMatrix t_2_3:')
t_2_3 = (a1.rot_x('0')) * (a2.trans_x('l2')) * (a3.trans_z('0')) * (a4.rot_z('theta_3'))
print(sympy.pretty(t_2_3))
print('\nMatrix t_3_4:')
t_3_4 = (a1.rot_x('0')) * (a2.trans_x('l3')) * (a3.trans_z('0')) * (a4.rot_z('theta_4'))
print(sympy.pretty(t_3_4))
print('\nMatrix t_4_5:')
t_4_5 = (a1.rot_x('0')) * (a2.trans_x('l4')) * (a3.trans_z('0')) * (a4.rot_z('0'))
print(sympy.pretty(t_4_5))
t_0_5 = sympy.simplify(t_0_1 * t_1_2 * t_2_3 * t_3_4 * t_4_5)
print('\nMatrix T_0_5: with substitutions Round')
print(sympy.pretty(sympy.simplify(printM(t_0_5.subs('alpha_1', np.deg2rad(90.00)), 3))))
t_0_5_subs = t_0_5.subs([('alpha_1', np.deg2rad(90.00)), ('l1', 230), ('l2', 500), ('l3', 500), ('l4', 180)])
print('\nMatrix T_0_5: with substitutions Round')
print(sympy.pretty(sympy.simplify(printM(t_0_5_subs, 3))))
t_1_5 = sympy.simplify(t_1_2 * t_2_3 * t_3_4 * t_4_5)
print('\nMatrix T_1_5:')
print(sympy.pretty(printM(t_1_5.subs('alpha_1', np.deg2rad(90.00)), 3)))
print('\nMatrix T_1_5: for theta_1 = 0 ')
print(sympy.pretty(printM(t_1_5.subs([('alpha_1', np.deg2rad(90.00)), ('theta_1', np.deg2rad(0.00))]), 3)))
# Calculations for the Inverse kinematics problem
t_1_4 = sympy.simplify(t_1_5 * t_4_5.inv())
print('\nMatrix T_1_4:')
print(sympy.pretty(printM(t_1_4.subs('alpha_1', np.deg2rad(90.00)), 3)))
t_3_5 = sympy.simplify(t_1_5 * t_1_2.inv() * t_2_3.inv())
print('\nMatrix t_3_5:')
print(sympy.pretty(printM(t_3_5.subs('alpha_1', np.deg2rad(90.00)), 3)))
if __name__ == '__main__':
main()
|
Output¶
Matrix t_0_1:
⎡1.0⋅cos(θ₁) -1.0⋅sin(θ₁) 0 0 ⎤
⎢ ⎥
⎢1.0⋅sin(θ₁) 1.0⋅cos(θ₁) 0 0 ⎥
⎢ ⎥
⎢ 0 0 1.0 1.0⋅l₁⎥
⎢ ⎥
⎣ 0 0 0 1.0 ⎦
Matrix t_1_2:
⎡1.0⋅cos(θ₂) -1.0⋅sin(θ₂) 0 0 ⎤
⎢ ⎥
⎢ 0 0 -1.0 0 ⎥
⎢ ⎥
⎢1.0⋅sin(θ₂) 1.0⋅cos(θ₂) 0.0 0 ⎥
⎢ ⎥
⎣ 0 0 0 1.0⎦
Matrix t_2_3:
⎡1.0⋅cos(θ₃) -1.0⋅sin(θ₃) 0 1.0⋅l₂ ⎤
⎢ ⎥
⎢1.0⋅sin(θ₃) 1.0⋅cos(θ₃) 0 0 ⎥
⎢ ⎥
⎢ 0 0 1.0 0 ⎥
⎢ ⎥
⎣ 0 0 0 1.0 ⎦
Matrix t_3_4:
⎡1.0⋅cos(θ₄) -1.0⋅sin(θ₄) 0 1.0⋅l₃ ⎤
⎢ ⎥
⎢1.0⋅sin(θ₄) 1.0⋅cos(θ₄) 0 0 ⎥
⎢ ⎥
⎢ 0 0 1.0 0 ⎥
⎢ ⎥
⎣ 0 0 0 1.0 ⎦
Matrix t_4_5:
⎡1.0 0 0 1.0⋅l₄⎤
⎢ ⎥
⎢ 0 1.0 0 0 ⎥
⎢ ⎥
⎢ 0 0 1.0 0 ⎥
⎢ ⎥
⎣ 0 0 0 1.0 ⎦
Matrix T_0_5: with substitutions Round
⎡1.0⋅cos(θ₁)⋅cos(θ₂ + θ₃ + θ₄) -1.0⋅sin(θ₂ + θ₃ + θ₄)⋅cos(θ₁) 1.0⋅sin(θ₁) 1.0⋅(l₂⋅cos(θ₂) + l₃⋅cos(θ₂ + θ₃) + l₄⋅cos(θ₂ + θ₃ + θ₄))⋅cos(θ₁) ⎤
⎢ ⎥
⎢1.0⋅sin(θ₁)⋅cos(θ₂ + θ₃ + θ₄) -1.0⋅sin(θ₁)⋅sin(θ₂ + θ₃ + θ₄) -1.0⋅cos(θ₁) 1.0⋅(l₂⋅cos(θ₂) + l₃⋅cos(θ₂ + θ₃) + l₄⋅cos(θ₂ + θ₃ + θ₄))⋅sin(θ₁) ⎥
⎢ ⎥
⎢ 1.0⋅sin(θ₂ + θ₃ + θ₄) 1.0⋅cos(θ₂ + θ₃ + θ₄) 0 1.0⋅l₁ + 1.0⋅l₂⋅sin(θ₂) + 1.0⋅l₃⋅sin(θ₂ + θ₃) + 1.0⋅l₄⋅sin(θ₂ + θ₃ + θ₄) ⎥
⎢ ⎥
⎣ 0 0 0 1.0 ⎦
Matrix T_0_5: with substitutions Round
⎡1.0⋅cos(θ₁)⋅cos(θ₂ + θ₃ + θ₄) -1.0⋅sin(θ₂ + θ₃ + θ₄)⋅cos(θ₁) 1.0⋅sin(θ₁) (500.0⋅cos(θ₂) + 500.0⋅cos(θ₂ + θ₃) + 180.0⋅cos(θ₂ + θ₃ + θ₄))⋅cos(θ₁) ⎤
⎢ ⎥
⎢1.0⋅sin(θ₁)⋅cos(θ₂ + θ₃ + θ₄) -1.0⋅sin(θ₁)⋅sin(θ₂ + θ₃ + θ₄) -1.0⋅cos(θ₁) (500.0⋅cos(θ₂) + 500.0⋅cos(θ₂ + θ₃) + 180.0⋅cos(θ₂ + θ₃ + θ₄))⋅sin(θ₁) ⎥
⎢ ⎥
⎢ 1.0⋅sin(θ₂ + θ₃ + θ₄) 1.0⋅cos(θ₂ + θ₃ + θ₄) 0 500.0⋅sin(θ₂) + 500.0⋅sin(θ₂ + θ₃) + 180.0⋅sin(θ₂ + θ₃ + θ₄) + 230.0 ⎥
⎢ ⎥
⎣ 0 0 0 1.0 ⎦
Matrix T_1_5:
⎡1.0⋅cos(θ₂ + θ₃ + θ₄) -1.0⋅sin(θ₂ + θ₃ + θ₄) 0 1.0⋅l₂⋅cos(θ₂) + 1.0⋅l₃⋅cos(θ₂ + θ₃) + 1.0⋅l₄⋅cos(θ₂ + θ₃ + θ₄) ⎤
⎢ ⎥
⎢ 0 0 -1.0 0 ⎥
⎢ ⎥
⎢1.0⋅sin(θ₂ + θ₃ + θ₄) 1.0⋅cos(θ₂ + θ₃ + θ₄) 0.0 1.0⋅l₂⋅sin(θ₂) + 1.0⋅l₃⋅sin(θ₂ + θ₃) + 1.0⋅l₄⋅sin(θ₂ + θ₃ + θ₄) ⎥
⎢ ⎥
⎣ 0 0 0 1.0 ⎦
Matrix T_1_5: for theta_1 = 0
⎡1.0⋅cos(θ₂ + θ₃ + θ₄) -1.0⋅sin(θ₂ + θ₃ + θ₄) 0 1.0⋅l₂⋅cos(θ₂) + 1.0⋅l₃⋅cos(θ₂ + θ₃) + 1.0⋅l₄⋅cos(θ₂ + θ₃ + θ₄) ⎤
⎢ ⎥
⎢ 0 0 -1.0 0 ⎥
⎢ ⎥
⎢1.0⋅sin(θ₂ + θ₃ + θ₄) 1.0⋅cos(θ₂ + θ₃ + θ₄) 0.0 1.0⋅l₂⋅sin(θ₂) + 1.0⋅l₃⋅sin(θ₂ + θ₃) + 1.0⋅l₄⋅sin(θ₂ + θ₃ + θ₄) ⎥
⎢ ⎥
⎣ 0 0 0 1.0 ⎦
Matrix T_1_5: for theta_1 = 0
⎡1.0⋅cos(θ₂ + θ₃ + θ₄) -1.0⋅sin(θ₂ + θ₃ + θ₄) 0 1.0⋅l₂⋅cos(θ₂) + 1.0⋅l₃⋅cos(θ₂ + θ₃) + 1.0⋅l₄⋅cos(θ₂ + θ₃ + θ₄) ⎤
⎢ ⎥
⎢ 0 0 -1.0 0 ⎥
⎢ ⎥
⎢1.0⋅sin(θ₂ + θ₃ + θ₄) 1.0⋅cos(θ₂ + θ₃ + θ₄) 0.0 1.0⋅l₂⋅sin(θ₂) + 1.0⋅l₃⋅sin(θ₂ + θ₃) + 1.0⋅l₄⋅sin(θ₂ + θ₃ + θ₄) ⎥
⎢ ⎥
⎣ 0 0 0 1.0 ⎦
Matrix T_1_4:
⎡1.0⋅cos(θ₂ + θ₃ + θ₄) -1.0⋅sin(θ₂ + θ₃ + θ₄) 0 1.0⋅l₂⋅cos(θ₂) + 1.0⋅l₃⋅cos(θ₂ + θ₃) ⎤
⎢ ⎥
⎢ 0 0 -1.0 0 ⎥
⎢ ⎥
⎢1.0⋅sin(θ₂ + θ₃ + θ₄) 1.0⋅cos(θ₂ + θ₃ + θ₄) 0.0 1.0⋅l₂⋅sin(θ₂) + 1.0⋅l₃⋅sin(θ₂ + θ₃) ⎥
⎢ ⎥
⎣ 0 0 0 1.0 ⎦
Matrix t_3_5:
⎡1.0⋅cos(θ₃)⋅cos(θ₃ + θ₄) 1.0⋅sin(θ₃)⋅cos(θ₃ + θ₄) -sin(θ₃ + θ₄) 1.0⋅l₂⋅cos(θ₂) - l₂⋅cos(θ₃)⋅cos(θ₃ + θ₄) + 1.0⋅l₃⋅cos(θ₂ + θ₃) + 1.0⋅l₄⋅cos(θ₂ + θ₃ + θ₄) ⎤
⎢ ⎥
⎢ -sin(θ₃) 1.0⋅cos(θ₃) 0.0 1.0⋅l₂⋅sin(θ₃) ⎥
⎢ ⎥
⎢1.0⋅sin(θ₃ + θ₄)⋅cos(θ₃) 1.0⋅sin(θ₃)⋅sin(θ₃ + θ₄) 1.0⋅cos(θ₃ + θ₄) 1.0⋅l₂⋅sin(θ₂) - l₂⋅sin(θ₃ + θ₄)⋅cos(θ₃) + 1.0⋅l₃⋅sin(θ₂ + θ₃) + 1.0⋅l₄⋅sin(θ₂ + θ₃ + θ₄) ⎥
⎢ ⎥
⎣ 0 0 0 1.0 ⎦