Our presentation has focused on mobile robots. Most educational robots are mobile robots and you may have encountered commercial mobile robots such as robotic vacuum cleaners. You probably have not encountered robotic manipulators, but you have seen pictures of factories that assemble electronic circuits or weld frames of cars (Fig. 1.3). The most important difference between mobile and fixed robots is the environment in which they work. A mobile robot moves within an environment that has obstacles and uneven ground, so the environment is not fully known in advance. A robotic vacuum cleaner does not ask you to give it a map of your apartment with the location of each piece of furniture, nor do you have to reprogram it whenever you move a sofa. Instead, the robot autonomously senses the layout of the apartment: the rooms and the position of the furniture. While maps and odometry are helpful in moving a robot to an approximate position, sensors must be used to precisely locate the robot within its environment.

A robotic manipulator in a factory is fixed to a stable concrete floor and its construction is robust: repeatedly issuing the same commands will move the manipulator to precisely the same position. In this chapter we present algorithms for the kinematics of manipulators: how the commands to a manipulator and the robot’s motion are related. The presentation will be in terms of an arm with two links in a plane whose joints can rotate.

There are two complementary tasks in kinematics:

• Forward kinematics (Sect. 16.1): Given a sequence of commands, what is the final position of the robotic arm?

• Inverse kinematics (Sect. 16.2): Given a desired position of the robotic arm, what sequence of commands will bring it to that position?

Forward kinematics is relatively easy to compute because the calculation of the change in position that results from moving each joint involves simple trigonometry. If there is more than one link, the final position is calculated by performing the calculations for one joint after another. Inverse kinematics is very difficult, because you start with one desired position and have to look for a sequence of commands to reach that position. A problem in inverse kinematics may have one solution, multiple solutions or even no solution at all.

Kinematic computations are performed in terms of coordinate frames. A frame is attached to each joint of the manipulator and motion is described as transformations from one frame to another by rotations and translations. Transformation of coordinate frames in two-dimensions is presented in Sects. 16.3 and 16.4. Most robots manipulators are three-dimensional. The mathematical treatment of 3D motion is beyond the scope of this book, but we hope to entice you to study this subject by presenting a taste of 3D rotations in Sects. 16.5 and 16.6.

## 16.1 Forward Kinematics

We develop the kinematics of a two-dimensional robotic arm with two links, two joints and an end effector such as a gripper, a welder or a paint sprayer (Fig. 16.1). The first joint can rotate but it is mounted on a base that is fixed to a table or the floor. Link $$l_1$$ connects this joint to a second joint that can move and rotate; a second link $$l_2$$ connects this joint to the fixed end effector.

A two-dimensional coordinate system is assigned with the first joint at (0, 0). The lengths of the two links are $$l_1$$ and $$l_2$$. Rotate the first joint by $$\alpha$$ to move the end of the first link with the second joint to $$(x',y')$$. Now rotate the second joint by $$\beta$$. What are the coordinates (xy) of the end of the arm, in terms of the two constants $$l_1,l_2$$ and the two parameters $$\alpha ,\beta$$?

Project $$(x',y')$$ on the x- and y-axes; by trigonometry its coordinates are:

\begin{aligned} x'= & {} l_1 \cos \alpha \\ y'= & {} l_1 \sin \alpha \,. \end{aligned}

Now take $$(x',y')$$ as the origin of a new coordinate system and project (xy) on its axes to obtain $$(x'',y'')$$. The position of the end effector relative to the new coordinate system is:

\begin{aligned} x''= & {} l_2 \cos (\alpha +\beta )\\ y''= & {} l_2 \sin (\alpha +\beta )\,. \end{aligned}

In Fig. 16.1, $$\beta$$ is negative (a clockwise rotation) so $$\alpha +\beta$$ is the angle between the second link and the line parallel to the x-axis.

Combining the results gives:

\begin{aligned} x= & {} l_1 \cos \alpha + l_2 \cos (\alpha + \beta )\\ y= & {} l_1 \sin \alpha + l_2 \sin (\alpha + \beta )\,. \end{aligned}

Example Let $$l_1 = l_2 = 1$$, $$\alpha = 60^{\circ }$$, $$\beta = -30^{\circ }$$. Then:

\begin{aligned} x= & {} 1\cdot \cos 60 + 1\cdot \cos (60-30) = \frac{1}{2} + \frac{\sqrt{3}}{2} = \frac{1+\sqrt{3}}{2}\\ y= & {} 1\cdot \sin 60 + 1\cdot \sin (60-30) = \frac{\sqrt{3}}{2} + \frac{1}{2} = \frac{1+\sqrt{3}}{2}\,. \end{aligned}

Let us check if this result makes sense. Figure 16.2 shows a triangle formed by adding a line between (0, 0) and (xy). The complement of the angle $$\beta$$ is $$180-30=150$$ and the triangle is isosceles since both sides are 1, so the other angles of the triangle are equal and their values are $$(180-150)/2=15$$. The angle that the new line forms with the x-axis is $$60-15=45$$, which is consistent with $$x=y$$.

### Activity 16.1:

Forward kinematics

• Program your robot so that it traces the path of the arm in Fig. 16.1: turn left $$60^{\circ }$$, move forward one unit (1m or some other convenient distance), turn right $$30^{\circ }$$, move forward one unit.

• Measure the x- and y-distances of the robot from the origin and compare them to the values computed by the equations for forward kinematics.

## 16.2 Inverse Kinematics

