\documentclass[11pt]{article}
\usepackage{fullpage}
\usepackage{setspace}
\usepackage{parskip}
\usepackage{titlesec}
\usepackage[section]{placeins}
\usepackage{xcolor}
\usepackage{breakcites}
\usepackage{lineno}
\usepackage{hyphenat}
\usepackage{times}
\PassOptionsToPackage{hyphens}{url}
\usepackage[colorlinks = true,
linkcolor = blue,
urlcolor = blue,
citecolor = blue,
anchorcolor = blue]{hyperref}
\usepackage{etoolbox}
\makeatletter
\patchcmd\@combinedblfloats{\box\@outputbox}{\unvbox\@outputbox}{}{%
\errmessage{\noexpand\@combinedblfloats could not be patched}%
}%
\makeatother
\renewenvironment{abstract}
{{\bfseries\noindent{\abstractname}\par\nobreak}\footnotesize}
{\bigskip}
\titlespacing{\section}{0pt}{*3}{*1}
\titlespacing{\subsection}{0pt}{*2}{*0.5}
\titlespacing{\subsubsection}{0pt}{*1.5}{0pt}
\usepackage{authblk}
\usepackage{graphicx}
\usepackage[space]{grffile}
\usepackage{latexsym}
\usepackage{textcomp}
\usepackage{longtable}
\usepackage{tabulary}
\usepackage{booktabs,array,multirow}
\usepackage{amsfonts,amsmath,amssymb}
\providecommand\citet{\cite}
\providecommand\citep{\cite}
\providecommand\citealt{\cite}
% You can conditionalize code for latexml or normal latex using this.
\newif\iflatexml\latexmlfalse
\providecommand{\tightlist}{\setlength{\itemsep}{0pt}\setlength{\parskip}{0pt}}%
\AtBeginDocument{\DeclareGraphicsExtensions{.pdf,.PDF,.eps,.EPS,.png,.PNG,.tif,.TIF,.jpg,.JPG,.jpeg,.JPEG}}
\usepackage[utf8]{inputenc}
\usepackage[english]{babel}
\newcommand{\msun}{\,\mathrm{M}_\odot}
\begin{document}
\title{Scientific Paper}
\author[1]{mohamed lamine talbi}%
\affil[1]{Affiliation not available}%
\vspace{-1em}
\date{\today}
\begingroup
\let\center\flushleft
\let\endcenter\endflushleft
\maketitle
\endgroup
\sloppy
2.7 Inverse of Transformation Matrices
As was mentioned earlier, there are many situations where the inverse of a matrix is needed in robotic analysis, as the following example shows. Suppose the robot in Figure \(2.18\) is to be moved to part \(P\) in order to drill a hole in the part. The robot's base position relative to the reference frame \(U\) is described by a frame \(R\), the robot's hand is described by frame \(H\), and the end effector (let's say the end of the drill bit that will be used to drill the hole) is described by frame \(E\). The part's position is described by frame \(P\). The location of the point
Figure 2.18 The Universe, robot, hand, part, and end effector frames.
where the hole will be drilled can be related to the reference frame \(U\) through two independent paths: one through the part, one through the robot. Therefore, the following equation can be written:
\begin{align*}
{ }^{U} T_{E}={ }^{U} T_{R}{ }^{R} T_{H}{ }^{H} T_{E}={ }^{U} T_{P}{ }^{P} T_{E}
\end{align*}
The location of point \(E\) on the part can be achieved by moving from \(U\) to \(P\) and from \(P\) to \(E\), or by a transformation from \(U\) to \(R\), from \(R\) to \(H\), and from \(H\) to \(E\).
In reality, the transformation of frame \(R\) relative to the Universe frame \(\left({ }^{U} T_{R}\right)\) is known, since the location of the robot's base must be known in any setup. For example, if a robot is installed in a workcell, the location of the robot's base is known since it is bolted to a table. Even if the robot is mobile or attached to a conveyor belt, its location at any instant is known because a controller must be following the position of the robot's base at all times. The \({ }^{H} T_{E}\), or the transformation of the end effector relative to the robot's hand, is also known since any tool used at the end effector is a known tool, and its dimensions and configuration are known. \({ }^{U} T_{P}\), or the transformation of the part relative to the Universe, is also known since we must know where the part is located if we are to drill a hole in it. This location is known by putting the part in a jig, through the use of a camera and vision system, through the use of a conveyor belt and sensors, or other similar devices. \({ }^{P} T_{E}\) is also known since we need to know where the hole is to be drilled on the part. Consequently, the only unknown transformation is \({ }^{R} T_{H}\), or the transformation of the robot's hand relative to the robot's base. This means we need to find the robot's joint variables (angles of revolute joints and lengths of prismatic joints of the robot) in order to position the end effector at the hole for drilling. Consequently, it is necessary to calculate this transformation, which tells us what needs to be accomplished. The transformation is later used to actually solve for joint angles and link lengths.
To calculate this matrix, unlike in an algebraic equation, we cannot simply divide the right side by the left side of the equation. We need to pre- or post-multiply by inverses of appropriate matrices to eliminate them. As a result, we have:
\begin{align*}
\left[{ }^{U} T_{R}\right]^{-1}\left[{ }^{U} T_{R}{ }^{R} T_{H}{ }^{H} T_{E}\right]\left[{ }^{H} T_{E}\right]^{-1}=\left[{ }^{U} T_{R}\right]^{-1}\left[{ }^{u} T_{P}{ }^{P} T_{E}\right]\left[{ }^{H} T_{E}\right]^{-1}
\end{align*}
or since \(\left({ }^{U} T_{R}\right)^{-1}\left({ }^{U} T_{R}\right)=I\) and \(\left({ }^{H} T_{E}\right)\left({ }^{H} T_{E}\right)^{-1}=I\), the left side of Eq. (2.26) simplifies to \({ }^{R} T_{H}\), and we get:
\begin{align*}
{ }^{R} T_{H}={ }^{U} T_{R}{ }^{-1 U} T_{P}{ }^{P} T_{E}{ }^{H} T_{E}{ }^{-1}
\end{align*}
We can check the accuracy of this equation by realizing that \({ }^{H} T_{E}{ }^{-1}\) is the same as \({ }^{E} T_{H}\). Therefore, the equation can be rewritten as:
\begin{align*}
{ }^{R} T_{H}={ }^{u} T_{R}{ }^{-1 U} T_{P}{ }^{P} T_{E}{ }^{H} T_{E}{ }^{-1}={ }^{R} T_{U}{ }^{U} T_{P}{ }^{p} T_{E}{ }^{E} T_{H}={ }^{R} T_{H}
\end{align*}
It is now clear that we need to be able to calculate the inverse of transformation matrices for kinematic analysis as well.
In order to see what transpires, let's calculate the inverse of a simple rotation matrix about the \(x\)-axis. Please review the process for calculation of square matrices in Appendix A. The rotation matrix about the \(x\)-axis is:
\begin{align*}
\operatorname{Rot}(x, \theta)=\left[\begin{array}{ccc}
1 & 0 & 0 \\
0 & C \theta & -S \theta \\
0 & S \theta & C \theta
\end{array}\right]
\end{align*}
Recall that the following steps must be taken to calculate the inverse of a matrix:
1) Calculate the determinant of the matrix.
2) Transpose the matrix.
3) Replace each element of the transposed matrix by its own minor (adjoint matrix).
4) Divide the adjoint matrix by the determinant.
Applying the process to the rotation matrix, we get:
\begin{align*}
\begin{array}{c}
\operatorname{det}[\operatorname{Rot}(x, \theta)]=1\left(C^{2} \theta+S^{2} \theta\right)+0=1 \\
\operatorname{Rot}(x, \theta)^{T}=\left[\begin{array}{ccc}
1 & 0 & 0 \\
0 & C \theta & S \theta \\
0 & -S \theta & C \theta
\end{array}\right]
\end{array}
\end{align*}
Now we calculate each minor. As an example, the minor for the 2,2 element is \(C \theta-0=C \theta\), the minor for the 1,1 element is \(C^{2} \theta+S^{2} \theta=1\), and so on. As you notice, for the rotation matrix, the minor for each element is the same as the element itself. Therefore,
\begin{align*}
\operatorname{Adj}[\operatorname{Rot}(x, \theta)]=\operatorname{Rot}(x, \theta)_{\operatorname{minor}}^{T}=\operatorname{Rot}(x, \theta)^{T}
\end{align*}
Since the determinant of the original rotation matrix is 1 , dividing the \(\operatorname{Adj}[\operatorname{Rot}(x, \theta)]\) matrix by the determinant yields the same matrix. Consequently, the inverse of a rotation matrix about the \(x\)-axis is the same as its transpose, or:
\begin{align*}
\operatorname{Rot}(x, \theta)^{-1}=\operatorname{Rot}(x, \theta)^{T}
\end{align*}
Of course, you would get the same result with the second method mentioned in Appendix A. A matrix with this characteristic is called a unitary matrix. It turns out that all rotation matrices are unitary matrices. Therefore, all we need to do to calculate the inverse of a rotation matrix is to transpose it. Verify that rotation matrices about the \(y\) - and \(z\)-axes are also unitary in nature. Be aware that only rotation matrices are unitary; if a matrix is not a simple rotation matrix, it may not be unitary.
The preceding result is true only for a simple \(3 \times 3\) rotation matrix. For a homogenous \(4 \times 4\) transformation matrix, it can be shown that the inverse of the matrix can be written by dividing the matrix into two portions: The rotation portion of the matrix can be simply transposed, as it is still unitary. The position portion of the homogeneous matrix is the negative of the dot product of the \(p\)-vector with each of the \(n - , o -\), and \(a\)-vectors, as follows:
\begin{align*}
T=\left[\begin{array}{cccc}
n_{x} & o_{x} & a_{x} & p_{x} \\
n_{y} & o_{y} & a_{y} & p_{y} \\
n_{z} & o_{z} & a_{z} & p_{z} \\
0 & 0 & 0 & 1
\end{array}\right] \text { and } T^{-1}=\left[\begin{array}{cccc}
n_{x} & n_{y} & n_{z} & - p \cdot n \\
o_{x} & o_{y} & o_{z} & - p \cdot o \\
a_{x} & a_{y} & a_{z} & - p \cdot a \\
0 & 0 & 0 & 1
\end{array}\right]
\end{align*}
As shown, the rotation portion of the matrix is simply transposed, the position portion is replaced by the negative of the dot products, and the last row (scale factors) is not affected. This is very helpful, since we will need to calculate inverses of transformation matrices, but direct calculation of \(4 \times 4\) matrices is a lengthy process.
Example \(2.13\) Calculate the matrix representing \(\operatorname{Rot}\left(x, 40^{\circ}\right)^{-1}\).
Solution:
The matrix representing a \(40^{\circ}\) rotation about the \(x\)-axis is:
\begin{align*}
\operatorname{Rot}\left(x, 40^{\circ}\right)=\left[\begin{array}{cccc}
1 & 0 & 0 & 0 \\
0 & 0.766 & -0.643 & 0 \\
0 & 0.643 & 0.766 & 0 \\
0 & 0 & 0 & 1
\end{array}\right]
\end{align*}
The inverse of this matrix is:
\begin{align*}
\operatorname{Rot}\left(x, 40^{\circ}\right)^{-1}=\left[\begin{array}{cccc}
1 & 0 & 0 & 0 \\
0 & 0.766 & 0.643 & 0 \\
0 & -0.643 & 0.766 & 0 \\
0 & 0 & 0 & 1
\end{array}\right]
\end{align*}
Since the position vector of the matrix is zero, its dot product with the \(n -, o -\), and a-vectors is also zero.
Example \(2.14\) Calculate the inverse of the given transformation matrix:
\begin{align*}
T=\left[\begin{array}{cccc}
0.5 & 0 & 0.866 & 3 \\
0.866 & 0 & -0.5 & 2 \\
0 & 1 & 0 & 5 \\
0 & 0 & 0 & 1
\end{array}\right]
\end{align*}
Solution:
Based on the previous discussion, the inverse of the transformation is:
\begin{align*}
T^{-1}=\left[\begin{array}{cccc}
0.5 & 0.866 & 0 & -(3 \times 0.5+2 \times 0.866+5 \times 0) \\
0 & 0 & 1 & -(3 \times 0+2 \times 0+5 \times 1) \\
0.866 & -0.5 & 0 & -(3 \times 0.866+2 \times-0.5+5 \times 0) \\
0 & 0 & 0 & 1
\end{array}\right]=\left[\begin{array}{cccc}
0.5 & 0.866 & 0 & -3.23 \\
0 & 0 & 1 & -5 \\
0.866 & -0.5 & 0 & -1.598 \\
0 & 0 & 0 & 1
\end{array}\right]
\end{align*}
You may want to verify that \(T T^{-1}\) is an identity matrix.
Example 2.15 In a robotic setup, a camera is attached to the fifth link of a 6-DOF robot. It observes an object and determines its frame relative to the camera's frame. Using the following information, determine the necessary motion the end effector must make to get to the object:
\begin{align*}
{ }^{5} T_{\text {cam }}=\left[\begin{array}{cccc}
0 & 0 & -1 & 3 \\
0 & -1 & 0 & 0 \\
-1 & 0 & 0 & 5 \\
0 & 0 & 0 & 1
\end{array}\right]{ }^{5} T_{H}=\left[\begin{array}{cccc}
0 & -1 & 0 & 0 \\
1 & 0 & 0 & 0 \\
0 & 0 & 1 & 4 \\
0 & 0 & 0 & 1
\end{array}\right]{ }^{\text {cam }} T_{\text {obj }}=\left[\begin{array}{llll}
0 & 0 & 1 & 2 \\
1 & 0 & 0 & 2 \\
0 & 1 & 0 & 4 \\
0 & 0 & 0 & 1
\end{array}\right]{ }^{H} T_{E}=\left[\begin{array}{llll}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & 3 \\
0 & 0 & 0 & 1
\end{array}\right]
\end{align*}
Solution:
Referring to Eq. (2.25), we can write a similar equation to relate the different transformations and frames to each other as:
\begin{align*}
{ }^{R} T_{5} \times{ }^{5} T_{H} \times{ }^{H} T_{E} \times{ }^{E} T_{o b j}={ }^{R} T_{5} \times{ }^{5} T_{c a m} \times{ }^{c a m} T_{o b j}
\end{align*}
Since \({ }^{R} T_{S}\) appears on both sides of the equation, we can simply neglect it. All other matrices, with the exception of \({ }^{E} T_{o b j}\), are known. Then:
\begin{align*}
{ }^{E} T_{o b j}={ }^{H} T_{E}{ }^{-1} \times{ }^{5} T_{H}{ }^{-1} \times{ }^{5} T_{c a m} \times{ }^{c a m} T_{a b j}={ }^{E} T_{H} \times{ }^{H} T_{5} \times{ }^{5} T_{c a m} \times{ }^{c a m} T_{o b j}
\end{align*}
where
\begin{align*}
{ }^{H} T_{E}{ }^{-1}=\left[\begin{array}{cccc}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & -3 \\
0 & 0 & 0 & 1
\end{array}\right]{ }^{5} T_{H}{ }^{-1}=\left[\begin{array}{cccc}
0 & 1 & 0 & 0 \\
-1 & 0 & 0 & 0 \\
0 & 0 & 1 & -4 \\
0 & 0 & 0 & 1
\end{array}\right]
\end{align*}
Substituting the matrices and the inverses in the equation representing \({ }^{E} T_{o b j}\) results in:
\begin{align*}
{ }^{E} T_{o b j}=\left[\begin{array}{cccc}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & -3 \\
0 & 0 & 0 & 1
\end{array}\right]\left[\begin{array}{cccc}
0 & 1 & 0 & 0 \\
-1 & 0 & 0 & 0 \\
0 & 0 & 1 & -4 \\
0 & 0 & 0 & 1
\end{array}\right]\left[\begin{array}{cccc}
0 & 0 & -1 & 3 \\
0 & -1 & 0 & 0 \\
-1 & 0 & 0 & 5 \\
0 & 0 & 0 & 1
\end{array}\right]\left[\begin{array}{llll}
0 & 0 & 1 & 2 \\
1 & 0 & 0 & 2 \\
0 & 1 & 0 & 4 \\
0 & 0 & 0 & 1
\end{array}\right]
\end{align*}
\begin{align*}
=\left[\begin{array}{cccc}
-1 & 0 & 0 & -2 \\
0 & 1 & 0 & 1 \\
0 & 0 & -1 & -4 \\
0 & 0 & 0 & 1
\end{array}\right]
\end{align*}
Example \(2.16\) The following transformation matrix \(F\) is the result of two rotations: one was \(\theta^{\bullet}\) about the \(z\)-axis, but it is unclear whether this was the first or second rotation. Determine the axis of rotation of \(\alpha\) and whether it was performed first or second.
\begin{align*}
T=\left[\begin{array}{ccc}
C \theta & -S \theta C \alpha & S \theta S \alpha \\
S \theta & C \theta C \alpha & -C \theta S \alpha \\
0 & S \alpha & C \alpha
\end{array}\right]
\end{align*}
Solution:
To do this, we need to decouple the two rotations by pre-multiplying the transformation by \(\operatorname{Rot}(z, \theta)^{-1}\). But since we do not know the order of rotations, we first assume the rotation were about the reference frame and that the rotation about the z-axis was performed first. We write:
\begin{align*}
\begin{array}{l}
T=\operatorname{Rot}(?, \alpha) \operatorname{Rot}(z, \theta) \\
T \times \operatorname{Rot}(z, \theta)^{-1}=\operatorname{Rot}(?, \alpha) \\
{\left[\begin{array}{ccc}
C \theta & -\operatorname{S} \theta \operatorname{C\alpha} & S \theta S \alpha \\
S \theta & C \theta C \alpha & -C \theta S \alpha \\
0 & S \alpha & C \alpha
\end{array}\right]\left[\begin{array}{ccc}
C \theta & S \theta & 0 \\
-S \theta & C \theta & 0 \\
0 & 0 & 1
\end{array}\right]=\left[\begin{array}{ccc}
C^{2} \theta+S^{2} \theta C \alpha & S \theta C \theta-S \theta C \theta C \alpha & S \theta S \alpha \\
S \theta C \theta-S \theta C \theta C \alpha & S^{2} \theta+C^{2} \theta C \alpha & -C \theta S \alpha \\
-S \theta S \alpha & S \alpha C \theta & C \alpha
\end{array}\right]}
\end{array}
\end{align*}
Obviously, this is not a satisfactory answer. Assuming that rotation about the \(z\)-axis was performed second, we write:
\begin{align*}
\begin{array}{l}
T=\operatorname{Rot}(z, \theta) \operatorname{Rot}(?, \alpha) \\
\operatorname{Rot}(z, \theta)^{-1} \times T=\operatorname{Rot}(?, \alpha) \\
{\left[\begin{array}{ccc}
C \theta & S \theta & 0 \\
-S \theta & C \theta & 0 \\
0 & 0 & 1
\end{array}\right]\left[\begin{array}{ccc}
C \theta & -S \theta C \alpha & S \theta S \alpha \\
S \theta & C \theta C \alpha & -C \theta S \alpha \\
0 & S \alpha & C \alpha
\end{array}\right]} \\
=\left[\begin{array}{ccc}
C^{2} \theta+S^{2} \theta & -S \theta C \theta C \alpha+S \theta C \theta C \alpha & S \theta C \theta S \alpha-S \theta C \theta S \alpha \\
-S \theta C \theta+S \theta C \theta & S^{2} \theta C \alpha+C^{2} \theta C \alpha & -S^{2} \theta S \alpha-C^{2} \theta S \alpha \\
& S \alpha & C \alpha
\end{array}\right] \\
=\left[\begin{array}{ccc}
1 & 0 & 0 \\
0 & C \alpha & -S \alpha \\
0 & S \alpha & C \alpha
\end{array}\right]
\end{array}
\end{align*}
This is clearly a first rotation about the \(x\)-axis or a second rotation about the \(n\)-axis.
2.8 Forward and Inverse Kinematics of Robots
Suppose we have a robot whose configuration is known. This means that all the link lengths and joint angles are known. Calculating the position and orientation of the hand of the robot is called forward kinematic analysis. In other words, if all robot joint variables are known, using forward kinematic equations, we can calculate, where the robot is at any instant. However, if we want to place the hand of the robot at a desired location and orientation, we need to know how much each link length or joint angle must be such that at those values the hand will be at the desired position and orientation. This is called inverse kinematic analysis. This means that instead of substituting the known robot variables in the forward kinematic equations of the robot, we need to find the inverse of these equations to enable us to find the necessary joint values to place the robot at the desired location and orientation. In reality, the inverse kinematic equations are far more important to enable the robot controller to calculate the joint values using these equations and to run the robot to the desired position and orientation. Later, we first develop the forward kinematic equations of robots; then, using these, equations, we derive the inverse kinematic equations.
You may recall from Chapter 1 that in order to position and orient a rigid body in space, we attach a frame, to the body and specify the position of the origin of the frame and the orientation of its 3 axes. This requires a total of 6 DOF or, alternately, six pieces of information, to completely define the position and orientation of the body. Here too, if we want to define or find the position and orientation of the hand of a robot in space, we attach a frame to it and define the position and orientation of the hand frame of the robot. For a particular
Figure 2.19 The hand frame of the robot relative to the reference frame.
configuration of the links and joints of a robot, a particular set of equations relate the hand frame to the reference frame. Figure \(2.19\) shows a hand frame, the reference frame, and their relative positions and orientations. The undefined connection between the two frames is related to the configuration of the robot. Of course, there are many different possibilities for this configuration, and we will later see how we can develop the equations relating the two frames depending on the robot configuration.
In order to simplify the process, we analyze the position and orientation issues separately. First we develop the position equations, and then the orientation equations. Later, we combine the two for a complete set of equations. Finally, we will see about the use of the Denavit-Hartenberg representation, which is an encompassing representation for any robot configuration.
2.9 Forward and Inverse Kinematic Equations: Position
In this section, we study the forward and inverse kinematic equations for position. As was mentioned earlier, the position of the origin of a frame attached to a rigid body has 3 DOF, and hence it can be completely defined by three pieces of information in any customary coordinate system, including:
- Cartesian coordinates (three linear motions)
- Cylindrical coordinates (two linear motions and one rotation)
- Spherical coordinates (two rotations and one linear motion)
- Articulated coordinates (three rotations)
We consider all four possibilities.
2.9.1 Cartesian (Gantry, Rectangular) Coordinates
In this case, there are three linear motions along the \(x-, y-\), and \(z\)-axes. In a Cartesian robot, all actuators are linear (such as a hydraulic ram or a linear power screw), and the positioning of the hand of the robot is accomplished by moving the three linear joints along the 3 axes (Figure \(2.20\) ). A gantry robot is a Cartesian robot, usually attached to a gantry frame upside down.
Of course, since there are no rotations, the transformation matrix representing this motion to point \(p\) is a simple translation matrix. Remember that here we are only concerned with the position of the origin of the frame, and not its orientation (which we will see next). The transformation matrix representing the forward kinematic equation of the position of the hand frame in a Cartesian coordinate system is:
\({ }^{R} T_{p}=T_{c a r t}\left(p_{x}, p_{y}, p_{z}\right)=\left[\begin{array}{cccc}1 & 0 & 0 & p_{x} \\ 0 & 1 & 0 & p_{y} \\ 0 & 0 & 1 & p_{z} \\ 0 & 0 & 0 & 1\end{array}\right]\)
Figure 2.20 Cartesian coordinates.
where \({ }^{R} T_{P}\) is the transformation between the reference frame and the origin of the hand \(p\), and \(T_{\text {cart }}\left(p_{x} p_{y} p_{z}\right)\) denotes Cartesian transformation matrix. All that is needed for the inverse kinematic solution is simply setting the desired position equal to \(p\).
Example \(2.17\) We desire to position the origin of the hand frame of a Cartesian robot at point \(p=[5,4,6]^{T}\). Calculate the necessary Cartesian coordinate motions that need to be made.
Solution:
Setting the forward kinematic equation, represented by the \({ }^{R} T_{P}\) matrix of Eq. (2.32), equal to the desired position yields:
\begin{align*}
{ }^{R} T_{p}=\left[\begin{array}{cccc}
1 & 0 & 0 & p_{x} \\
0 & 1 & 0 & p_{y} \\
0 & 0 & 1 & p_{z} \\
0 & 0 & 0 & 1
\end{array}\right]=\left[\begin{array}{cccc}
1 & 0 & 0 & 5 \\
0 & 1 & 0 & 4 \\
0 & 0 & 1 & 6 \\
0 & 0 & 0 & 1
\end{array}\right] \quad \text { or } p_{x}=5, p_{y}=4, p_{z}=6
\end{align*}
2.9.2 Cylindrical Coordinates
A cylindrical coordinate system includes two linear translations and one rotation. The sequence is a translation of \(r\) along the \(x\)-axis, a rotation of \(\alpha\) about the z-axis, and a translation of \(l\) along the \(z\)-axis, as shown in Figure \(2.21\). Since these transformations are all relative to the Universe frame, the total transformation caused by these three transformations is found by pre-multiplying by each matrix, as follows:
Figure \(2.21\) Cylindrical coordinates.
\begin{align*}
\begin{array}{c}
{ }^{R} T_{p}=T_{c y l}(r, \alpha, l)=\operatorname{Trans}(0,0, l) \operatorname{Rot}(z, \alpha) \operatorname{Trans}(r, 0,0) \\
{ }^{R} T_{p}=\left[\begin{array}{llll}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & l \\
0 & 0 & 0 & 1
\end{array}\right] \times\left[\begin{array}{cccc}
C \alpha & -S \alpha & 0 & 0 \\
S \alpha & C \alpha & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{array}\right] \times\left[\begin{array}{llll}
1 & 0 & 0 & r \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{array}\right] \\
{ }^{R} T_{p}=T_{c y l}(r, \alpha, l)=\left[\begin{array}{cccc}
C \alpha & -S \alpha & 0 & r C \alpha \\
S \alpha & C \alpha & 0 & r S \alpha \\
0 & 0 & 1 & l \\
0 & 0 & 0 & 1
\end{array}\right]
\end{array}
\end{align*}
The first three columns represent the orientation of the frame after this series of transformations. However, at this point we are only interested in the position of the origin of the frame, or the last column. Obviously, in cylindrical coordinate movements, due to the rotation of \(\alpha\) about the z-axis, the orientation of the moving frame changes. This orientation change will be discussed later.
You may restore the original orientation of the frame by rotating the \(n, o, a\) frame about the \(a\)-axis an angle of \(-\alpha\), which is equivalent to post-multiplying the cylindrical coordinate matrix by a rotation matrix of \(\operatorname{Rot}(a,-\alpha)\). As a result, the frame will be at the same location, but will be parallel to the reference frame again, as follows:
\begin{align*}
T_{c y l} \times \operatorname{Rot}(a,-\alpha)=\left[\begin{array}{cccc}
C \alpha & -S \alpha & 0 & r C \alpha \\
S \alpha & C \alpha & 0 & r S \alpha \\
0 & 0 & 1 & l \\
0 & 0 & 0 & 1
\end{array}\right] \times\left[\begin{array}{cccc}
C(-\alpha) & -S(-\alpha) & 0 & 0 \\
S(-\alpha) & C(-\alpha) & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{array}\right]=\left[\begin{array}{cccc}
1 & 0 & 0 & r C \alpha \\
0 & 1 & 0 & r S \alpha \\
0 & 0 & 1 & l \\
0 & 0 & 0 & 1
\end{array}\right]
\end{align*}
Clearly, the location of the origin of the moving frame did not change, but the orientation was restored to being parallel to the reference frame. Notice that the last rotation was performed about the local \(a\)-axis in order to not cause any change in the position of the frame, but only in its orientation.
For inverse kinematic equations, we equate the position terms of Eq. (2.34) to the desired values. However, as is discussed in Appendix A, it is very important to determine in which quadrant the actual answer is; otherwise the result will be incorrect. Throughout the book, whenever possible, we try to find the sine and the cosine of the angle independently and use the ATAN2 function for correct answer. Unfortunately, in many instances this is not possible. For cylindrical coordinates, we get:
\begin{align*}
\left.\begin{array}{l}
r C \alpha=P_{x} \\
r S \alpha=P_{y} \\
l=P_{z}
\end{array}\right\} \rightarrow \alpha=A \operatorname{TAN} 2\left(P_{y}, P_{x}\right)
\end{align*}
Example 2.18 Suppose we desire to place the origin of the hand frame of a cylindrical robot at \([4,5,6]^{T}\). Calculate the joint variables of the robot.
Solution:
Using Eq. (2.35), we get:
\begin{align*}
l=6
\end{align*}
\(r C \alpha=5\) and \(r S \alpha=4\), and therefore, \(\alpha=\operatorname{ATAN} 2(4,5)=38.7^{\circ}\)
Substituting \(\alpha\) into either equation yields \(r=6.4\).
The final answer is \(r=6.4\) units, \(\alpha=38.7^{\circ}\), and \(l=6\) units.
Example \(2.19\) The position and restored orientation of a cylindrical robot are given. Find the matrix representing the original position and orientation of the robot before it was restored.
\begin{align*}
T=\left[\begin{array}{cccc}
1 & 0 & 0 & -2.394 \\
0 & 1 & 0 & 6.578 \\
0 & 0 & 1 & 9 \\
0 & 0 & 0 & 1
\end{array}\right]
\end{align*}
Solution:
Since \(r\) is always positive, it is clear that \(S \alpha\) and \(C \alpha\) are positive and negative, respectively. Therefore, \(\alpha\) is in the second quadrant. From \(T\), we get
\begin{align*}
\begin{array}{l}
l=9 \\
\alpha=A T A N 2(6.578,-2.394)=110^{\circ} \\
r \sin (\alpha)=6.578 \rightarrow r=7
\end{array}
\end{align*}
Substituting these values into Eq. (2.34) yields the original orientation of the robot:
\begin{align*}
{ }^{R} T_{p}=\left[\begin{array}{cccc}
C \alpha & -S \alpha & 0 & r C \alpha \\
S \alpha & C \alpha & 0 & r S \alpha \\
0 & 0 & 1 & l \\
0 & 0 & 0 & 1
\end{array}\right]=\left[\begin{array}{cccc}
-0.342 & -0.9397 & 0 & -2.394 \\
0.9397 & -0.342 & 0 & 6.578 \\
0 & 0 & 1 & 9 \\
0 & 0 & 0 & 1
\end{array}\right]
\end{align*}
2.9.3 Spherical Coordinates
A spherical coordinate system consists of one linear motion and two rotations. The sequence we study here is a translation of \(r\) along the \(z\)-axis, a rotation of \(\beta\) about the \(y\)-axis, and a rotation of \(\gamma\) about the \(z\)-axis, as shown in Figure \(2.22\). Since these transformations are all relative to the Universe frame, the total transformation can be found by pre-multiplying by each matrix, as follows:
\begin{align*}
{ }^{R} T_{P}=T_{s p h}(r, \beta, \gamma)=\operatorname{Rot}(z, \gamma) \operatorname{Rot}(y, \beta) \operatorname{Trans}(0,0, r)
\end{align*}
Figure \(2.22\) Spherical coordinates.
\begin{align*}
\begin{array}{c}
{ }^{R} T_{p}=\left[\begin{array}{ccc}
\end{array}\right] \times\left[\begin{array}{cccc}
C \beta & 0 & S \beta & 0 \\
0 & 1 & 0 & 0 \\
-S \beta & 0 & C \beta & 0 \\
0 & 0 & 0 & 1
\end{array}\right] \times\left[\begin{array}{llll}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & r \\
0 & 0 & 0 & 1
\end{array}\right] \\
{ }^{R} T_{P}=T_{s p h}(r, \beta, \gamma)=\left[\begin{array}{cccc}
C \beta C \gamma & -S \gamma & S \beta C \gamma & r S \beta C \gamma \\
C \beta S \gamma & C \gamma & S \beta S \gamma & r S \beta S \gamma \\
-S \beta & 0 & C \beta & r C \beta \\
0 & 0 & 0 & 1
\end{array}\right]
\end{array}
\end{align*}
The first three columns represent the orientation of the frame, while the last column is the position of the origin. Here, too, the original orientation of the final frame may be restored to make it parallel to the reference frame. This exercise is left for the reader to find the correct sequence of movements to get the right answer.
The inverse kinematic equations for spherical coordinates are more complicated than the simple Cartesian or cylindrical coordinates because the two angles \(\beta\) and \(\gamma\) are coupled. Let's see how this could be done through an example.
Example \(2.20\) Suppose that we now want to place the origin of the hand of a spherical robot at \([4,5,6]^{T}\). Calculate the joint variables of the robot.
Solution:
Setting the components of the location of the origin of the frame from \(T_{s p h}\) matrix of Eq. (2.37) to the desired values, we get:
\begin{align*}
\begin{array}{l}
r S \beta C \gamma=5 \\
r S \beta S \gamma=4 \\
r C \beta=6
\end{array}
\end{align*}
From the third equation, we determine that the \(C \beta\) is positive, but there are no independent values for \(S \beta, S \gamma, C \gamma\), and, consequently, we cannot use the ATAN2 function. As a result, there are two possible solutions for each angle. Later, we will have to check the final results to ensure they are correct.
\(\begin{array}{lll}\tan \gamma=4 / 5 \rightarrow & \gamma=38.7^{\circ} & \text { or } 218.7^{\circ} \\ \text { then } & S \gamma=0.625 & \text { or }-0.625 \\ \text { and } & C \gamma=0.781 & \text { or }-0.781 \\ \text { and } & r S \beta=4 / 0.625=6.4 & \text { or }-6.4 \\ \text { and since } & r C \beta=6, \rightarrow \beta=46.8^{\circ} & \text { or }-46.8^{\circ} \\ \text { and } & r=8.77 & \end{array}\)
You may check both answers and verify that they both satisfy all position equations. If you also follow these angles about the given axes in 3D, you get to the same point physically. However, you must notice that only one set of answers will also satisfy the orientation equations. In other words, these two answers result in the same position, but at different orientations. Since we are not concerned with the orientation of the hand frame at this point, both position answers are correct. In fact, since we cannot specify any orientation for a 3-DOF robot anyway, we cannot determine which of the two answers relates to a desired orientation.
2.9.4 Articulated Coordinates
Articulated coordinates consist of three rotations, as shown in Figure \(2.23 .\) We will develop the matrix representation for this later, when we discuss the Denavit-Hartenberg representation.
Figure \(2.23\) Articulated coordinates.
2.10 Forward and Inverse Kinematic Equations: Orientation
Suppose the moving frame attached to the hand of the robot has already moved to a desired position (in Cartesian, cylindrical, spherical, or articulated coordinates) and is either parallel to the reference frame or in an orientation other than what is desired. The next step is to rotate the frame appropriately in order to achieve a desired orientation without changing its position. This can only be accomplished by rotating about the current frame axes; rotations about the reference frame axes change the position. The appropriate sequence of rotations depends on the design of the wrist of the robot and the way the joints are assembled together. We consider the following three common configurations:
- Roll, pitch, yaw (RPY) angles
- Euler angles
- Articulated joints
2.10.1 Roll, Pitch, Yaw (RPY) Angles
This is a sequence of three rotations about the current \(a-, o-, n-\) axes, respectively, which orients the hand to a desired orientation. The assumption here is that the current frame is parallel to the reference frame before the application of RPY. Otherwise, the final orientation of the robot's hand is a combination of the previous orientation, post-multiplied by the RPY.
To maintain the position of the frame and only rotate it to the desired orientation, we post-multiply the frame by the RPY matrices. Referring to Figure \(2.24\), the RPY sequence of rotations consists of:
Rotation of \(\phi_{a}\) about the \(a\)-axis \((z\)-axis of the moving frame) called roll Rotation of \(\phi_{o}\) about the \(o\)-axis \((y\)-axis of the moving frame) called pitch Rotation of \(\phi_{n}\) about the \(n\)-axis ( \(x\)-axis of the moving frame) called \(y a w\)
Figure \(2.24 RPY\) rotations about the current axes.
The matrix representing the RPY orientation change is:
\begin{align*}
\begin{array}{l}
\operatorname{RPY}\left(\phi_{a}, \phi_{o}, \phi_{n}\right)=\operatorname{Rot}\left(a, \phi_{a}\right) \operatorname{Rot}\left(o, \phi_{o}\right) \operatorname{Rot}\left(n, \phi_{n}\right)= \\
{\left[\begin{array}{cccc}
C \phi_{a} C \phi_{o} & C \phi_{a} S \phi_{o} S \phi_{n}-S \phi_{a} C \phi_{n} & C \phi_{a} S \phi_{o} C \phi_{n}+S \phi_{a} S \phi_{n} & 0 \\
S \phi_{a} C \phi_{o} & S \phi_{a} S \phi_{o} S \phi_{n}+C \phi_{a} C \phi_{n} & S \phi_{a} S \phi_{o} C \phi_{n}-C \phi_{a} S \phi_{n} & 0 \\
-S \phi_{o} & C \phi_{o} S \phi_{n} & C \phi_{o} C \phi_{n} & 0 \\
0 & 0 & 0 & 1
\end{array}\right]}
\end{array}
\end{align*}
This matrix represents the orientation change caused by the RPY alone. The location and the final orientation of the frame relative to the reference frame will be the product of the two matrices representing the position change and the RPY. For example, suppose that a robot is designed based on spherical coordinates and RPY. Then the robot may be represented by:
\begin{align*}
{ }^{ R } T_{H}=T_{s p h}(r, \beta, \gamma) \times \operatorname{RPY}\left(\phi_{a}, \phi_{o}, \phi_{n}\right)
\end{align*}
This matrix represents the orientation change caused by the RPY alone. The location and the final orientation of the frame relative to the reference frame will be the product of the two matrices representing the position change and the RPY. For example, suppose that a robot is designed based on spherical coordinates and RPY. Then the robot may be represented by:
\begin{align*}
{ }^{ R } T_{H}=T_{s p h}(r, \beta, \gamma) \times \operatorname{RPY}\left(\phi_{a}, \phi_{o^{\prime}} \phi_{n}\right)
\end{align*}
The inverse kinematic solution for the RPY is more complicated than the spherical coordinates, because here, there are three coupled angles, where we need to have information about the sines and the cosines of all three angles individually to solve for the angles. To decouple these angles, we pre-multiply both sides of Eq. (2.38) by \(\operatorname{Rot}\left(a, \phi_{a}\right)^{-1}:\)
\begin{align*}
\operatorname{Rot}\left(a, \phi_{a}\right)^{-1} \operatorname{RPY}\left(\phi_{a}, \phi_{o}, \phi_{n}\right)=\operatorname{Rot}\left(o, \phi_{o}\right) \operatorname{Rot}\left(n, \phi_{n}\right)
\end{align*}
Assuming that the final desired orientation achieved by RPY is represented by the \((n, o, a)\) matrix, we have:
\begin{align*}
\operatorname{Rot}\left(a, \phi_{a}\right)^{-1}\left[\begin{array}{cccc}
n_{x} & o_{x} & a_{x} & 0 \\
n_{y} & o_{y} & a_{y} & 0 \\
n_{z} & o_{z} & a_{z} & 0 \\
0 & 0 & 0 & 1
\end{array}\right]=\operatorname{Rot}\left(o, \phi_{o}\right) \operatorname{Rot}\left(n, \phi_{n}\right)
\end{align*}
Multiplying the matrices, we get:
\begin{align*}
\left[\begin{array}{cccc}
n_{x} C \phi_{a}+n_{y} S \phi_{a} & o_{x} C \phi_{a}+o_{y} S \phi_{a} & a_{x} C \phi_{a}+a_{y} S \phi_{a} & 0 \\
n_{y} C \phi_{a}-n_{x} S \phi_{a} & o_{y} C \phi_{a}-o_{x} S \phi_{a} & a_{y} C \phi_{a}-a_{x} S \phi_{a} & 0 \\
n_{z} & o_{z} & a_{z} & 0 \\
0 & 0 & 0 & 1
\end{array}\right]=\left[\begin{array}{cccc}
C \phi_{o} & S \phi_{o} S \phi_{n} & S \phi_{o} C \phi_{n} & 0 \\
0 & C \phi_{n} & -S \phi_{n} & 0 \\
-S \phi_{o} & C \phi_{o} S \phi_{n} & C \phi_{o} C \phi_{n} & 0 \\
0 & 0 & 0 & 1
\end{array}\right]
\end{align*}
Remember that the \(n, o, a\) components in Eq. ( \(2.40\) ) represent the final desired values normally given or known. The values of the RPY angles are the unknown variables.
Equating the different elements of the right-hand and the left-hand sides of Eq. (2.41) results in the following.
From the 2,1 elements, we get:
\begin{align*}
n_{y} C \phi_{a}-n_{x} S \phi_{a}=0 \rightarrow \phi_{a}=\operatorname{ATAN} 2\left(n_{y}, n_{x}\right) \text { and } \phi_{a}=\operatorname{ATAN} 2\left(-n_{y},-n_{x}\right)
\end{align*}
Note that since we do not know the sign of \(\sin \left(\phi_{a}\right)\) or \(\cos \left(\phi_{a}\right)\), two complementary solutions are possible. From the 3,1 and 1,1 elements, we get:
\begin{align*}
\begin{array}{l}
S \phi_{o}=-n_{z} \\
C \phi_{o}=n_{x} C \phi_{a}+n_{y} S \phi_{a} \rightarrow \phi_{o}=A T A N 2\left[-n_{z},\left(n_{x} C \phi_{a}+n_{y} S \phi_{a}\right)\right]
\end{array}
\end{align*}
And finally, from the 2,2 and 2,3 elements, we get:
\begin{align*}
\begin{array}{l}
C \phi_{n}=o_{y} C \phi_{a}-o_{x} S \phi_{a} \\
S \phi_{n}=-a_{y} C \phi_{a}+a_{x} S \phi_{a} \rightarrow \phi_{n}=A T A N 2\left[\left(-a_{y} C \phi_{a}+a_{x} S \phi_{a}\right),\left(o_{y} C \phi_{a}-o_{x} S \phi_{a}\right)\right]
\end{array}
\end{align*}
Later, we use the same approach for decoupling angles in other equations.
Example \(2.21\) The desired final position and orientation of the hand of a Cartesian + RPY robot is as follows. Find the necessary RPY angles and displacements.
\begin{align*}
{ }^{R} T_{P}=\left[\begin{array}{cccc}
n_{x} & o_{x} & a_{x} & p_{x} \\
n_{y} & o_{y} & a_{y} & p_{y} \\
n_{z} & o_{z} & a_{z} & p_{z} \\
0 & 0 & 0 & 1
\end{array}\right]=\left[\begin{array}{cccc}
0.233 & 0.255 & 0.939 & 4.24 \\
0.113 & 0.951 & -0.287 & 2.65 \\
-0.966 & 0.173 & 0.192 & 6 \\
0 & 0 & 0 & 1
\end{array}\right]
\end{align*}
Solution:
From the equations, we find two sets of answers:
\begin{align*}
\begin{array}{c}
\phi_{a}=A T A N 2\left(n_{y}, n_{x}\right)=A T A N 2(0.113,0.233)=26^{\circ} \text { or } 206^{\circ} \\
\phi_{o}=A T A N 2\left(-n_{z},\left(n_{x} C \phi_{a}+n_{y} S \phi_{a}\right)\right)=A T A N 2(0.966,0.259)=75^{\circ} \text { or } 255^{\circ} \\
\phi_{n}=A T A N 2\left(\left(-a_{y} C \phi_{a}+a_{x} S \phi_{a}\right),\left(o_{y} C \phi_{a}-o_{x} S \phi_{a}\right)\right)=A T A N 2(0.669,0.743)=42^{\circ} \text { or } 222^{\circ} \\
p_{x}=4.24 \quad p_{y}=2.65 \quad p_{z}=6 \text { units }
\end{array}
\end{align*}
Example \(2.22\) For the same position and orientation as in Example \(2.21\), find all necessary joint variables if the robot is cylindrical \(+\) RPY.
Solution:
In this case, we use:
\begin{align*}
{ }^{R} T_{P}=\left[\begin{array}{cccc}
0.233 & 0.255 & 0.939 & 4.24 \\
0.113 & 0.951 & -0.287 & 2.65 \\
-0.966 & 0.173 & 0.192 & 6 \\
0 & 0 & 0 & 1
\end{array}\right]=T_{c y l}(r, \alpha, l) \times \operatorname{RPY}\left(\phi_{a}, \phi_{o}, \phi_{n}\right)
\end{align*}
The right-hand side of this equation now involves four coupled angles; as before, these must be decoupled. However, since the rotation of \(\alpha\) about the \(z\)-axis for the cylindrical coordinates does
Figure \(2.25\) Cylindrical and RPY coordinates from Example \(2.21 .\)
not affect the \(a\)-axis, it remains parallel to the \(z\)-axis. As a result, the rotation of \(\phi_{a}\) about the \(a\)-axis for RPY will simply be added to \(\alpha\). This means that the \(26^{\circ}\) angle we found for \(\phi_{a}\) is the summation of \(\phi_{a}+\alpha\) (see Figure 2.25). Using the position information given, the solution from Example 2.21, and referring to Eq. \((2.34)\), we get:
\begin{align*}
\begin{array}{ll}
r C \alpha=4.24, \quad r S \alpha=2.65 & \rightarrow \alpha=32^{\circ} \\
\phi_{a}+\alpha=26^{\circ} & \rightarrow \phi_{a}=-6^{\circ} \\
S \alpha=0.53 & \rightarrow r=5 \\
p_{z}=6 & \rightarrow l=6 \\
\text { As in Example 2.21, } & \rightarrow \phi_{o}=75^{\circ}, \phi_{n}=42^{\circ}
\end{array}
\end{align*}
Of course, a similar solution may be found for the second set of answers.
2.10.2 Euler Angles
Euler angles are very similar to RPY, except that the last rotation is also about the current \(a\)-axis, as shown in Figure \(2.26\). We still need to make all rotations relative to the current axes to prevent any change in the position of the robot. The rotations representing the Euler angles are:
Rotation of \(\phi\) about the \(a\)-axis ( \(z\) axis of the moving frame), followed by Rotation of \(\theta\) about the \(o\) - axis ( yaxis of the moving frame), followed by Rotation of \(\psi\) about the \(a\)-axis ( \(z\) axis of the moving frame)
Figure 2.26 Euler rotations about the current axes.
The matrix representing the Euler angles orientation change is:
\begin{align*}
\begin{array}{l}
\text { Euler }(\phi, \theta, \psi)=\operatorname{Rot}(a, \phi) \operatorname{Rot}(o, \theta), \operatorname{Rot}(a, \psi)= \\
{\left[\begin{array}{ccccc}
C \phi C \theta C \psi-S \phi S \psi & -C \phi C \theta S \psi-S \phi C \psi & C \phi S \theta & 0 \\
S \phi C \theta C \psi+C \phi S \psi & -S \phi C \theta S \psi+C \phi C \psi & S \phi S \theta & 0 \\
-S \theta C \psi & S \theta S \psi & C \theta & 0 \\
0 & 0 & 0 & 1
\end{array}\right]}
\end{array}
\end{align*}
Once again, this matrix represents the orientation change caused by the Euler angles alone. The location and the final orientation of the frame relative to the reference frame will be the product of the two matrices representing the position change and the Euler angles.
The inverse kinematic solution for the Euler angles is found in a manner very similar to RPY. We premultiply the two sides of Eq. \((2.45)\) by \(\operatorname{Rot}^{-1}(a, \phi)\) to separate \(\phi\) from one side and move it to the other. By equating the elements of the two sides to each other, we find the following equations. Assuming the final desired orientation achieved by the Euler angles is represented by the \((n, o, a)\) matrix:
\begin{align*}
\operatorname{Rot}^{-1}(a, \phi) \times\left[\begin{array}{llll}
n_{x} & o_{x} & a_{x} & 0 \\
n_{y} & o_{y} & a_{y} & 0 \\
n_{z} & o_{z} & a_{z} & 0 \\
0 & 0 & 0 & 1
\end{array}\right]=\left[\begin{array}{cccc}
C \theta C \psi & -C \theta S \psi & S \theta & 0 \\
S \psi & C \psi & 0 & 0 \\
-S \theta C \psi & S \theta S \psi & C \theta & 0 \\
0 & 0 & 0 & 1
\end{array}\right]
\end{align*}
\(\left[\begin{array}{cccc}n_{x} C \phi+n_{y} S \phi & o_{x} C \phi+o_{y} S \phi & a_{x} C \phi+a_{y} S \phi & 0 \\ -n_{x} S \phi+n_{y} C \phi & -o_{x} S \phi+o_{y} C \phi & -a_{x} S \phi+a_{y} C \phi & 0 \\ n_{z} & o_{z} & a_{z} & 0 \\ 0 & 0 & 0 & 1\end{array}\right]=\left[\begin{array}{cccc}C \theta C \psi & -C \theta S \psi & S \theta & 0 \\ S \psi & C \psi & 0 & 0 \\ -S \theta C \psi & S \theta S \psi & C \theta & 0 \\ 0 & 0 & 0 & 1\end{array}\right]\)
Remember that the \(n, o, a\) components in Eq. (2.46) represent the final desired values that are normally given or known. The values of the Euler angles are the unknown variables.
Equating the different elements of the right-hand and the left-hand sides of Eq. (2.47) results in the following.
From the 2,3 elements, we get:
\begin{align*}
-a_{x} S \phi+a_{y} C \phi=0 \rightarrow \phi=A T A N 2\left(a_{y}, a_{x}\right) \text { or } \phi=A T A N 2\left(-a_{y},-a_{x}\right)
\end{align*}
With \(\phi\) known, all the elements of the left-hand side of Eq. \((2.47)\) are known. From the 2,1 and 2,2 elements, we get
\begin{align*}
\begin{array}{l}
S \psi=-n_{x} S \phi+n_{y} C \phi \\
C \psi=-o_{x} S \phi+o_{y} C \phi \rightarrow \psi=A T A N 2\left[\left(-n_{x} S \phi+n_{y} C \phi\right),\left(-o_{x} S \phi+o_{y} C \phi\right)\right]
\end{array}
\end{align*}
And finally, from the 1,3 and 3,3 elements, we get:
\begin{align*}
\begin{array}{l}
S \theta=a_{x} C \phi+a_{y} S \phi \\
\left.C \theta=a_{z} \rightarrow \theta=A T A N 2\left[\left(a_{x} C \phi+a_{y} S \phi\right), a_{z}\right)\right]
\end{array}
\end{align*}
\selectlanguage{english}
\FloatBarrier
\end{document}