Home Subscribe

4. Homogeneous Transformations

In order to position the end-effector of a robot at a specific point in the workspace, the robot must go through a series of rotations and translations of its links about the joints. This requires creating a composite homogeneous transformation matrix, which is the result of matrix multiplications. It is important to note that matrix multiplication is non-commutative, meaning the order of the multiplications matters. To create the composite matrix, the following rules should be followed:

  1. Initially, the fixed and mobile coordinate systems are coincident, so the homogeneous transformation matrix is an identity matrix.

  2. If the mobile frame (OABC) is rotating or translating about one of the axes (OXYZ) of the fixed frame, the previous result matrix should be "pre-multiplied" by the current homogeneous rotation or translation matrix.

  3. If the mobile coordinate system (OABC) is rotating or translating about one of its own axes, the previous result matrix should be "post-multiplied" by the present homogeneous transformation matrix.

Idea

Example 1

Determine the homogeneous transformation matrix to represent a rotation of \(40^\circ\) about OX-axis and a translation of \(7\) units along the OB-axis of the mobile frame.

Solution:
\[\begin{align*} H(x,\alpha) &= H(x, 40^{\circ}) \\ H(x,\beta) &= H(B, 7) \\ H &= H(x, 40^{\circ}) \cdot I \cdot H(B, 7) \\ &=\left[\begin{array}{cccc}1 & 0 & 0 & 0 \\0 & \cos 40^{\circ} & -\sin 40^{\circ} & 0 \\0 & \sin 40^{\circ} & \cos 40^{\circ} & 0 \\0 & 0 & 0 & 1\end{array}\right] \cdot \left[\begin{array}{cccc}1 & 0 & 0 & 0 \\0 & 1 & 0 & 0 \\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1\end{array}\right] \cdot \left[\begin{array}{cccc}1 & 0 & 0 & 0 \\0 & 1 & 0 & 7 \\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1\end{array}\right] \\ &=\left[\begin{array}{cccc}1 & 0 & 0 & 0 \\0 & \cos 40^{\circ} & -\sin 40^{\circ} & 0 \\0 & \sin 40^{\circ} & \cos 40^{\circ} & 0 \\0 & 0 & 0 & 1\end{array}\right] \cdot \left[\begin{array}{cccc}1 & 0 & 0 & 0 \\0 & 1 & 0 & 7 \\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1\end{array}\right] \\ &=\left[\begin{array}{cccc}1 & 0 & 0 & 0 \\0 & 0.7660 & -0.6428 & 0 \\0 & 0.6428 & 0.7660 & 0 \\0 & 0 & 0 & 1\end{array}\right] \cdot \left[\begin{array}{cccc}1 & 0 & 0 & 0 \\0 & 1 & 0 & 7 \\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1\end{array}\right] \\ &=\left[\begin{array}{cccc}1 & 0 & 0 & 0 \\0 & 0.7660 & -0.6428 & 5.3620 \\0 & 0.6428 & 0.7660 & 4.4996 \\0 & 0 & 0 & 1\end{array}\right]\\ \end{align*}\]
The following is a Python code implementation of the above calculation (Click to reveal). This code implementation provides 3 outputs: a matrix in surd form, 4 decimal numerical values, and a latex format.

 1# orcid.org/0000-0002-7326-7502
 2import numpy as np
 3import sympy
 4
 5# Convert the angle to radians
 6angle = np.radians(40)
 7
 8# Create the rotation matrix
 9rotation_matrix = \
10np.array([[1, 0, 0, 0],
11[0, np.cos(angle), -np.sin(angle), 0],
12[0, np.sin(angle), np.cos(angle), 0],
13[0, 0, 0, 1]])
14
15# Create the identity matrix
16identity_matrix = np.array([[1, 0, 0, 0],
17                            [0, 1, 0, 0],
18                            [0, 0, 1, 0],
19                            [0, 0, 0, 1]])
20
21# Create the translation matrix
22translation_matrix = np.array([[1, 0, 0, 0],
23                               [0, 1, 0, 7],
24                               [0, 0, 1, 0],
25                               [0, 0, 0, 1]])
26
27# Multiply the matrices together 
28# to get the final transformation matrix
29transformation_matrix = \
30np.matmul(rotation_matrix, \
31np.matmul(identity_matrix, translation_matrix))
32
33# Convert the result to a sympy matrix
34transformation_matrix = sympy.Matrix(transformation_matrix)
35
36# Convert the symbolic values to numerical values with 4 decimal places
37transformation_matrix = transformation_matrix.applyfunc(lambda x: sympy.N(x, 4))
38
39# Display the transformation matrix in LaTeX-like format
40print("\nnumerical values with 4 decimal places:")
41sympy.pprint(transformation_matrix)
42
43print("\nlatex output:")
44print(sympy.latex(transformation_matrix))

Idea

Example 2

Determine the overall homogeneous transformation matrix for the following sequence of operations. Rotation of \(50^\circ\) OX-axis, translation of \(4\) units along OX-axis, translation of \(-6\) units along OC-axis, and rotation of \(25^\circ\) about OB-axis.