The gray ring in Fig. 16.3 shows the workspace of the two-link arm, the set of positions that the end effector can reach. (We assume that $$l_2<l_1$$.) The workspace is circularly symmetric since we assume that there are no limitations of the rotation of the joints in a full circle between $$-180^{\circ }$$ and $$180^{\circ }$$. Any point like a on the circumference of the outer circle is a furthest position of the arm from the origin; it is obtained when the two links are lined up so the arm length is $$l_1+l_2$$. The closest positions to the origin in the workspace are points like b on the circumference of the inner circle; they are obtained when the second link is bend back on the first link giving a length of $$l_1-l_2$$. Another reachable position c is shown; there are two configurations (rotations of the joints) that cause the arm to be positioned at c.

Under the assumption that $$l_2<l_1$$, no sequence of rotations can position the end of the arm closer to the origin that $$l_1-l_2$$ and no position at a distance greater than $$l_1+l_2$$ from the origin is accessible. From the figure we learn that a problem in inverse kinematics—finding commands to reach a specified point—can have zero, one or many solutions.

The computation of the inverse kinematics uses the lassw of cosines (Fig. 16.4):

$$a^2 + b^2 - 2ab \cos \theta = c^2\,.$$

In a right triangle $$\cos 90^{\circ } = 0$$ and the law reduces to the Pythagorean theorem.

Suppose now that we are given a point (xy) and we want values for $$\alpha ,\beta$$ (if any exist) which will bring the arm to that point. Figure 16.5 similar to Fig. 16.2 except that the specific values are replaced by arbitrary angles and lengths.

By the Pythagorean theorem, $$r=\sqrt{x^2 + y^2}$$.

The law of cosines gives:

$$l_1^2 + l_2^2 - 2l_1 l_2 \cos (180^\circ -\beta ) = r^2\,,$$

which can be solved for $$\beta$$:

\begin{aligned} \cos (180^\circ -\beta )= & {} \frac{l_1^2 + l_2^2 - r^2}{2l_1 l_2}\\ \beta= & {} 180^\circ -\cos ^{-1}\left( \frac{l_1^2 + l_2^2 - r^2}{2l_1 l_2}\right) \,. \end{aligned}

To obtain $$\gamma$$ and then $$\alpha$$, use the law of cosines with $$\gamma$$ as the central angle:

$$\cos \gamma = \frac{l_1^2 +r^2 - l_2^2}{2l_1 r}\,.$$

From the right triangle formed by (xy) we have:

\begin{aligned} \tan (\alpha - \gamma )= & {} \frac{y}{x}\\ \alpha= & {} \tan ^{-1} \frac{y}{x} + \gamma \,, \end{aligned}

so:

$$\alpha = \tan ^{-1} \frac{y}{x} + \cos ^{-1}\left( \frac{l_1^2 +r^2 - l_2^2}{2l_1 r}\right) \,.$$

Example Assume again that $$l_1 = l_2 = 1$$ and that the end effector is at the point computed from the forward kinematics:

$$(x,y) = \left( \frac{1+\sqrt{3}}{2},\frac{1+\sqrt{3}}{2}\right) \,.$$

First, compute $$r^2$$:

$$r^2 = x^2+y^2 = \left( \frac{1+\sqrt{3}}{2}\right) ^2 + \left( \frac{1+\sqrt{3}}{2}\right) ^2 = 2+\sqrt{3}\,,$$

and use it in the computation of $$\beta$$:

\begin{aligned} \beta= & {} 180^{\circ } - \cos ^{-1} \left( \frac{1^2 + 1^2 - (2+\sqrt{3})}{2\cdot 1\cdot 1}\right) \\= & {} 180^{\circ } - \cos ^{-1}\left( -\frac{\sqrt{3}}{2}\right) \\= & {} 180^{\circ } \pm 150^{\circ }\\= & {} \pm 30^{\circ }\,, \end{aligned}

since $$330^{\circ }=-30^{\circ } \pmod {360^\circ }$$. There are two solutions because there are two ways of moving the arm to (xy).

Next compute $$\gamma$$:

\begin{aligned} \gamma = \cos ^{-1}\left( \frac{1^2 + r^2 - 1^2}{2\cdot 1 \cdot r}\right) = \cos ^{-1}\left( \frac{r}{2}\right) = \cos ^{-1}\left( \frac{\sqrt{2+\sqrt{3}}}{2}\right) = \pm 15^{\circ }\,. \end{aligned}
(16.1)

The inverse cosine can be obtained numerically on a calculator or algebraically as shown in Appendix B.7.

Since $$x=y$$, the computation of $$\alpha$$ is easy:

$$\alpha = \tan ^{-1}\frac{y}{x} + \gamma = \tan ^{-1}1 + \gamma = 45^{\circ } \pm 15^{\circ } = 60^{\circ } \;\text {or}\; 30^{\circ }\,.$$

The solution $$\alpha =60^{\circ },\beta =-30^{\circ }$$ corresponds to the rotation of the joints in Fig. 16.1, while the solution $$\alpha =30^{\circ },\beta =30^{\circ }$$ corresponds to rotating both of the joints $$30^{\circ }$$ counterclockwise.

In this simple case, it is possible to solve the forward kinematics equation to obtain formulas for the inverse kinematics. In general this is not possible so approximate numerical solutions are used.

### Activity 16.2:

Inverse kinematics

• Use the formulas for the inverse kinematics to program your robot to move to a specified coordinate.

• Measure the x- and y-distances of the robot from the origin and compare them to the specified coordinates.

• If your robot’s computer does not have the capability to compute the formulas, compute them offline and then input the commands to the robot.

## 16.3 Rotations

The motion of a robotic manipulator is described in terms of coordinate frames . Three frames are associated with the arm in Fig. 16.1: one frame is associated with the joint at the origin (which we assume is fixed to a table or the floor), a second frame is associated with the joint between the two links, and a third frame is associated with the end effector at the end of the second link.

In this section we describe how the rotational motion of a robotic arm can be mathematically modeled using rotation matrices. The links in robotic arms introduce translations: the second joint is offset by a linear distance of $$l_1$$ from the first joint, and the end effector is offset by a linear distance of $$l_2$$ from the second joint. The mathematical treatment of translations uses an extension of rotation matrices called homogeneous transforms.

Rotations can be confusing because a rotation matrix can have three interpretations that are described in the following subsections: rotating a vector, rotating a coordinate frame and transforming a vector from one coordinate frame to another.

### 16.3.1 Rotating a Vector

Consider a vector with cartesian coordinates (xy) and polar coordinates $$(r,\phi )$$ (Fig. 16.6a). Now rotate the vector by an angle $$\theta$$ (Fig. 16.6b). Its polar coordinates are $$(r,\phi +\theta )$$. What are its cartesian coordinates?

Using the trigonometric identities for the sum of two angles and the conversion of $$(r,\phi )$$ to (xy) we have:

\begin{aligned} x'= & {} r\cos (\phi +\theta )\\= & {} r\cos \phi \cos \theta - r\sin \phi \sin \theta \\= & {} (r\cos \phi )\cos \theta - (r\sin \phi )\sin \theta \\= & {} x\cos \theta - y\sin \theta \,,\\ \\ y'= & {} r\sin (\phi +\theta )\\= & {} r\sin \phi \cos \theta + r\cos \phi \sin \theta \\= & {} (r\sin \phi )\cos \theta + (r\cos \phi )\sin \theta \\= & {} y\cos \theta + x\sin \theta \\= & {} x\sin \theta + y\cos \theta \,.\\ \end{aligned}

These equations can be expressed as the multiplication of a matrix called the rotation matrix and a vector:

$$\left[ \begin{array}{c}x'\\ y'\end{array}\right] = \left[ \begin{array}{cc}\cos \theta &{}-\sin \theta \\ \sin \theta &{}\cos \theta \end{array}\right] \left[ \begin{array}{c}x\\ y\end{array}\right] \,.$$

Example Let p be the point at the tip of a vector of length $$r=1$$ that forms an angle of $$\phi =30^{\circ }$$ with the positive x-axis. The cartesian coordinates of p are $$\left( \frac{\sqrt{3}}{2},\,\frac{1}{2}\right)$$. Suppose that the vector is rotated by $$\theta =30^{\circ }$$. What are the new cartesian coordinates of p? Using matrix multiplication:

$$\left[ \begin{array}{c}x'\\ y'\end{array}\right] \;=\; \left[ \begin{array}{cc}\frac{\sqrt{3}}{2}&{}-\frac{1}{2}\\ \frac{1}{2}&{}\frac{\sqrt{3}}{2}\end{array}\right] \left[ \begin{array}{c}\frac{\sqrt{3}}{2}\\ \frac{1}{2}\end{array}\right] \;=\; \left[ \begin{array}{c}\frac{1}{2}\\ \frac{\sqrt{3}}{2}\end{array}\right] \,.$$

The result makes sense because rotating a vector whose angle with the x-axis is $$30^{\circ }$$ by $$30^{\circ }$$ should give a vector whose angle with the x-axis is $$60^{\circ }$$.

Suppose that the vector is rotated by an additional $$30^{\circ }$$; its new coordinates are:

\begin{aligned} \left[ \begin{array}{cc}\frac{\sqrt{3}}{2}&{}-\frac{1}{2}\\ \frac{1}{2}&{}\frac{\sqrt{3}}{2}\end{array}\right] \;\; \left( \left[ \begin{array}{cc}\frac{\sqrt{3}}{2}&{}-\frac{1}{2}\\ \frac{1}{2}&{}\frac{\sqrt{3}}{2}\end{array}\right] \left[ \begin{array}{c}\frac{\sqrt{3}}{2}\\ \frac{1}{2}\end{array}\right] \right) \;=\; \left[ \begin{array}{cc}\frac{\sqrt{3}}{2}&{}-\frac{1}{2}\\ \frac{1}{2}&{}\frac{\sqrt{3}}{2}\end{array}\right] \left[ \begin{array}{c}\frac{1}{2}\\ \frac{\sqrt{3}}{2}\end{array}\right] \;=\; \left[ \begin{array}{c}0\\ 1\end{array}\right] \,. \nonumber \\ \end{aligned}
(16.2)

This result also makes sense. Rotating a vector whose angle is $$30^{\circ }$$ twice by $$30^{\circ }$$ (for a total of $$60^\circ$$) should give $$90^{\circ }$$. The cosine of $$90^{\circ }$$ is 0 and the sine of $$90^{\circ }$$ is 1.

Since matrix multiplication is associative, the multiplication could also be performed as follows:

\begin{aligned} \left( \left[ \begin{array}{cc}\frac{\sqrt{3}}{2}&{}-\frac{1}{2}\\ \frac{1}{2}&{}\frac{\sqrt{3}}{2}\end{array}\right] \left[ \begin{array}{cc}\frac{\sqrt{3}}{2}&{}-\frac{1}{2}\\ \frac{1}{2}&{}\frac{\sqrt{3}}{2}\end{array}\right] \right) \;\; \left[ \begin{array}{c}\frac{\sqrt{3}}{2}\\ \frac{1}{2}\end{array}\right] \,. \end{aligned}
(16.3)

### Activity 16.3:

Rotation matrices