Solution:
\[\begin{align*} H &= H(x, 4) \cdot H(x, 50^{\circ}) \cdot I \cdot H(c, -6) \cdot H(b, 25^{\circ}) \\ &=\left[\begin{array}{cccc}1 & 0 & 0 & 4 \\0 & 1 & 0 & 0 \\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1\end{array}\right] \left[\begin{array}{cccc}1 & 0 & 0 & 0 \\0 & 0.6428 & -0.7660 & 0 \\0 & 0.7660 & 0.6428 & 0 \\0 & 0 & 0 & 1\end{array}\right] \\ & \times \left[\begin{array}{cccc}1 & 0 & 0 & 0 \\0 & 1 & 0 & 0 \\0 & 0 & 1 & -6 \\0 & 0 & 0 & 1\end{array}\right] \left[\begin{array}{cccc}0.9063 & 0 & 0.4226 & 0 \\0 & 1 & 0 & 0 \\-0.4226 & 0 & 0.9063 & 0 \\0 & 0 & 0 & 1\end{array}\right] \\ &= \left[\begin{matrix}0.9063 & 0 & 0.4226 & 4.0\\0.3237 & 0.6428 & -0.6943 & 4.596\\-0.2717 & 0.766 & 0.5826 & -3.857\\0 & 0 & 0 & 1.0\end{matrix}\right] \end{align*}\]
The following is a Python code implementation of the above calculation (Click to reveal). This code implementation provides 3 outputs: a matrix in surd form, 4 decimal numerical values, and a latex format.

 1# orcid.org/0000-0002-7326-7502
 2import numpy as np
 3import sympy
 4
 5# Enable LaTeX-like output
 6sympy.init_printing()
 7
 8a = np.radians(50)
 9b = np.radians(25)
10
11# Convert the rotations to matrices
12rx = np.array([[1, 0, 0, 0],
13               [0, np.cos(a), -np.sin(a), 0],
14               [0, np.sin(a), np.cos(a), 0],
15               [0, 0, 0, 1]])
16rb = np.array([[np.cos(b), 0, np.sin(b), 0],
17               [0, 1, 0, 0],
18               [-np.sin(b), 0, np.cos(b), 0],
19               [0, 0, 0, 1]])
20
21# Convert the translations to matrices
22tx = np.array([[1, 0, 0, 4],
23               [0, 1, 0, 0],
24               [0, 0, 1, 0],
25               [0, 0, 0, 1]])
26tc = np.array([[1, 0, 0, 0],
27               [0, 1, 0, 0],
28               [0, 0, 1, -6],
29               [0, 0, 0, 1]])
30
31# Multiply the matrices together to get the final transformation matrix
32tcrb = np.matmul(tc, rb)
33rxtcrb = np.matmul(rx, tcrb)
34txrxtcrb = np.matmul(tx, rxtcrb)
35
36# Convert the result to a sympy matrix
37transformation_matrix = sympy.Matrix(txrxtcrb)
38
39# Convert the symbolic values to numerical values with 4 decimal places
40transformation_matrix = transformation_matrix.applyfunc(lambda x: sympy.N(x, 4))
41
42# Display the transformation matrix in LaTeX-like format
43print("\nnumerical values with 4 decimal places:")
44sympy.pprint(transformation_matrix)
45
46print("\nlatex output:")
47print(sympy.latex(transformation_matrix))

4.1. Composite Homogeneous Transformations

Idea

Example 3

A robotic work cell has a camera within the setup. The camera can see the origin of the six-joint robot fixed to a base. A cube placed on the work cell table is also seen by the camera. The homogeneous transformation matrix \(H_1\) maps the camera to the centre of the cube. The origin of the base coordinate system as seen from the camera is represented by the homogeneous transformation matrix \(H_2\).
What is the position and orientation of the cube with respect to the base coordinate system?
After the system is set up, someone rotates the camera \(60^\circ\) about the \(Z\) axis of the camera. What is the position and orientation of the camera with respect to the robot’s base coordinate system?
The same person rotated the cube by \(60^\circ\) about the \(X\) axis of the cube and translated \(5\) units of distance along the \(Y\) axis. What is the position and orientation of the cube with respect to the robot’s base coordinate system?

\[\begin{aligned} H_1 = \begin{bmatrix} 0 & 1 & 0 & 2\\ 1 & 0 & 0 & 8\\ 0 & 0 & -1 & 7\\ 0 & 0 & 0 & 1 \end{bmatrix}, H_2 = \begin{bmatrix} 1 & 0 & 0 & -8\\ 0 & -1 & 0 & 15\\ 0 & 0 & -1 & 6\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{aligned}\]
Solution:

The position and orientation of the cube with respect to the base coordinate system.

\[\begin{aligned} & { }^\text{camera}H_{\text {cube }}=H_{1}=\left[\begin{array}{rrrr}0 & 1 & 0 & 2 \\1 & 0 & 0 & 8 \\0 & 0 & -1 & 7 \\0 & 0 & 0 & 1\end{array}\right] \\ & { }^\text{camera}H_{\text {base }}=H_{2}=\left[\begin{array}{rrrr}1 & 0 & 0 & -8 \\0 & -1 & 0 & 15 \\0 & 0 & -1 & 6 \\0 & 0 & 0 & 1\end{array}\right] \\ & \text {To find }{ }^{\text{base}} H_{\text {cube }} \text { by chain product rule, } \\ { }^{\text{base}} H_{\text {cube }} &= { }^\text{base}H_{\text {camera}} \text{ . } { }^\text{camera}H_{\text {cube}} = (H_{2})^{-1} \text{ . } H_{1} \\ & =\left[\begin{matrix}1 & 0 & 0 & 8\\0 & -1 & 0 & 15\\0 & 0 & -1 & 6\\0 & 0 & 0 & 1\end{matrix}\right] \left[\begin{matrix}0 & 1 & 0 & 2\\1 & 0 & 0 & 8\\0 & 0 & -1 & 7\\0 & 0 & 0 & 1\end{matrix}\right]\\ &=\left[\begin{matrix}0 & 1 & 0 & 10\\-1 & 0 & 0 & 7\\0 & 0 & 1 & -1\\0 & 0 & 0 & 1\end{matrix}\right] \\ &\text{The position of the cube is given by } \left[\begin{matrix}10\\7\\-1\end{matrix}\right] \\ &\text{The orientation } [n, s, a]=\left[\begin{matrix}0 & 1 & 0\\-1 & 0 & 0\\0 & 0 & 1\end{matrix}\right] \\ \end{aligned}\]