• Demonstrate the associativity of matrix multiplication by showing that the multiplication in Eq. 16.3 gives the same result as the multiplication in Eq. 16.2.

• Compute the matrix for a rotation of $$-30^{\circ }$$ and show that multiplying this matrix by the matrix for a rotation of $$30^{\circ }$$ gives the matrix for a rotation of $$0^{\circ }$$.

• Is this multiplication commutative?

• Is multiplication of two-dimensional rotational matrices commutative?

### 16.3.2 Rotating a Coordinate Frame

Let us reinterpret Fig. 16.6a, b. Figure 16.7a shows a coordinate frame (blue) defined by two orthogonal unit vectors:

$$\mathbf {x}= \left[ \begin{array}{c}1\\ 0\end{array}\right] \,,\;\; \mathbf {y}= \left[ \begin{array}{c}0\\ 1\end{array}\right] \,.$$

Figure 16.7b shows the coordinate frame rotated by $$\theta$$ degrees (red). The new unit vectors $$\mathbf {x'}$$ and $$\mathbf {y'}$$ can be obtained by multiplication by the rotation matrix derived above:

\begin{aligned} \mathbf {x'}= & {} \left[ \begin{array}{cc}\cos \theta &{}-\sin \theta \\ \sin \theta &{}\cos \theta \end{array}\right] \left[ \begin{array}{c}1\\ 0\end{array}\right] = \left[ \begin{array}{l}\cos \theta \\ \sin \theta \;\;\,\end{array}\right] \\ \,\\ \mathbf {y'}= & {} \left[ \begin{array}{cc}\cos \theta &{}-\sin \theta \\ \sin \theta &{}\cos \theta \end{array}\right] \left[ \begin{array}{c}0\\ 1\end{array}\right] = \left[ \begin{array}{l}-\sin \theta \\ \cos \theta \end{array}\right] \,. \end{aligned}

Example For the unit vectors in Fig. 16.7a and a rotation of $$30^\circ$$:

\begin{aligned} \mathbf {x'}= & {} \left[ \begin{array}{cc}\frac{\sqrt{3}}{2}&{}-\frac{1}{2}\\ \frac{1}{2}&{}\frac{\sqrt{3}}{2}\end{array}\right] \left[ \begin{array}{c}1\\ 0\end{array}\right] = \left[ \begin{array}{c}\frac{\sqrt{3}}{2}\\ \frac{1}{2}\end{array}\right] \\ \,\\ \mathbf {y'}= & {} \left[ \begin{array}{cc}\frac{\sqrt{3}}{2}&{}-\frac{1}{2}\\ \frac{1}{2}&{}\frac{\sqrt{3}}{2}\end{array}\right] \left[ \begin{array}{c}0\\ 1\end{array}\right] = \left[ \begin{array}{c}-\frac{1}{2}\\ \frac{\sqrt{3}}{2}\end{array}\right] \,. \end{aligned}

### 16.3.3 Transforming a Vector from One Coordinate Frame to Another

Let the origin of a coordinate frame b (blue) represent the joint of an end effector such as a welder and let point p be the tip of the welder (Fig. 16.8a). By convention in robotics, the coordinate frame of an entity is denoted by a “pre” superscript.Footnote 1 In the frame b, the point $${^{b}}\!{p}{}$$ has polar coordinates $$(r,\phi )$$ and cartesian coordinates $$({^{b}}\!{x}{},{^{b}}\!{y}{})$$ related by the usual trigonometric formulas:

$${^{b}}\!{p}{}= ({^{b}}\!{x}{},{^{b}}\!{y}{}) = (r\cos \phi ,\, r\sin \phi )\,.$$

Suppose that the joint (with its coordinate frame) is rotated by the angle $$\theta$$. The coordinates of the point relative to b remain the same, but the coordinate frame has moved so we ask: What are the coordinates $${^{a}}\!{p}{}=({^{a}}\!{x}{},{^{a}}\!{y}{})$$ of the point in the coordinate frame before it was moved? In Fig. 16.8b the original frame b is shown rotated to a new position (and still shown in blue), while the coordinate frame a is in the old position of b and is shown as red dashed lines. In the previous section we asked how to transform one coordinate frame into another; here, we are asking how to transform the coordinates of a point in a frame to its coordinates in another frame.

In terms of the robotic arm: we know $$({^{b}}\!{x}{},{^{b}}\!{y}{})$$, the coordinates of the tip of the end effector relative to the frame of the end effector, and we now ask for its coordinates $${^{a}}\!{p}{}=({^{a}}\!{x}{},{^{a}}\!{y}{})$$ relative to the fixed base. This is important because if we know $${^{a}}\!{p}{}$$, we can compute the distance and angle from the tip of the welder to the parts of the car it must now weld.

We can repeat the computation used for rotating a vector:

\begin{aligned} {^{a}}\!{x}{}= & {} r\cos (\phi +\theta )\\= & {} r\cos \phi \cos \theta - r\sin \phi \sin \theta \\= & {} {^{b}}\!{x}{}\cos \theta - {^{b}}\!{y}{}\sin \theta \,,\\ \\ {^{a}}\!{y}{}= & {} r\sin (\phi +\theta )\\= & {} r\sin \phi \cos \theta + r\cos \phi \sin \theta \\= & {} {^{b}}\!{x}{}\sin \theta + {^{b}}\!{y}{}\cos \theta \,,\\ \end{aligned}

to obtain the rotation matrix:

\begin{aligned} \left[ \begin{array}{c}{^{a}}\!{x}{}\\ {^{a}}\!{y}{}\end{array}\right] = \left[ \begin{array}{cc}\cos \theta &{}-\sin \theta \\ \sin \theta &{}\cos \theta \end{array}\right] \left[ \begin{array}{c}{^{b}}\!{x}{}\\ {^{b}}\!{y}{}\end{array}\right] \,. \end{aligned}
(16.4)

The matrix is called the rotation matrix from frame b to frame a and denoted $${^a_b}{R}{}$$. Pre-multiplying the point $${^{b}}\!{p}{}$$ in frame b by the rotation matrix gives $${^{a}}\!{p}{}$$ its coordinates in frame a:

$${^{a}}\!{p}{}= {^a_b}{R}{} \; {^{b}}\!{p}{}\,.$$

Example Let $${^{b}}\!{p}{}$$ be the point in frame b at the tip of a vector of length $$r=1$$ that forms an angle of $$\phi =30^{\circ }$$ with the positive x-axis. The coordinates of $${^{b}}\!{p}{}$$ are $$\left( \frac{\sqrt{3}}{2},\,\frac{1}{2}\right)$$. Suppose that the coordinate frame b (together with the point p) is rotated by $$\theta =30^{\circ }$$ to obtain the coordinate frame a. What are the coordinates of $${^{a}}\!{p}{}$$? Using Eq. 16.4:

$${^{a}}\!{p}{}= \left[ \begin{array}{c}{^{a}}\!{x}{}\\ {^{a}}\!{y}{}\end{array}\right] \;=\; \left[ \begin{array}{cc}\frac{\sqrt{3}}{2}&{}-\frac{1}{2}\\ \frac{1}{2}&{}\frac{\sqrt{3}}{2}\end{array}\right] \left[ \begin{array}{c}\frac{\sqrt{3}}{2}\\ \frac{1}{2}\end{array}\right] \;=\; \left[ \begin{array}{c}\frac{1}{2}\\ \frac{\sqrt{3}}{2}\end{array}\right] \,.$$

If frame a is now rotated $$30^{\circ }$$, we obtain the coordinates of the point in a third frame a1. Pre-multiply $${^{a}}\!{p}{}$$ by the rotation matrix for $$30^{\circ }$$ to obtain $${^{a1}}\!{p}{}$$:

$${^{a1}}\!{p}{}= \left[ \begin{array}{cc}\frac{\sqrt{3}}{2}&{}-\frac{1}{2}\\ \frac{1}{2}&{}\frac{\sqrt{3}}{2}\end{array}\right] \;\; \left( \left[ \begin{array}{cc}\frac{\sqrt{3}}{2}&{}-\frac{1}{2}\\ \frac{1}{2}&{}\frac{\sqrt{3}}{2}\end{array}\right] \left[ \begin{array}{c}\frac{\sqrt{3}}{2}\\ \frac{1}{2}\end{array}\right] \right) \;=\; \left[ \begin{array}{cc}\frac{\sqrt{3}}{2}&{}-\frac{1}{2}\\ \frac{1}{2}&{}\frac{\sqrt{3}}{2}\end{array}\right] \left[ \begin{array}{c}\frac{1}{2}\\ \frac{\sqrt{3}}{2}\end{array}\right] \;=\; \left[ \begin{array}{c}0\\ 1\end{array}\right] \,.$$

The product of the two rotation matrices:

$$\left[ \begin{array}{cc}\frac{\sqrt{3}}{2}&{}-\frac{1}{2}\\ \frac{1}{2}&{}\frac{\sqrt{3}}{2}\end{array}\right] \left[ \begin{array}{cc}\frac{\sqrt{3}}{2}&{}-\frac{1}{2}\\ \frac{1}{2}&{}\frac{\sqrt{3}}{2}\end{array}\right] = \left[ \begin{array}{cc}\frac{1}{2}&{}-\frac{\sqrt{3}}{2}\\ \frac{\sqrt{3}}{2}&{}\frac{1}{2}\end{array}\right]$$

results in the rotation matrix for rotating the original coordinate frame b by $$60^{\circ }$$:

$$\left[ \begin{array}{cc}\frac{1}{2}&{}-\frac{\sqrt{3}}{2}\\ \frac{\sqrt{3}}{2}&{}\frac{1}{2}\end{array}\right] \left[ \begin{array}{c}\frac{\sqrt{3}}{2}\\ \frac{1}{2}\end{array}\right] \;=\; \left[ \begin{array}{c}0\\ 1\end{array}\right] \,.$$

Given a sequence of rotations, pre-multiplying their rotation matrices gives the rotation matrix for the rotation equivalent to the sum of the individual rotations.

## 16.4 Rotating and Translating a Coordinate Frame

The joints on robotics manipulators are connected by links so the coordinate systems are related not just by rotations but also by translations. The point p in Fig. 16.9 represents a point in the (red) coordinate frame b, but relative to the (blue dashed) coordinate frame a, frame b is both rotated by the angle $$\theta$$ and its origin is translated by $$\varDelta x$$ and $$\varDelta y$$. If $${^{b}}\!{p}{}=({^{b}}\!{x}{},{^{b}}\!{y}{})$$, the coordinates of the point in frame b, are known, what are its coordinates $${^{a}}\!{p}{}=({^{a}}\!{x}{},{^{a}}\!{y}{})$$ in frame a?

To perform this computation, we define an intermediate (green) coordinate frame a1 that has the same origin as b and the same orientation as a (Fig. 16.10). What are the coordinates $${^{a1}}\!{p}{}=({^{a1}}\!{x}{},{^{a1}}\!{y}{})$$ of the point in frame a1? This is simply the rotation by $$\theta$$ that we have done before:

$${^{a1}}\!{p}{}=\left[ \begin{array}{c}{^{a1}}\!{x}{}\\ {^{a1}}\!{y}{}\end{array}\right] = \left[ \begin{array}{cc}\cos \theta &{}-\sin \theta \\ \sin \theta &{}\cos \theta \end{array}\right] \left[ \begin{array}{c}{^{b}}\!{x}{}\\ {^{b}}\!{y}{}\end{array}\right] \,.$$

Now that we have the coordinates of the point in a1, it is easy to obtain the coordinates in frame a by adding the offsets of the translation. In matrix form:

$${^{a}}\!{p}{}=\left[ \begin{array}{c}{^{a}}\!{x}{}\\ {^{a}}\!{y}{}\end{array}\right] = \left[ \begin{array}{c}{^{a1}}\!{x}{}\\ {^{a1}}\!{y}{}\end{array}\right] \,+\, \left[ \begin{array}{c}\varDelta x\\ \varDelta y\end{array}\right] \,.$$

Homogeneous transforms are used to combine a rotation and a translation in one operator. The two-dimensional vector giving the coordinates of a point is extended with a third element that has a fixed value of 1:

$$\left[ \begin{array}{c}x\\ y\\ 1\end{array}\right] \,.$$

The rotation matrix is extended to a $$3\times 3$$ matrix with a 1 in the lower right corner and zeros elsewhere. It is easy to check that multiplication of a vector in frame b by the rotation matrix results in the same vector as before except for the extra 1 element:

$$\left[ \begin{array}{c}{^{a1}}\!{x}{}\\ {^{a1}}\!{y}{}\\ 1\end{array}\right] \;=\; \left[ \begin{array}{ccc}\cos \theta &{}-\sin \theta &{}0\\ \sin \theta &{}\cos \theta &{}0\\ 0&{}0&{}1\end{array}\right] \left[ \begin{array}{c}{^{b}}\!{x}{}\\ {^{b}}\!{y}{}\\ 1\end{array}\right] \,.$$

The result is the coordinates of the point in the intermediate frame a1. To obtain the coordinates in frame a, we multiply by a matrix that performs the translation:

$$\left[ \begin{array}{c}{^{a}}\!{x}{}\\ {^{a}}\!{y}{}\\ 1\end{array}\right] \;=\; \left[ \begin{array}{ccc}1&{}0&{}\varDelta x\\ 0&{}1&{}\varDelta y\\ 0&{}0&{}1\end{array}\right] \left[ \begin{array}{c} {^{a1}}\!{x}{}\\ {^{a1}}\!{y}{}\\ 1\end{array}\right] \,.$$

By multiplying the two transforms, we obtain a single homogeneous transform that can perform both the rotation and that translation:

$$\left[ \begin{array}{ccc}1&{}0&{}\varDelta x\\ 0&{}1&{}\varDelta y\\ 0&{}0&{}1\end{array}\right] \left[ \begin{array}{ccc}\cos \theta &{}-\sin \theta &{}0\\ \sin \theta &{}\cos \theta &{}0\\ 0&{}0&{}1\end{array}\right] \;=\; \left[ \begin{array}{ccc}\cos \theta &{}-\sin \theta &{}\varDelta x\\ \sin \theta &{}\cos \theta &{}\varDelta y\\ 0&{}0&{}1\end{array}\right] \,.$$

Example Let us extend the previous example by adding a translation of (3, 1) to the rotation of $$30^\circ$$. The homogeneous transform of the rotation followed by the translation is:

$$\left[ \begin{array}{ccc}1&{}0&{}3\\ 0&{}1&{}1\\ 0&{}0&{}1\end{array}\right] \left[ \begin{array}{ccc}\frac{\sqrt{3}}{2}&{}-\frac{1}{2}&{}0\\ \frac{1}{2}&{}\frac{\sqrt{3}}{2}&{}0\\ 0&{}0&{}1\end{array}\right] \;\;= \left[ \begin{array}{ccc}\frac{\sqrt{3}}{2}&{}-\frac{1}{2}&{}3\\ \frac{1}{2}&{}\frac{\sqrt{3}}{2}&{}1\\ 0&{}0&{}1\end{array}\right] \,.$$

The coordinates of the point in frame a are:

$$\left[ \begin{array}{c}{^{a}}\!{x}{}\\ {^{a}}\!{y}{}\\ 1\end{array}\right] \;=\; \left[ \begin{array}{ccc}\frac{\sqrt{3}}{2}&{}-\frac{1}{2}&{}3\\ \frac{1}{2}&{}\frac{\sqrt{3}}{2}&{}1\\ 0&{}0&{}1\end{array}\right] \left[ \begin{array}{c}\frac{\sqrt{3}}{2}\\ \frac{1}{2}\\ 1\end{array}\right] \;=\; \left[ \begin{array}{c}\frac{1}{2}+3\\ \frac{\sqrt{3}}{2}+1\\ 1\end{array}\right] \,.$$

### Activity 16.4:

Homogeneous transforms

• Draw the diagram for a rotation of $$-30^\circ$$ followed by a translation of $$(3,-1)$$.

• Compute the homogeneous transform.

## 16.5 A Taste of Three-Dimensional Rotations

The concepts of coordinate transformations and kinematics in three-dimensions are the same as in two dimensions, however, the mathematics is more complicated. Furthermore, many of us find it difficult to visualize three-dimensional motion when all we are shown are two-dimensional representations of three-dimensional objects. In this section we give a taste of three-dimensional robotics by looking at rotations in three dimensions.

### 16.5.1 Rotations Around the Three Axes