The position and orientation of the camera with respect to the robot’s base coordinate system.

\[\begin{aligned} { }^\text{base}H_{\text {camera}} &=\left(H_{2}\right)^{-1} \cdot H\left(z, 60^{\circ}\right)_{\text {camera}} \\ H\left(z, 60^{\circ}\right)_{\text{camera}} &=\left[\begin{array}{cccc} \cos 60^{\circ} & -\sin 60^{\circ} & 0 & 0 \\ \sin 60^{\circ} & \cos 60^{\circ} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right] \\ { }^{\text {base }} \mathrm{H}_{\text {camera }} &=\left[\begin{matrix}1 & 0 & 0 & 8\\0 & -1 & 0 & 15\\0 & 0 & -1 & 6\\0 & 0 & 0 & 1\end{matrix}\right] \left[\begin{matrix}\frac{1}{2} & - \frac{\sqrt{3}}{2} & 0 & 0\\\frac{\sqrt{3}}{2} & \frac{1}{2} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]\\ &=\left[\begin{matrix}\frac{1}{2} & - \frac{\sqrt{3}}{2} & 0 & 8\\- \frac{\sqrt{3}}{2} & - \frac{1}{2} & 0 & 15\\0 & 0 & -1 & 6\\0 & 0 & 0 & 1\end{matrix}\right] \\ &\text{The position of the camera after the change is given by } \\ &\left[\begin{matrix}8\\15\\6\end{matrix}\right] \\ &\text{The orientation of the camera with respect to the base } \\ &[n, s, a]=\left[\begin{matrix}\frac{1}{2} & - \frac{\sqrt{3}}{2} & 0\\- \frac{\sqrt{3}}{2} & - \frac{1}{2} & 0\\0 & 0 & -1\end{matrix}\right] \\ \end{aligned}\]

The position and orientation of the cube with respect to the robot’s base coordinate system?