A two-dimensional x-y coordinate frame can be considered to be embedded in a three-dimensional coordinate frame by adding a z-axis perpendicular to the x- and y-axes. Figure 16.11 shows a two-dimensional representation of the three-dimensional frame. The x-axis is drawn left and right on the paper and the y-axis is drawn up and down. The diagonal line represents the z-axis which is perpendicular to the other two axes. The “standard” x-y-z coordinate frame has the positive directions of its axes defined by the right-hand rule (see below). The positive directions are right for the x-axis, up for the y-axis and out (of the paper towards the observer) for the z-axis.

Rotate the coordinate frame counterclockwise around the z-axis, so that the z-axis remains unchanged (Fig. 16.12a, b). The new orientation of the frame is (up, left, out). Consider now a rotation of $$90^\circ$$ around the x-axis (Fig. 16.13a, b). This causes the y-axis to “jump out” of the paper and the z-axis to “fall down” onto the paper, resulting in the orientation (right, out, down). Finally, consider a rotation of $$90^\circ$$ around the y-axis (Fig. 16.14a, b). The x-axis “drops into” the paper and the z-axis “falls right” onto the paper. The new position of the frame is (in, up, right).

### 16.5.2 The Right-Hand Rule

There are two orientations for each axis, $$2^3=8$$ orientations overall. What matters is the relative orientation of one axis with respect to the other two; for example, once the x- and y-axes have been chosen to lie in the plane of the paper, the z-axis can have its positive direction pointing out of the paper or into the paper. The choice must be consistent. The convention in physics and mechanics is the right-hand rule . Curl the fingers of your right hand so that they go from the one axis to another axis. Your thumb now points in the positive direction of the third axis. For the familiar x- and y-axes on paper, curl your fingers on the path from the x-axis to the y-axis. When you do so your thumb points out of the paper and this is taken as the positive direction of the z-axis. Figure 16.15 shows the right-hand coordinate system displayed with each of the three axes pointing out of the paper. According to the right-hand rule the three rotations are:

Rotate from x to y around z,

Rotate from y to z around x,

Rotate from z to x around y.

### 16.5.3 Matrices for Three-Dimensional Rotations

A three-dimensional rotation matrix is a $$3\times 3$$ matrix because each point p in a frame has three coordinates $$p_x,p_y,p_z$$ that must be moved. Start with a rotation of $$\psi$$ around the z-axis, followed by a rotation of $$\theta$$ around the y axis and finally a rotation of $$\phi$$ around the x-axis. For the first rotation around the z-axis, the x and y coordinates are rotated as in two dimensions and the z coordinate remains unchanged. Therefore, the matrix is:

$$R_{z(\psi )}=\left[ \begin{array}{ccc}\cos \psi &{}-\sin \psi &{}0\\ \sin \psi &{}\cos \psi &{}0\\ 0&{}0&{}1 \end{array}\right] \,.$$

For the rotation by $$\theta$$ around the y-axis, the y coordinate is unchanged and the z and x coordinates are transformed “as if” they were the x and y coordinates of a rotation around the z-axis:

$$R_{y(\theta )}=\left[ \begin{array}{ccc}\cos \theta &{}0&{}\sin \theta \\ 0&{}1&{}0\\ -\sin \theta &{}0&{}\cos \theta \\ \end{array}\right] \,.$$

For the rotation by $$\phi$$ around the x-axis, the x coordinate is unchanged and the y and z coordinates are transformed “as if” they were the x and y coordinates of a rotation around the z-axis:

$$R_{x(\phi )}=\left[ \begin{array}{ccc}1&{}0&{}0\\ 0&{}\cos \phi &{}-\sin \phi \\ 0&{}\sin \phi &{}\cos \phi \\ \end{array}\right] \,.$$

It may seem strange that in the matrix for the rotation around the y-axis the signs of the sine function have changed. To convince yourself that matrix for this rotation is correct, redraw the diagram in Fig. 16.8b, substituting z for x and x for y and perform the trigonometric computation.

### 16.5.4 Multiple Rotations

There is a caveat to composing rotations: like matrix multiplication, three-dimensional rotations do not commute. Let us demonstrate this by a simple sequence of two rotations. Consider a rotation of $$90^\circ$$ around the z-axis, followed by a rotation of $$90^\circ$$ around the (new position of the) x-axis (Fig. 16.16). The result can be expressed as (up, out, right).

Now consider the commuted operation: a rotation of $$90^\circ$$ around the x-axis, followed by a rotation of $$90^\circ$$ around the z-axis (Fig. 16.17). The result can be expressed as (out, left, down), which is not the same as the previous orientation.

### 16.5.5 Euler Angles

An arbitrary rotation can be obtained by three individual rotations around the three axes, so the matrix for an arbitrary rotation can be obtained by multiplying the matrices for each single rotation. The angles of the rotations are called Euler angles. The formulas are somewhat complex and can be found in the references listed at the end of the chapter. Here we demonstrate Euler angles with an example.

Example Figure 16.18 shows a coordinate frame rotated sequentially $$90^\circ$$ around the z-axis, then the y-axis and finally the x-axis. This is called a zyx Euler angle rotation. The final orientation is (in, up, right).

Let us consider a robotic manipulator that consists of a single joint that can rotate around all three axes. A sequence of rotations is performed as shown in Fig. 16.18. Consider the point at coordinates (1, 1, 1) relative to the joint (Fig. 16.19a). After the rotations, what are the coordinates of this point in the original fixed frame?

This can be computed by leaving the vector fixed and considering the rotations of the coordinate frames. To reach the final position shown in Fig. 16.19a the frame was rotated around the x-axis from the orientation shown in Fig. 16.19b. By examining the figure we see that the coordinates in this frame are $$(1,-1,1)$$. Proceeding through the previous two frames (Fig. 16.20a, b), the coordinates are $$(1,-1,-1)$$ and $$(1,1,-1)$$.