\[\begin{aligned} { }^{\text{base}} H_{\text{cube-2}} &={ }^{\text {base }} H_{\text {cube }} \text{ . } H\left(x, 60^{\circ}\right) \text{ . } H (y, 5) \\ { }^{\text{base}} H_{\text{cube-2}} &=\left[\begin{matrix}0 & 1 & 0 & 10\\-1 & 0 & 0 & 7\\0 & 0 & 1 & -1\\0 & 0 & 0 & 1\end{matrix}\right] \left[\begin{matrix}1 & 0 & 0 & 0\\0 & \frac{1}{2} & - \frac{\sqrt{3}}{2} & 0\\0 & \frac{\sqrt{3}}{2} & \frac{1}{2} & 0\\0 & 0 & 0 & 1\end{matrix}\right] \left[\begin{matrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 5\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1 \end{matrix}\right]\\ &=\left[\begin{matrix}0 & \frac{1}{2} & - \frac{\sqrt{3}}{2} & \frac{25}{2}\\-1 & 0 & 0 & 7\\0 & \frac{\sqrt{3}}{2} & \frac{1}{2} & -1 + \frac{5 \sqrt{3}}{2}\\0 & 0 & 0 & 1\end{matrix}\right] \\ &\text{The position of the object with respect to the base } \\ &= \left[\begin{matrix}\frac{25}{2}\\7\\-1 + \frac{5 \sqrt{3}}{2}\end{matrix}\right] \\ &\text {The orientation, }[n, s, a]=\left[\begin{matrix}0 & \frac{1}{2} & - \frac{\sqrt{3}}{2}\\-1 & 0 & 0\\0 & \frac{\sqrt{3}}{2} & \frac{1}{2}\end{matrix}\right] \\ \end{aligned}\]
The following is a Python code implementation of the above calculation (Click to reveal).

 1# orcid.org/0000-0002-7326-7502
 2import sympy as sp
 3from sympy.matrices import Matrix
 4
 5# Homogeneous transformation matrices
 6H_1 = Matrix([[0, 1, 0, 2], 
 7              [1, 0, 0, 8], 
 8              [0, 0, -1, 7], 
 9              [0, 0, 0, 1]])
10H_2 = Matrix([[1, 0, 0, -8], 
11              [0, -1, 0, 15], 
12              [0, 0, -1, 6], 
13              [0, 0, 0, 1]])
14
15# Calculate the angle of and matrix of rotation
16angle = 60
17theta = sp.pi*angle/180
18H_z60 = Matrix([[sp.cos(theta), -sp.sin(theta), 0, 0], 
19                [sp.sin(theta), sp.cos(theta), 0, 0], 
20                [0, 0, 1, 0], [0, 0, 0, 1]])
21
22# Position and orientation of cube with respect to base
23H_base_cube = H_2.inv() * H_1
24pos_cube = H_base_cube[:3,3]
25orient_cube = H_base_cube[:3,:3]
26
27# Position and orientation of camera with respect to base
28H_base_camera = H_2.inv() * H_z60
29pos_camera = H_base_camera[:3,3]
30orient_camera = H_base_camera[:3,:3]
31
32# Position and orientation of cube-2 with respect to base
33H_x90 = Matrix([[1, 0, 0, 0], 
34                [0, sp.cos(theta), -sp.sin(theta), 0], 
35                [0, sp.sin(theta), sp.cos(theta), 0], 
36                [0, 0, 0, 1]])
37trans_y5 = Matrix([[1, 0, 0, 0], 
38                   [0, 1, 0, 5], 
39                   [0, 0, 1, 0], 
40                   [0, 0, 0, 1]])
41H_base_cube2 = H_base_cube * H_x90 * trans_y5
42pos_cube2 = H_base_cube2[:3,3]
43orient_cube2 = H_base_cube2[:3,:3]
44
45# Print results
46print("Position and orientation of cube with respect to base:")
47sp.pprint(pos_cube)
48sp.pprint(orient_cube)
49print("\nPosition and orientation of camera with respect to base:")
50sp.pprint(pos_camera)
51sp.pprint(orient_camera)
52print("\nPosition and orientation of cube-2 with respect to base:")
53sp.pprint(pos_cube2)
54sp.pprint(orient_cube2)

Idea

Example 4

A six-joint robotic manipulator equipped with a digital television camera is capable of continuously monitoring the position and orientation of an object. The position and orientation of the object with respect to the camera are expressed by a matrix \((T_1)\), the origin of the robot’s base coordinate with respect to the camera is given by \((T_2)\), and the position and orientation of the gripper with respect to the base coordinate frame are given by \((T_3)\).
Determine,
The position and orientation of the object with respect to the base coordinate system.
The position and orientation of the object with respect to the gripper.

\[\begin{aligned} T_1 = \left[\begin{matrix}0 & 1 & 0 & 4\\1 & 0 & 0 & 3\\0 & 0 & -1 & 7\\0 & 0 & 0 & 1\end{matrix}\right], T_2 = \left[\begin{matrix}1 & 0 & 0 & -15\\0 & -1 & 0 & 5\\0 & 0 & -1 & 6\\0 & 0 & 0 & 1\end{matrix}\right], T_3 = \left[\begin{matrix}1 & 0 & 0 & 7\\0 & 1 & 0 & 2\\0 & 0 & 1 & 3\\0 & 0 & 0 & 1 \end{matrix}\right] \end{aligned}\]
Solution:

The position and orientation of the object with respect to the base coordinate system.

\[\begin{aligned} % \left[{ }^{\text{base}}T_\text{object} \right] \text{ by chain product rule, } \\ { }^\text{base}T_{\text {object}} &={ }^{\text {base}} T_{\text {camera}} . { }^{\text {camera}} T_{\text {object}} \\ &=\left[T_{2}\right]^{-1} \cdot\left[T_{1}\right] \\ &=\left[\begin{matrix}1 & 0 & 0 & 15\\0 & -1 & 0 & 5\\0 & 0 & -1 & 6\\0 & 0 & 0 & 1\end{matrix}\right] \left[\begin{matrix}0 & 1 & 0 & 4\\1 & 0 & 0 & 3\\0 & 0 & -1 & 7\\0 & 0 & 0 & 1\end{matrix}\right] \\ &=\left[\begin{matrix}0 & 1 & 0 & 19\\-1 & 0 & 0 & 2\\0 & 0 & 1 & -1\\0 & 0 & 0 & 1\end{matrix}\right] \\ &\text{The position vector} =\left[\begin{matrix}19\\2\\-1\end{matrix}\right] \\ &\text{The orientation matrix, } \\ &[n, s, a]=\left[\begin{matrix}0 & 1 & 0\\-1 & 0 & 0\\0 & 0 & 1\end{matrix}\right] \\ \end{aligned}\]

The position and orientation of the object with respect to the gripper.

\[\begin{aligned} { }^\text {gripper} T_{\text {object}} &= { }^\text{gripper}T_{\text {base }} \cdot{ }^{\text {base }} T_{\text {object }}=\left[T_{3}\right]^{-1}\left(\left[T_{2}\right]^{-1}\left[T_{1}\right]\right) \\ & =\left[\begin{matrix}1 & 0 & 0 & -7\\0 & 1 & 0 & -2\\0 & 0 & 1 & -3\\0 & 0 & 0 & 1\end{matrix}\right] \left[\begin{matrix}0 & 1 & 0 & 19\\-1 & 0 & 0 & 2\\0 & 0 & 1 & -1\\0 & 0 & 0 & 1\end{matrix}\right] \\ &= \left[\begin{matrix}0 & 1 & 0 & 12\\-1 & 0 & 0 & 0\\0 & 0 & 1 & -4\\0 & 0 & 0 & 1\end{matrix}\right] \\ &\text{The position vector } =\left[\begin{matrix}12\\0\\-4\end{matrix}\right] \\ &\text{The orientation vectors, }\\ &[n, s, a]=\left[\begin{matrix}0 & 1 & 0\\-1 & 0 & 0\\0 & 0 & 1\end{matrix}\right] \\ \end{aligned}\]
The following is a Python code implementation of the above calculation (Click to reveal).

 1# orcid.org/0000-0002-7326-7502
 2import sympy as sp
 3from sympy.matrices import Matrix
 4
 5# Homogeneous transformation matrices
 6H_1 = Matrix([[0, 1, 0, 4], 
 7              [1, 0, 0, 3], 
 8              [0, 0, -1, 7], 
 9              [0, 0, 0, 1]])
10H_2 = Matrix([[1, 0, 0, -15], 
11              [0, -1, 0, 5], 
12              [0, 0, -1, 6], 
13              [0, 0, 0, 1]])
14H_3 = Matrix([[1, 0, 0, 7], 
15              [0, 1, 0, 2], 
16              [0, 0, 1, 3], 
17              [0, 0, 0, 1]])
18
19# Position and orientation of object with respect to base
20H_base_object = H_2.inv() * H_1
21pos_object = H_base_object[:3,3]
22orient_object = H_base_object[:3,:3]
23
24# Position and orientation of object with respect to gripper
25H_gripper_object = H_3.inv() * H_base_object
26pos_object_gripper = H_gripper_object[:3,3]
27orient_object_gripper = H_gripper_object[:3,:3]
28
29# Print results
30print("Position and orientation of object with respect to base:")
31sp.pprint(pos_object)
32sp.pprint(orient_object)
33print("\nPosition and orientation of object with respect to gripper:")
34sp.pprint(pos_object_gripper)
35sp.pprint(orient_object_gripper)

Idea

Example 5

A triangular prism with the coordinates of its vertices \(A(1, 3, 0)\), \(B(-1, 3, 0)\), \(C(-1, 3, 2)\), \(D(1, 3, 2)\), \(E(1, 5, 2)\) and \(F(-1, 5, 2)\) relative to the fixed reference frame \(OXYZ\) is shown in Figure 7. The prism is moved to the new position with a rotation of \(+60^\circ\) about the \(X\) axis, a rotation of \(-60^\circ\) about the \(Z\) axis and a translation of 5 units in the \(Y\) direction.
Determine,
The homogeneous transformation that describes the change in position of the prism.
The new coordinates of the vertices \(A,B,C,D,E\) of the prism.

prism-robot
Figure 6. Prism bot.
Solution:

The homogeneous transformation that describes the change in position of the prism.

\[\begin{aligned} %\iota H &= H_{t}(0,5,0) H\left(z,-60^{\circ}\right) H\left(x,+60^{\circ}\right) . I_{4} \\ &=\left[\begin{array}{llll}1 & 0 & 0 & 0 \\0 & 1 & 0 & 5 \\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1\end{array}\right]\left[\begin{array}{cccc}\cos \left(-60^{\circ}\right) & -\sin \left(-60^{\circ}\right) & 0 & 0 \\\sin \left(-60^{\circ}\right) & \cos \left(-60^{\circ}\right) & 0 & 0 \\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1\end{array}\right] \\ &\times\left[\begin{array}{cccc}1 & 0 & 0 & 0 \\0 & \cos 60^{\circ} & -\sin 60^{\circ} & 0 \\0 & \sin 60^{\circ} & \cos 60^{\circ} & 0 \\0 & 0 & 0 & 1\end{array}\right] \\ & =\left[\begin{matrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 5\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1 \end{matrix}\right] \left[\begin{matrix}\frac{1}{2} & \frac{\sqrt{3}}{2} & 0 & 0\\- \frac{\sqrt{3} }{2} & \frac{1}{2} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right] \left[\begin{matrix}1 & 0 & 0 & 0\\0 & \frac{1}{2} & - \frac{\sqrt{3}}{2} & 0\\0 & \frac{\sqrt{3}}{2} & \frac{1}{2} & 0\\0 & 0 & 0 & 1\end{matrix}\right] \\ & =\left[\begin{matrix}\frac{1}{2} & \frac{\sqrt{3}}{4} & - \frac{3}{4} & 0\\- \frac{\sqrt{3}}{2} & \frac{1}{4} & - \frac{\sqrt{3}}{4} & 5\\0 & \frac{\sqrt{3}} {2} & \frac{1}{2} & 0\\0 & 0 & 0 & 1\end{matrix}\right] \\ \end{aligned}\]

The new coordinates of the vertices \(A,B,C,D,E\) of the prism.

\[\begin{aligned} \left[\begin{array}{c} \mathrm{A}_{x} \\ \mathrm{~A}_{y} \\ \mathrm{~A}_{z} \\ 1 \end{array}\right] &=\left[\begin{matrix}\frac{1}{2} & \frac{\sqrt{3}}{4} & - \frac{3}{4} & 0\\- \frac{\sqrt{3}}{2} & \frac{1}{4} & - \frac{\sqrt{3}}{4} & 5\\0 & \frac{\sqrt{3}} {2} & \frac{1}{2} & 0\\0 & 0 & 0 & 1\end{matrix}\right]\left[\begin{array}{l} q_{x} \\ q_{y} \\ q_{z} \\ 1 \end{array}\right] \\ \mathrm{A} &= [\mathrm{H}][q] \\ \left[\begin{array}{c} \mathrm{A}_{x} \\ \mathrm{~A}_{y} \\ \mathrm{~A}_{z} \\ 1 \end{array}\right] &= \left[\begin{matrix}\frac{1}{2} & \frac{\sqrt{3}}{4} & - \frac{3}{4} & 0\\- \frac{\sqrt{3}}{2} & \frac{1}{4} & - \frac{\sqrt{3}}{4} & 5\\0 & \frac{\sqrt{3}} {2} & \frac{1}{2} & 0\\0 & 0 & 0 & 1\end{matrix}\right]\left[\begin{array}{l} 1 \\ 3 \\ 0 \\ 1 \end{array}\right] = \left[\begin{matrix}\frac{1}{2} + \frac{3 \sqrt{3}}{4}\\\frac{23}{4} - \frac{\sqrt{3}}{2}\\\frac{3 \sqrt{3}}{2}\\1\end{matrix}\right] \\ [\mathrm{B}] &= [\mathrm{H}][b] \\ \left[\begin{array}{c} \mathrm{B}_{x} \\ \mathrm{~B}_{y} \\ \mathrm{~B}_{z} \\ 1 \end{array}\right] &= \left[\begin{matrix}\frac{1}{2} & \frac{\sqrt{3}}{4} & - \frac{3}{4} & 0\\- \frac{\sqrt{3}}{2} & \frac{1}{4} & - \frac{\sqrt{3}}{4} & 5\\0 & \frac{\sqrt{3}} {2} & \frac{1}{2} & 0\\0 & 0 & 0 & 1\end{matrix}\right]\left[\begin{array}{r} -1 \\ 3 \\ 0 \\ 1 \end{array}\right] = \left[\begin{matrix}- \frac{1}{2} + \frac{3 \sqrt{3}}{4}\\\frac{\sqrt{3}}{2} + \frac{23}{4}\\\frac{3 \sqrt{3}}{2}\\1\end{matrix}\right] \\ {[C] } &=[H][c] \\ &=\left[\begin{matrix}\frac{1}{2} & \frac{\sqrt{3}}{4} & - \frac{3}{4} & 0\\- \frac{\sqrt{3}}{2} & \frac{1}{4} & - \frac{\sqrt{3}}{4} & 5\\0 & \frac{\sqrt{3}} {2} & \frac{1}{2} & 0\\0 & 0 & 0 & 1\end{matrix}\right]\left[\begin{array}{r} -1 \\ 3 \\ 2 \\ 1 \end{array}\right] = \left[\begin{matrix}-2 + \frac{3 \sqrt{3}}{4}\\ \frac{23}{4}\\1 + \frac{3 \sqrt{3}}{2}\\1\end{matrix}\right] \\ {[D] } &=[\mathrm{H}][d] \\ &=\left[\begin{matrix}\frac{1}{2} & \frac{\sqrt{3}}{4} & - \frac{3}{4} & 0\\- \frac{\sqrt{3}}{2} & \frac{1}{4} & - \frac{\sqrt{3}}{4} & 5\\0 & \frac{\sqrt{3}} {2} & \frac{1}{2} & 0\\0 & 0 & 0 & 1\end{matrix}\right]\left[\begin{array}{l} 1 \\ 3 \\ 2 \\ 1 \end{array}\right] = \left[\begin{matrix}-1 + \frac{3 \sqrt{3}}{4}\\ \frac{23}{4} - \sqrt{3}\\1 + \frac{3 \sqrt{3}}{2}\\1\end{matrix}\right] \\ {[\mathrm{E}] } &=[\mathrm{H}][e] \\ &=\left[\begin{matrix}\frac{1}{2} & \frac{\sqrt{3}}{4} & - \frac{3}{4} & 0\\- \frac{\sqrt{3}}{2} & \frac{1}{4} & - \frac{\sqrt{3}}{4} & 5\\0 & \frac{\sqrt{3}} {2} & \frac{1}{2} & 0\\0 & 0 & 0 & 1\end{matrix}\right]\left[\begin{array}{l} 1 \\ 5 \\ 2 \\ 1 \end{array}\right] = \left[\begin{matrix}-1 + \frac{5 \sqrt{3}}{4}\\ \frac{25}{4} - \sqrt{3}\\1 + \frac{5 \sqrt{3}}{2}\\1\end{matrix}\right] \\ \end{aligned}\]
The following is a Python code implementation of the above calculation (Click to reveal).

 1# orcid.org/0000-0002-7326-7502
 2import sympy as sp
 3from sympy.matrices import Matrix
 4
 5# Initialization
 6trans_y = Matrix([[1, 0, 0, 0],
 7                [0, 1, 0, 5],
 8                [0, 0, 1, 0],
 9                [0, 0, 0, 1]])
10angle = 60
11theta = sp.pi*angle/180
12H_z60 = Matrix([[sp.cos(-theta), -sp.sin(-theta), 0, 0],
13                [sp.sin(-theta), sp.cos(-theta), 0, 0],
14                [0, 0, 1, 0], [0, 0, 0, 1]])
15H_x60 = Matrix([[1, 0, 0, 0],
16                [0, sp.cos(theta), -sp.sin(theta), 0],
17                [0, sp.sin(theta), sp.cos(theta), 0],
18                [0, 0, 0, 1]])
19
20# Calculate homogeneous transformation matrix
21H = trans_y * H_z60 * H_x60
22
23# Calculate new coordinates of vertices
24vertex_a = Matrix([1, 3, 0, 1])
25vertex_b = Matrix([-1, 3, 0, 1])
26vertex_c = Matrix([-1, 3, 2, 1])
27vertex_d = Matrix([1, 3, 2, 1])
28vertex_e = Matrix([1, 5, 2, 1])
29vertex_f = Matrix([-1, 5, 2, 1])
30
31new_a = H * vertex_a
32new_b = H * vertex_b
33new_c = H * vertex_c
34new_d = H * vertex_d
35new_e = H * vertex_e
36new_f = H * vertex_f
37
38# Print results
39print("New coordinates of A:")
40sp.pprint(new_a)
41print("New coordinates of B:")
42sp.pprint(new_b)
43print("New coordinates of C:")
44sp.pprint(new_c)
45print("New coordinates of D:")
46sp.pprint(new_d)
47print("New coordinates of E:")
48sp.pprint(new_e)
49print("New coordinates of F:")
50sp.pprint(new_f)

Idea

Assignment 1

Compute the homogeneous transformation matrices for the coordinate frames situated at the points \(A(10, 10, 0)\), \(B(5, -20, 5)\) and \(C(0, 6, 6)\) with respect to the \(OX_0Y_0Z_0\) frame as shown in Figure 8.
Evaluate, by inspection and matrix operation, the position and orientation of frame \(B\) with respect to frame \(C\)

coordinate-frames
Figure 7. Coordinate frames.
Solution:

Please attempt this assignment.

The following is a Python code implementation of the above calculation (Click to reveal).

 1# orcid.org/0000-0002-7326-7502
 2from sympy import *
 3
 4# Define the identity matrix
 5I = eye(4)
 6# Homogeneous transformation for A
 7translation_A = Matrix([[1, 0, 0, 10], 
 8                        [0, 1, 0, 10], 
 9                        [0, 0, 1, 0], 
10                        [0, 0, 0, 1]])
11transformation_A = translation_A * I
12
13print("Transformation matrix for A:")
14pprint(transformation_A)
15
16# Homogeneous transformation for B
17# To reach point B, perform the following operations:
18# Rotation about Y by angle -180 degrees (clockwise)
19# Rotation about Z by angle 90 degrees (counterclockwise)
20# Translation to position (5, -20, 5)
21
22angle_Y = -180
23angle_Y = pi*angle_Y/180
24angle_Z = 90
25angle_Z = pi*angle_Z/180
26
27rotation_Y = Matrix([[cos(angle_Y), 0, sin(angle_Y), 0], 
28                     [0, 1, 0, 0], 
29                     [-sin(angle_Y), 0, cos(angle_Y), 0], 
30                     [0, 0, 0, 1]])
31rotation_Z = Matrix([[cos(angle_Z), -sin(angle_Z), 0, 0], 
32                     [sin(angle_Z), cos(angle_Z), 0, 0], 
33                     [0, 0, 1, 0], 
34                     [0, 0, 0, 1]])
35translation_B = Matrix([[1, 0, 0, 5], 
36                        [0, 1, 0, -20], 
37                        [0, 0, 1, 5], 
38                        [0, 0, 0, 1]])
39
40transformation_B = translation_B * rotation_Z * rotation_Y * I
41print("Transformation matrix for B:")
42pprint(transformation_B)
43
44# Homogeneous transformation for C
45# Rotation of 90 degrees about Y (counterclockwise)
46# Rotation of 90 degrees about Z (counterclockwise)
47# Translation by (0,6,6) from origin
48
49angle_Y = 90
50angle_Y = pi*angle_Y/180
51angle_Z = 90
52angle_Z = pi*angle_Z/180
53
54rotation_Y = Matrix([[cos(angle_Y), 0, sin(angle_Y), 0], 
55                     [0, 1, 0, 0], 
56                     [-sin(angle_Y), 0, cos(angle_Y), 0], 
57                     [0, 0, 0, 1]])
58rotation_Z = Matrix([[cos(angle_Z), -sin(angle_Z), 0, 0], 
59                     [sin(angle_Z), cos(angle_Z), 0, 0], 
60                     [0, 0, 1, 0], 
61                     [0, 0, 0, 1]])
62translation_C = Matrix([[1, 0, 0, 0], 
63                        [0, 1, 0, 6], 
64                        [0, 0, 1, 6], 
65                        [0, 0, 0, 1]])
66
67transformation_C = translation_C * rotation_Z * rotation_Y * I
68print("Transformation matrix for C:")
69pprint(transformation_C)
70
71# Position and orientation of frame B with respect to frame C
72# Using chain product rule 
73frame_B_in_C = transformation_C.inv() * transformation_B
74print("Transformation matrix for B with respect to C:")
75pprint(frame_B_in_C)
76
77# Extract the position and orientation of frame B with respect to frame C
78position_B_in_C = frame_B_in_C[:3, 3]
79orientation_B_in_C = frame_B_in_C[:3, :3]
80
81# Print the results
82print("Position of frame B with respect to frame C:")
83pprint(position_B_in_C)
84
85print("Orientation of frame B with respect to frame C:")
86pprint(orientation_B_in_C)

Idea

Assignment 2

Compute the homogeneous transformation matrices for the coordinate frames attached to the corners \(A\), \(B\), \(C\) and \(D\) with respect to the base coordinate frame \(O\).
Compute the transformation matrix for \(A\) with respect to \(C\) frame, and verify the same by finding the inverse.
The object frame is as shown in Figure 9. \(P=20,\ Q=25,\ R=20,\ S=40,\ T=50\)

coordinate-frames2
Figure 8. Coordinate frames.
Solution:

Please attempt this assignment.

The following is a Python code implementation of the above calculation (Click to reveal).

  1# orcid.org/0000-0002-7326-7502 
  2from sympy import *
  3
  4# Homogeneous transformation matrix for A
  5# The transformation involves the following sequence of operations:
  6# Rotation by 90 degrees about X
  7# Rotation by -180 degrees about Z
  8# Translation by (0,45,20) from origin
  9
 10#define the angles in degrees
 11theta_X = 90
 12theta_Z = -180
 13
 14#convert the angles to radians
 15theta_X = rad(theta_X)
 16theta_Z = rad(theta_Z)
 17
 18#initialize the rotation matrices using trigonometric functions
 19R_X = Matrix([[1, 0, 0, 0], 
 20              [0, cos(theta_X), -sin(theta_X), 0], 
 21              [0, sin(theta_X), cos(theta_X), 0], 
 22              [0, 0, 0, 1]])
 23R_Z = Matrix([[cos(theta_Z), -sin(theta_Z), 0, 0], 
 24              [sin(theta_Z), cos(theta_Z), 0, 0], 
 25              [0, 0, 1, 0], 
 26              [0, 0, 0, 1]])
 27
 28#initialize the translation matrices
 29T_A = Matrix([[1, 0, 0, 0], 
 30              [0, 1, 0, 45], 
 31              [0, 0, 1, 20], 
 32              [0, 0, 0, 1]])
 33
 34#initialize the identity matrix
 35I = eye(4)
 36
 37#calculate the homogeneous transformation matrix for A
 38print("Homogeneous transformation matrix for A:")
 39H_A = T_A * R_Z * R_X * I
 40pprint(H_A)
 41
 42# Homogeneous transformation matrix of B: 
 43# Rotation by -90 degrees about Z
 44# Translation by (45,−40,0)
 45
 46theta_Z = -90
 47theta_Z = rad(theta_Z)
 48R_Z = Matrix([[cos(theta_Z), -sin(theta_Z), 0, 0], 
 49              [sin(theta_Z), cos(theta_Z), 0, 0], 
 50              [0, 0, 1, 0], 
 51              [0, 0, 0, 1]])
 52T_B = Matrix([[1, 0, 0, 45], 
 53              [0, 1, 0, -40], 
 54              [0, 0, 1, 0], 
 55              [0, 0, 0, 1]])
 56
 57#calculate the homogeneous transformation matrix for B
 58print("Homogeneous transformation matrix for B:")
 59H_B = T_B * R_Z * I 
 60pprint(H_B)
 61
 62# Homogeneous transformation of matrix C: 
 63# Rotation of 90 degrees about X
 64# Rotation of 90 degrees about Z
 65# Translation by (−40,20,50)
 66
 67theta_X = 90
 68theta_Z = 90
 69theta_X = rad(theta_X)
 70theta_Z = rad(theta_Z)
 71R_X = Matrix([[1, 0, 0, 0], 
 72              [0, cos(theta_X), -sin(theta_X), 0], 
 73              [0, sin(theta_X), cos(theta_X), 0], 
 74              [0, 0, 0, 1]])
 75R_Z = Matrix([[cos(theta_Z), -sin(theta_Z), 0, 0], 
 76              [sin(theta_Z), cos(theta_Z), 0, 0], 
 77              [0, 0, 1, 0], 
 78              [0, 0, 0, 1]])
 79T_C = Matrix([[1, 0, 0, -40], 
 80              [0, 1, 0, 20], 
 81              [0, 0, 1, 50], 
 82              [0, 0, 0, 1]])
 83
 84#calculate the homogeneous transformation matrix for C
 85print("Homogeneous transformation matrix for C:")
 86H_C = T_C * R_Z * R_X * I
 87pprint(H_C)
 88
 89# Homogeneous transformation matrix for D: 
 90# Rotation by 90 degrees about Z
 91# Rotation by -90 degress about X 
 92# Translation of (0,0,50)
 93
 94theta_Z = 90
 95theta_X = -90
 96theta_Z = rad(theta_Z)
 97theta_X = rad(theta_X)
 98R_Z = Matrix([[cos(theta_Z), -sin(theta_Z), 0, 0], 
 99              [sin(theta_Z), cos(theta_Z), 0, 0], 
100              [0, 0, 1, 0], 
101              [0, 0, 0, 1]])
102R_X = Matrix([[1, 0, 0, 0], 
103              [0, cos(theta_X), -sin(theta_X), 0], 
104              [0, sin(theta_X), cos(theta_X), 0], 
105              [0, 0, 0, 1]])
106T_D = Matrix([[1, 0, 0, 0], 
107              [0, 1, 0, 0], 
108              [0, 0, 1, 50], 
109              [0, 0, 0, 1]])
110
111#calculate the homogeneous transformation matrix for D
112print("Homogeneous transformation matrix for D:")
113H_D = T_D * R_X * R_Z * I
114pprint(H_D)
115
116# Transformation matrix for A with respect to C frame 
117print("Homogeneous transformation for A with respect to C:")
118# [H_A wrt C] = [H_O_C] * [H_A_O] = [H_C_O]inv * [H_A_O]
119H_C_inv = H_C.inv()
120H_A_C = H_C_inv * H_A
121pprint(H_A_C)


Add Comment

* Required information
1000
Drag & drop images (max 3)
Enter the third word of this sentence.

Comments (4)

Avatar
New
Dr. Sam

Using the knowledge of spatial mechanics and computer programming introduced in this course, you can go on to perform advanced kinematic analysis of 3D mechanisms.

These skills apply to robotics systems design, research, and development.

Avatar
New
Joe Disamalu

The notes are on point. I've gained alot of skill and visualization from the unit.

Avatar
New
Aggrey

Does it mean the inverse of a 3x3 matrix same as its transpose

Upload
Avatar
New
Dr. Sam

In spatial mechanics, the inverse and transpose of a 3x3 matrix are not the same, but they can be related in specific cases. The inverse (A⁻¹) is a matrix that, when multiplied by A, results in the identity matrix (A * A⁻¹ = I). The transpose (Aᵀ) is obtained by interchanging the rows and columns of A.

For 3x3 orthogonal matrices, often used to represent rotations, the transpose is equal to the inverse (A⁻¹ = Aᵀ) because they satisfy Aᵀ * A = I. This property is unique to orthogonal matrices, and for general 3x3 matrices, the inverse and transpose are not the same.