These coordinates can be computed from the rotation matrices for the rotations around the three axes. The coordinates of the final coordinate frame are (1, 1, 1), so in the frame before the rotation around the x-axis the coordinates were:

$$\left[ \begin{array}{ccc}1 &{} 0 &{} 0 \\ 0 &{} 0 &{}-1\\ 0&{}1&{}0 \\ \end{array}\right] \left[ \begin{array}{c}1\\ 1\\ 1 \end{array}\right] = \left[ \begin{array}{c}1\\ -1 \\ 1 \end{array}\right] \,.$$

The coordinates in the frame before the rotation around the y-axis were:

$$\left[ \begin{array}{ccc}0&{}0&{}1\\ 0&{}1&{}0\\ -1&{}0&{}0\\ \end{array}\right] \left[ \begin{array}{c}1\\ -1\\ 1\end{array}\right] = \left[ \begin{array}{c}1\\ -1\\ -1\end{array}\right] \,.$$

Finally, the coordinates in the fixed frame before the rotation around the z-axis were:

$$\left[ \begin{array}{ccc}0&{}-1&{}0\\ 1&{}0&{}0\\ 0&{}0&{}1\\ \end{array}\right] \left[ \begin{array}{c}1\\ -1\\ -1\end{array}\right] = \left[ \begin{array}{c}1\\ 1\\ -1\end{array}\right] \,.$$

For three arbitrary zyx Euler angle rotations: $$\psi$$ around the z-axis, then $$\theta$$ around the y-axis and finally $$\phi$$ around the x-axis the rotation matrix is:

$$R=R_{z(\psi )}R_{y(\theta )}R_{x(\phi )}\,.$$

It may seem strange that the order of the matrix multiplication (which is always from right to left) is opposite the order of the rotations. This is because we are taking a vector in the final coordinate frame and transforming it back into the fixed frame to determine its coordinates in the fixed frame.

### Activity 16.5:

Multiple Euler angles

• Multiply the three matrices to obtain a single matrix that directly transforms the coordinates from (1, 1, 1) to $$(1,1,-1)$$.

• Perform the same computation for other rotations, changing the sequence of the axes and the angles of rotation.

### 16.5.6 The Number of Distinct Euler Angle Rotations

There are three axes so there should be $$3^3=27$$ sequences of Euler angles. However, there is no point in rotating around the same axis twice in succession because the same result can be obtained by rotating once by the sum of the angles, so there are only $$3\cdot 2\cdot 2=12$$ different Euler angles sequences. The following activity asks you to explore different Euler angle sequences.

### Activity 16.6:

Distinct Euler angles

• To experiment with three-dimensional rotations, it is helpful to construct a coordinate frame from three mutually perpendicular pencils or straws.

• Draw the coordinate frames for a zyz Euler angle rotation, where each rotation is by $$90^\circ$$.

• What zyz rotation gives the same result at the zyx rotation shown in Fig. 16.18?

• Experiment with other rotation sequences and with angles other than $$90^\circ$$.

## 16.6 Advanced Topics in Three-Dimensional Transforms

Now that you have tasted three-dimensional rotations, we survey the next steps in learning this topic which you can study in the textbooks listed in the references.

There are 12 Euler angles and the choice of which to use depends on the intended application. Furthermore, there is a different way of defining rotations. Euler angles are moving axes transforms, that is, each rotation is around the new position of the axis after the previous rotation. In Fig. 16.18, the second rotation is around the y-axis that now points left, not around the original y-axis that points up. It is also possible to define fixed axes rotations in which subsequent rotations are around the original axes of the coordinate system. In three dimensions, homogeneous transforms that include translations in addition to rotations can be efficiently represented as $$4\times 4$$ matrices.

Euler angles are relatively inefficient to compute and suffer from computational instabilities. These can be overcome by using quaternions , which are a generalization of complex numbers. Quaternions use three “imaginary” numbers ijk, where:

$$i^2 = j^2 = k^2 = ij\,k = -1\,.$$

Recall that a vector in the two-dimensional plane can be expressed as a complex number $$x+\mathbf {i}\,y$$. Rotating the vector by an angle $$\theta$$ can be performed by multiplying by the value $$\cos \theta + \mathbf {i} \sin \theta$$. Similarly, in three dimensions, a vector can be expressed as a pure quaternion with a zero real component: $$p=0+x\,\mathbf {i} + y\,\mathbf {j} + z\,\mathbf {k}$$. Given an axis and an angle, there exists a quaternion q that rotates the vector around the axis by this angle using the formula $$qpq^{-1}$$. This computation is more efficient and robust than the equivalent computation with Euler angles and is used in a variety of contexts such as aircraft control and computer graphics.

## 16.7 Summary

Kinematics is the description of the motion of a robot. In forward kinematics, we are given a set of commands for the robot and we need to compute its final position relative to its initial position. In inverse kinematics, we are given a desired final position and need to compute the commands that will bring the robot to that position. This chapter has demonstrated kinematic computations for a simple two-dimensional robotic manipulator arm. In practice, manipulators move in three-dimensions and the computations are more difficult. Exact solutions for computing inverse kinematics usually cannot be found and approximate numerical solutions are used.

There are many ways of defining and computing arbitrary rotations. We mentioned the Euler angles where an arbitrary rotation is obtained by a sequence of three rotations around the coordinate axes. Quaternions, a generalization of complex numbers, are often used in practice because they are computationally more efficient and robust.