# Robot Kinematics: Motion, Kinematics and Dynamics

Jiawei Zhang<EMAIL_ADDRESS>
Founder and Director
Information Fusion and Mining Laboratory
(First Version: November 2022; Revision: November 2022.)

###### Abstract

This is a follow-up tutorial article of [5]. For better understanding of the
topics covered in this articles, we recommend the readers to first read our
previous tutorial article [5] on robot basics. Specifically, in this article,
we will cover some more advanced topics on robot kinematics, including robot
motion, forward kinematics, inverse kinematics, and robot dynamics. For the
topics, terminologies and notations introduced in the previous article[5], we
will use them directly without re-introducing them again in this article. Also
similar to the previous article, math and formulas will also be heavily used
in this article as well (hope the readers are well prepared for the upcoming
math bomb). After reading this article, readers should be able to have a
deeper understanding about how robot motion, kinematics and dynamics. As to
some more advanced topics about robot control, we will introduce them in the
following tutorial articles for readers instead.

Keywords: Robotics; Motion; Forward Kinematics; Inverse Kinematics; Dynamics

###### Contents

  1. 1 Introduction
    1. 1.1 This Article
    2. 1.2 Basic Notations
  2. 2 Robot Motion
    1. 2.1 Homogeneous Transformation Matrix
      1. 2.1.1 Special Orthogonal Group and Special Euclidean Group
      2. 2.1.2 Chain Rule on Homogeneous Transformation Matrix
      3. 2.1.3 Robot Arm Link Projection across Coordinate Systems
    2. 2.2 Robot Twist Velocity
      1. 2.2.1 Twist Velocity in World Coordinate System
      2. 2.2.2 Twist in Local Coordinate System
    3. 2.3 Twist Velocity Transformation across Coordinate Systems
      1. 2.3.1 Relationship Between $\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{a}]}$ And $\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{\textsc{w}}]}$
      2. 2.3.2 Relationship Between ${\boldsymbol{\nu}}_{a}^{[\Sigma_{a}]}$ And ${\boldsymbol{\nu}}_{a}^{[\Sigma_{\textsc{w}}]}$
    4. 2.4 Screw
      1. 2.4.1 A Toy Example
      2. 2.4.2 Screw Axis
      3. 2.4.3 Screw Axis Representation Simplification
    5. 2.5 Motion Exponential and Logarithm
      1. 2.5.1 Motion Exponential Representation
      2. 2.5.2 Matrix Logarithm Representation
    6. 2.6 Wrench
      1. 2.6.1 Wrench Definition
      2. 2.6.2 Wrench Transformation
  3. 3 Forward Kinematics
    1. 3.1 What is Forward Kinematics?
    2. 3.2 Pose in Forward Kinematics
      1. 3.2.1 Chain Rule
      2. 3.2.2 Product of Exponentials
    3. 3.3 Velocity in Forward Kinematics
      1. 3.3.1 Jacobian Matrix
      2. 3.3.2 Velocity Kinematics in World Coordinate System
      3. 3.3.3 Velocity Kinematics in Local Coordinate System
    4. 3.4 Torque in Forward Kinematics
      1. 3.4.1 Static Analysis
      2. 3.4.2 Force-Torque Relationship
  4. 4 Inverse Kinematics
    1. 4.1 What is Inverse Kinematics?
    2. 4.2 Analytic Solution
    3. 4.3 Inverse Position and Orientation Kinematics
      1. 4.3.1 Numerical Solution Method
      2. 4.3.2 Newton-Raphson Method
    4. 4.4 Inverse Velocity and Torque Kinematics
      1. 4.4.1 Inverse Velocity Kinematics
      2. 4.4.2 Inverse Torque Kinematics
    5. 4.5 Singular Jacobian Matrix
      1. 4.5.1 Singularity and Gimbal Lock
      2. 4.5.2 Jacobian Singularity
      3. 4.5.3 Robot Singular Pose in Application
    6. 4.6 Singularity-Robust Inverse of Jacobian
  5. 5 Robot Dynamics
    1. 5.1 What is Robot Dynamics?
    2. 5.2 Lagrangian Dynamics Formulation
      1. 5.2.1 Euler-Lagrange Equation
      2. 5.2.2 Lagrangian Dynamics of 2-Link Robot Arm
      3. 5.2.3 Lagrangian Dynamics of Multi-Link Robot Arm
    3. 5.3 Newton-Euler Dynamics Formulation
      1. 5.3.1 Newton-Euler Formulation for Rigid Body
      2. 5.3.2 Rotational Inertia Matrix
      3. 5.3.3 Twist-Wrench Formulation
    4. 5.4 A Practical Case Study: Robot Joint Actuator Dynamics
      1. 5.4.1 Actuator
      2. 5.4.2 Angular Velocity
      3. 5.4.3 Friction
      4. 5.4.4 Gearbox
      5. 5.4.5 Motor Torque Balance
  6. 6 What’s Next?

##  1 Introduction

According to the forecast report from UN (United Nations) [4], the world
population aging will become one of the most challenging global problem of the
21st century. In the upcoming decades, the working age population growth of
the major developing countries (e.g., China, India) will gradually slow down
and even start to decrease together with the major developed countries (e.g.,
UK, France, Germany, Japan and USA). To fulfill the “tremendous gap” between
the supply and demand of working labor forces, various robots and automated
machines has been (and will continue to be) developed and employed for massive
production work globally. This is an irreversible trend for the 21st century.
To accomplish such an objective, educating and training researchers and
practitioner on robotics is imperative and critical at present.

###  1.1 This Article

This is a follow-up tutorial article of [5]. In the previous tutorial article,
we have introduced the basic knowledge about robot representation, robot
rotation, position and orientation transformation and velocity transformation
already. In this follow-up article, we will introduce several advanced topics
about robot kinematics for readers, which include robot motion, forward
kinematics, inverse kinematics, and robot dynamics.

A Reminder: Similar to [5], this article will also be very math-heavy. When
you read the equations in this article, it will be great for you to figure out
their physical meanings in mind as well. The math equations will deliver the
same information to you just like the textual descriptions. We prefer to use
math equations, since they can deliver information to readers in a more
precise way.

For some more advanced topics about robotics, like robot control, trajectory
generation, motion planning, zero moment point, biped walking, robot
manipulation and robot simulation, we plan to introduce them in the follow-up
tutorial articles. If the readers are interested in robotics, we also have
several textbooks recommended for you to read as well, like [2, 3, 1].

###  1.2 Basic Notations

In the sequel of this article, we will use the lower case letters (e.g., $x$)
to represent scalars, lower case bold letters (e.g., $\mathbf{x}$) to denote
column vectors, bold-face upper case letters (e.g., $\mathbf{X}$) to denote
matrices. Given a vector $\mathbf{x}$, its length is denoted as
$\left\|\mathbf{x}\right\|$. Given a matrix $\mathbf{X}$, we denote
$\mathbf{X}(i,:)$ and $\mathbf{X}(:,j)$ as its $i_{th}$ row and $j_{th}$
column, respectively. The ($i_{th}$, $j_{th}$) entry of matrix $\mathbf{X}$
can be denoted as either $\mathbf{X}(i,j)$ or $\mathbf{X}_{i,j}$, which will
be used interchangeably. We use $\mathbf{X}^{\top}$ and $\mathbf{x}^{\top}$ to
represent the transpose of matrix $\mathbf{X}$ and vector $\mathbf{x}$. The
cross product of vectors $\mathbf{x}$ and $\mathbf{y}$ is represented as
$\mathbf{x}\times\mathbf{y}$. A coordinate system is denoted as $\Sigma$, and
the vector $\mathbf{x}$ in coordinate system $\Sigma$ can also be specified as
$\mathbf{x}^{[\Sigma]}$. For a scalar $x$, vector $\mathbf{x}$ and matrix
$\mathbf{X}$, we can also represent their first-order derivatives as
$\dot{x}$, $\dot{\mathbf{x}}$ and $\dot{\mathbf{X}}$, and second-order
derivatives as $\ddot{x}$, $\ddot{\mathbf{x}}$ and $\ddot{\mathbf{X}}$.

##  2 Robot Motion

Before we talk about the forward kinematics and inverse kinematics, we would
like to discuss about the rigid-body robot motion first in this section. We
will revisit the homogeneous transformation matrix defined in the previous
tutorial article and discuss about some of its properties in this section as
well.

###  2.1 Homogeneous Transformation Matrix

In the previous tutorial article, most of the materials we have introduced are
about the robot rotation matrix and its transformation impact on changing the
position, orientation, linear velocity and angular velocity of the robot arm
end point. We have also briefly introduced the homogeneous transformation
matrix for readers but we didn’t discuss much about its property or usage in
robot motion actually, which will be covered in this section.

####  2.1.1 Special Orthogonal Group and Special Euclidean Group

The set of rotation matrix we introduce in the previous article can also be
clearly represented as the special orthogonal group defined as follows.

###### Definition 1

(Special Orthogonal Group): The special orthogonal group $SO(3)$, also known
as the group of rotation matrices in $\mathbbm{R}^{3}$, denotes the set of all
$3\times 3$ matrices $\mathbf{R}\in\mathbbm{R}^{3\times 3}$ that satisfy (1)
$\mathbf{R}^{\top}\mathbf{R}=\mathbf{I}$ and (2) $\det\mathbf{R}=1$.

In the above definition, the first constraint
$\mathbf{R}^{\top}\mathbf{R}=\mathbf{I}$ denotes the rotation matrix
$\mathbf{R}$ is orthogonal and each column vector is a unit vector. Meanwhile,
the second constraint $\det\mathbf{R}=1$ indicates that the rotation is a
right-handed frame.

Meanwhile, based on the rotation matrix, we can also represent the set of
homogeneous transformation matrix as the special Euclidean group as follows.

###### Definition 2

(Special Euclidean Group): The special Euclidean group $SE(3)$, also known as
the homogeneous transformation matrix in $\mathbbm{R}^{3}$, denotes the set of
all $4\times 4$ matrices $\mathbf{T}\in\mathbbm{R}^{4\times 4}$ in the
following form

$\mathbf{T}=\begin{bmatrix}\mathbf{R}&\mathbf{p}\\\ \mathbf{0}&1\\\
\end{bmatrix}=\begin{bmatrix}\mathbf{R}_{1,1}&\mathbf{R}_{1,2}&\mathbf{R}_{1,3}&\mathbf{p}_{1}\\\
\mathbf{R}_{2,1}&\mathbf{R}_{2,2}&\mathbf{R}_{2,3}&\mathbf{p}_{2}\\\
\mathbf{R}_{3,1}&\mathbf{R}_{3,2}&\mathbf{R}_{3,3}&\mathbf{p}_{3}\\\
0&0&0&1\\\ \end{bmatrix},$ (1)

where the matrix
$\mathbf{R}=\begin{bmatrix}\mathbf{R}_{1,1}&\mathbf{R}_{1,2}&\mathbf{R}_{1,3}\\\
\mathbf{R}_{2,1}&\mathbf{R}_{2,2}&\mathbf{R}_{2,3}\\\
\mathbf{R}_{3,1}&\mathbf{R}_{3,2}&\mathbf{R}_{3,3}\\\ \end{bmatrix}\in SO(3)$
is a rotation matrix and $\mathbf{p}=\begin{bmatrix}\mathbf{p}_{1}\\\
\mathbf{p}_{3}\\\ \mathbf{p}_{3}\\\ \end{bmatrix}\in\mathbbm{R}^{3}$ is a
column vector denoting the robot arm position.

In the above definitions, the numerical number $3$ in the notations $SO(3)$
and $SE(3)$ indicates that these matrices are defined in the $\mathbbm{R}^{3}$
space, and it doesn’t denote the dimensions of the matrices. Like for a
homogeneous transformation matrix $\mathbf{T}\in SE(3)$, it actually has a
dimension of $4\times 4$.

####  2.1.2 Chain Rule on Homogeneous Transformation Matrix

The homogeneous transformation matrix defined across coordinate systems
follows the chain rule.

Figure 1: An Example of Rigid-Body Robot Structure (involving 1 base link and
3 movable links connected via joints $a$, $b$ and $c$). The arm rotation
angles are denoted as $\theta$, $\phi$ and $\psi$, respectively.

###### Example 1

Here, we will take a 4-link robot arm shown in Figure 1 as an example to
illustrate the forward kinematics process. Specifically, the robot arm
involves 1 base link from the origin $o$ of the world coordinate
$\Sigma_{\textsc{w}}$ to joint $a$, and 3 movable links connected via joints
$a$, $b$ and $c$. Attached to each joint, we define a local coordinate with
origin attached to them, which are denoted as $\Sigma_{a}$, $\Sigma_{b}$ and
$\Sigma_{c}$, respectively. Within the local coordinate systems, we can
represent the robot arm links as vectors $\mathbf{p}_{a\to b}^{[\Sigma_{a}]}$,
$\mathbf{p}_{b\to c}^{[\Sigma_{b}]}$, and $\mathbf{p}_{c\to
e}^{[\Sigma_{c}]}$, respectively.

According to the previous tutorial article [5], based on the robot arm
rotation angles, we can define the homogeneous transformation matrix between
the coordinate systems as follows:

$\displaystyle\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}$
$\displaystyle=\begin{bmatrix}\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}&\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&1\\\
\end{bmatrix}=\begin{bmatrix}\exp^{\widehat{\boldsymbol{\omega}}_{a}^{[\Sigma_{\textsc{w}}]}}&\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&1\\\ \end{bmatrix},$ (2)

where the rotation matrix
$\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}=\exp^{\widehat{\boldsymbol{\omega}}_{a}^{[\Sigma_{\textsc{w}}]}}$
is obtained from the previous tutorial article [5]. Notation
$\widehat{\boldsymbol{\omega}}_{a}^{[\Sigma_{\textsc{w}}]}$ projects the
angular velocity vector ${\boldsymbol{\omega}}_{a}^{[\Sigma_{\textsc{w}}]}$ to
the matrix representation. As introduced before, the angular velocity vector
can also be represented as
${\boldsymbol{\omega}}_{a}^{[\Sigma_{\textsc{w}}]}=\mathbf{e}_{a}^{[\Sigma_{\textsc{w}}]}\dot{q}_{a}$,
where $\mathbf{e}_{a}^{[\Sigma_{\textsc{w}}]}$ denotes the axis unit vector of
the joint $a$ and scalar $\dot{q}_{a}$ denotes the angular velocity with the
unit rad/s.

Similar to the homogeneous transition matrix
$\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}$ introduced above, we can
also define the homogeneous transition matrices between other local coordinate
systems, including

$\mathbf{T}^{[\Sigma_{b}\to\Sigma_{a}]}=\begin{bmatrix}\mathbf{R}^{[\Sigma_{b}\to\Sigma_{a}]}&\mathbf{p}_{a\to
b}^{[\Sigma_{a}]}\\\ \mathbf{0}&1\\\ \end{bmatrix}\text{, and
}\mathbf{T}^{[\Sigma_{c}\to\Sigma_{b}]}=\begin{bmatrix}\mathbf{R}^{[\Sigma_{c}\to\Sigma_{b}]}&\mathbf{p}_{b\to
c}^{[\Sigma_{b}]}\\\ \mathbf{0}&1\\\ \end{bmatrix}.\\\ $ (3)

Via the chain rule, we can obtain the homogeneous transition matrix
$\mathbf{T}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}$, which can be denoted as
follows:

$\mathbf{T}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}=\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\mathbf{T}^{[\Sigma_{b}\to\Sigma_{a}]}\mathbf{T}^{[\Sigma_{c}\to\Sigma_{b}]}.$
(4)

####  2.1.3 Robot Arm Link Projection across Coordinate Systems

Via the homogeneous transformation matrix, we can project a robot arm from a
local coordinate to another local/world coordinate system.

###### Example 2

Still based on the example illustrated in Figure 1, with the end point’s
current position vector within the local coordinate $\Sigma_{c}$, we can
calculate its end point’s position within the world coordinate as

$\begin{bmatrix}\mathbf{p}_{e}^{[\Sigma_{\textsc{w}}]}\\\
1\end{bmatrix}=\mathbf{T}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}\begin{bmatrix}\mathbf{p}_{c\to
e}^{[\Sigma_{c}]}\\\ 1\end{bmatrix}.$ (5)

In the above example, vector $\mathbf{p}_{c\to e}^{[\Sigma_{c}]}$ denotes the
third movable robot arm link pointing from the joint $c$ to the end point $e$
in the local coordinate system $\Sigma_{c}$. Via the homogeneous
transformation matrix $\mathbf{T}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}$, we
can project it to the world coordinate system and the result vector
$\mathbf{p}_{e}^{[\Sigma_{\textsc{w}}]}$ pointing from the origin $o$ of the
world coordinate system $\Sigma_{\textsc{w}}$ to the end point $e$ in
$\Sigma_{\textsc{w}}$.

###  2.2 Robot Twist Velocity

At the last section of the previous tutorial article [5], we have introduced
the transformation of the robot arm end point’s linear velocity and angular
velocity during the robot arm rotation. The previous analytic representations
of the linear velocity and angular velocity are derived separately from each
other. In this part, we will combine the robot linear velocity and angular
velocity together, which is also named as the twist of robots.

####  2.2.1 Twist Velocity in World Coordinate System

###### Definition 3

(Twist): Formally, given the angular velocity $\boldsymbol{\omega}$ and linear
velocity $\mathbf{v}$ of a point in robot body, we can combine
$\boldsymbol{\omega}$ and $\mathbf{v}$ into one vector, which defines the
twist velocity vector of the robot body:

$\boldsymbol{\nu}=\begin{bmatrix}\boldsymbol{\omega}\\\ \mathbf{v}\\\
\end{bmatrix}\in\mathbbm{R}^{6}.$ (6)

In the previous article, we introduce the operators
$\land:\boldsymbol{\omega}\to\dot{\mathbf{R}}\mathbf{R}^{\top}$ (or
$\land:\boldsymbol{\omega}\to\dot{\mathbf{R}}\mathbf{R}^{-1}$) and
$\lor:\dot{\mathbf{R}}\mathbf{R}^{\top}\to\boldsymbol{\omega}$ (or
$\lor:\dot{\mathbf{R}}\mathbf{R}^{-1}\to\boldsymbol{\omega}$) to create the
relationships between the angular velocity vector $\omega$ and the rotation
matrix $\mathbf{R}$. Here, we can introduce similar operators to the twist
vector $\boldsymbol{\nu}$ and the homogeneous transformation matrix
$\mathbf{T}$.

###### Definition 4

(Operator Definition): In this paper, we introduce ab operator $\land$ to
project the twist velocity vector
$\boldsymbol{\nu}=\begin{bmatrix}\boldsymbol{\omega}\\\ \mathbf{v}\\\
\end{bmatrix}$ to its matrix representation as follows:

$\widehat{\boldsymbol{\nu}}=\begin{bmatrix}\widehat{\boldsymbol{\omega}}&\mathbf{v}\\\
\mathbf{0}&0\\\ \end{bmatrix}.$ (7)

The above matrix representation of $\widehat{\boldsymbol{\nu}}$ is closely
correlated with the homogeneous transformation matrix $\mathbf{T}$ actually,
and their relationship will be analyzed as follows.

###### Example 3

To clearly explain the physical meanings of the terms to be derived below, we
can take the homogeneous transformation matrix
$\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}=\begin{bmatrix}\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}&\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&1\\\ \end{bmatrix}$ as an example. Let’s first calculate the
product of matrices
$\dot{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}$,
which can be represented as follows:

$\displaystyle\dot{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\left(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\right)^{-1}$
(8)
$\displaystyle=\begin{bmatrix}\dot{\mathbf{R}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}&\dot{\mathbf{p}}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&0\\\
\end{bmatrix}\begin{bmatrix}(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}&-(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&1\\\ \end{bmatrix}$ (9)
$\displaystyle=\begin{bmatrix}\dot{\mathbf{R}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}({\mathbf{R}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}&\dot{\mathbf{p}}_{a}^{[\Sigma_{\textsc{w}}]}-\dot{\mathbf{R}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}({\mathbf{R}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&0\\\ \end{bmatrix}.$ (10)

Considering that
$\dot{\mathbf{R}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\left({\mathbf{R}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\right)^{-1}=\dot{\mathbf{R}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\left({\mathbf{R}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\right)^{\top}=\widehat{\boldsymbol{\omega}}_{a}^{[\Sigma_{\textsc{w}}]}$
and
$\widehat{\boldsymbol{\omega}}_{a}^{[\Sigma_{\textsc{w}}]}\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}={\boldsymbol{\omega}}_{a}^{[\Sigma_{\textsc{w}}]}\times\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}$,
the above representation of matrix
$\dot{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}$
can be represented with

$\dot{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\left(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\right)^{-1}=\begin{bmatrix}\widehat{\boldsymbol{\omega}}_{a}^{[\Sigma_{\textsc{w}}]}&\dot{\mathbf{p}}_{a}^{[\Sigma_{\textsc{w}}]}-{\boldsymbol{\omega}}_{a}^{[\Sigma_{\textsc{w}}]}\times\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&0\\\ \end{bmatrix}.$ (11)

The physical meaning of
$\widehat{\boldsymbol{\omega}}_{a}^{[\Sigma_{\textsc{w}}]}=\dot{\mathbf{R}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\left({\mathbf{R}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\right)^{-1}$
is very straight-forward and we have introduced the operator in the previous
article already. As to the term
$\dot{\mathbf{p}}_{a}^{[\Sigma_{\textsc{w}}]}-{\boldsymbol{\omega}}_{a}^{[\Sigma_{\textsc{w}}]}\times\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}$,
it actually denotes the linear velocity of joint $a$ within the world
coordinate system $\Sigma_{\textsc{w}}$ with the velocity caused by the
rotation being excluded, i.e.,
${\boldsymbol{\omega}}_{a}^{[\Sigma_{\textsc{w}}]}\times\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}$.
Such a linear velocity can be caused by the movement of the robot arm base,
which introduce extra velocity for all points in the robot arm.

For simplicity, we can denote
$\mathbf{v}_{a}^{[\Sigma_{\textsc{w}}]}=\dot{\mathbf{p}}_{a}^{[\Sigma_{\textsc{w}}]}-{\boldsymbol{\omega}}_{a}^{[\Sigma_{\textsc{w}}]}\times\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}$,
so we can simlify rewrite the homogeneous transformation matrix product as
follows:

$\dot{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\left(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\right)^{-1}=\begin{bmatrix}\widehat{\boldsymbol{\omega}}_{a}^{[\Sigma_{\textsc{w}}]}&\mathbf{v}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&0\\\ \end{bmatrix}.$ (12)

Based on the above analysis, we observe that with in the world coordinate
system $\Sigma_{\textsc{w}}$, we can apply the $\land$ operator to the twist
velocity $\boldsymbol{\nu}_{a}^{[\Sigma_{\textsc{w}}]}$, the result matrix is
identical as the corresponding homogeneous matrix product
$\dot{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\left(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\right)^{-1}$
actually:

$({\boldsymbol{\nu}}_{a}^{[\Sigma_{\textsc{w}}]})^{\land}=\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{\textsc{w}}]}=\begin{bmatrix}\widehat{\boldsymbol{\omega}}_{a}^{[\Sigma_{\textsc{w}}]}&\mathbf{v}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&0\\\
\end{bmatrix}=\dot{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\left(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\right)^{-1}.$
(13)

####  2.2.2 Twist in Local Coordinate System

In the above analysis, we multiply
$\left(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\right)^{-1}$ on the
right side of matrix $\dot{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}$.
Some readers may also wonder if we multiple it to the left side will affect
the result or not? In this part, we will answer this question.

Similar to the above analysis, we can first calculate the left multiplication
of $\left(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\right)^{-1}$ to
$\dot{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}$ as follows:

$\displaystyle\left(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\right)^{-1}\dot{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}$
(14)
$\displaystyle=\begin{bmatrix}(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}&-(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&1\\\
\end{bmatrix}\begin{bmatrix}\dot{\mathbf{R}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}&\dot{\mathbf{p}}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&0\\\ \end{bmatrix}$ (15)
$\displaystyle=\begin{bmatrix}(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}\dot{\mathbf{R}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}&(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}\dot{\mathbf{p}}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&0\\\ \end{bmatrix}$ (16)
$\displaystyle=\begin{bmatrix}(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{\top}\dot{\mathbf{R}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}&(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{\top}\dot{\mathbf{p}}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&0\\\ \end{bmatrix}.$ (17)

In the above representation, the physical meaning of term
$(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{\top}\dot{\mathbf{p}}_{a}^{[\Sigma_{\textsc{w}}]}=\mathbf{v}_{a}^{[\Sigma_{a}]}$
should be trivial for the readers to figure out. As to
$(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{\top}\dot{\mathbf{R}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}$,
as illustrated as follows, it actually denotes
$\widehat{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}$.

$\displaystyle\widehat{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}$
$\displaystyle=\left((\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}\boldsymbol{\omega}_{a}^{\Sigma_{\textsc{w}}}\right)^{\land}$
(18)
$\displaystyle=\left((\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{\top}\boldsymbol{\omega}_{a}^{\Sigma_{\textsc{w}}}\right)^{\land}$
(19)
$\displaystyle=(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{\top}\widehat{\boldsymbol{\omega}}_{a}^{\Sigma_{\textsc{w}}}(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})$
(20)
$\displaystyle=(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{\top}\left(\dot{\mathbf{R}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{\top}\right)\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}$
(21)
$\displaystyle=(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{\top}\dot{\mathbf{R}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\left((\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{\top}\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\right)$
(22)
$\displaystyle=(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{\top}\dot{\mathbf{R}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}.$
(23)

Therefore, we observe that the left multiplication of the homogeneous
transformation matrix terms will be denoted as

$\left(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\right)^{-1}\dot{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}=\begin{bmatrix}\widehat{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}&\mathbf{v}_{a}^{[\Sigma_{a}]}\\\
\mathbf{0}&0\\\
\end{bmatrix}=\begin{bmatrix}{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}\\\
\mathbf{v}_{a}^{[\Sigma_{a}]}\\\
\end{bmatrix}^{\land}=\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{a}]}.$ (24)

In other words, the left matrix multiplication
$\left(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\right)^{-1}\dot{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}$
will be equivalent to the result we can get by applying the $\land$ operator
to the twist velocity ${\boldsymbol{\nu}}_{a}^{[\Sigma_{a}]}$ at joint $a$
within the local coordinate system $\Sigma_{a}$.

###  2.3 Twist Velocity Transformation across Coordinate Systems

In the previous subsection, we introduce the twist velocity vector for
readers, and also illustrate the twist velocity vector representations within
different coordinate systems subject to the new $\land$ operator. In this
part, we will further discuss about the relationships between the twist
velocity vectors $\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{a}]}$ and
$\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{\textsc{w}}]}$, as well as between
${\boldsymbol{\nu}}_{a}^{[\Sigma_{a}]}$ and
${\boldsymbol{\nu}}_{a}^{[\Sigma_{\textsc{w}}]}$, across coordinate systems.

####  2.3.1 Relationship Between
$\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{a}]}$ And
$\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{\textsc{w}}]}$

Let’s first talk about the relationship between
$\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{a}]}$ and
$\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{\textsc{w}}]}$ in different
coordinate systems. Based on the detailed derivations in the previous two
subsections, we know that

$\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{a}]}=\left(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\right)^{-1}\dot{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}.$
(25)

Meanwhile, since

$\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{\textsc{w}}]}=\dot{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\left(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\right)^{-1},$
(26)

by multiplying $\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}$ to both sides
of the above equation, we can get

$\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{\textsc{w}}]}\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}=\dot{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}.$
(27)

By replacing the representation of
$\dot{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}$ in the above equation
into Equation 25, we can illustrate the relationship between
$\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{\textsc{w}}]}$ and
$\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{a}]}$ as follows:

$\displaystyle\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{a}]}$
$\displaystyle=\left(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\right)^{-1}\dot{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}$
(28)
$\displaystyle=\left(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\right)^{-1}\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{\textsc{w}}]}\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}.$
(29)

From it, we can also obtain that

$\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{\textsc{w}}]}=\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{a}]}(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}.$
(30)

In other word, via the homogeneous transformation matrix, we can project the
twist velocity vectors subject to the $\land$ operator to each other.

####  2.3.2 Relationship Between ${\boldsymbol{\nu}}_{a}^{[\Sigma_{a}]}$ And
${\boldsymbol{\nu}}_{a}^{[\Sigma_{\textsc{w}}]}$

Based on the above analysis, we can further derive the relationship between
the twist velocity vectors ${\boldsymbol{\nu}}_{a}^{[\Sigma_{a}]}$ and
${\boldsymbol{\nu}}_{a}^{[\Sigma_{\textsc{w}}]}$. By replace the terms
$\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{\textsc{w}}]}$,
$\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}$ and
$\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{a}]}$ with their concrete matrix
representation into Equation 30, we can get

$\displaystyle\begin{bmatrix}\widehat{\boldsymbol{\omega}}_{a}^{[\Sigma_{\textsc{w}}]}&\mathbf{v}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&0\\\ \end{bmatrix}$ (31)
$\displaystyle=\begin{bmatrix}\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}&\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&1\\\
\end{bmatrix}\begin{bmatrix}\widehat{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}&\mathbf{v}_{a}^{[\Sigma_{a}]}\\\
\mathbf{0}&0\\\
\end{bmatrix}\begin{bmatrix}(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}&-(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&1\\\ \end{bmatrix}$ (32)
$\displaystyle=\begin{bmatrix}\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\widehat{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}&\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\mathbf{v}_{a}^{[\Sigma_{a}]}\\\
\mathbf{0}&0\\\
\end{bmatrix}\begin{bmatrix}(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}&-(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&1\\\ \end{bmatrix}$ (33)
$\displaystyle=\begin{bmatrix}\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\widehat{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}&-\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\widehat{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}+\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\mathbf{v}_{a}^{[\Sigma_{a}]}\\\
\mathbf{0}&0\\\ \end{bmatrix}$ (34)
$\displaystyle=\begin{bmatrix}\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\widehat{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{\top}&-\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\widehat{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{\top}\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}+\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\mathbf{v}_{a}^{[\Sigma_{a}]}\\\
\mathbf{0}&0\\\ \end{bmatrix}$ (35)
$\displaystyle=\begin{bmatrix}\left(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}\right)^{\land}&-\left(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}\right)^{\land}\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}+\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\mathbf{v}_{a}^{[\Sigma_{a}]}\\\
\mathbf{0}&0\\\ \end{bmatrix}$ (36)

Based on some basic properties about cross-product and the $\land$ operator on
the rotation matrix, the term
$-\left(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}\right)^{\land}\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}$
can be rewritten as

$\displaystyle-\left(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}\right)^{\land}\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}$
$\displaystyle=-\left(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}\right)\times\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}$
(38)
$\displaystyle=\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}\times\left(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}\right)$
(39)
$\displaystyle=(\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]})^{\land}\left(\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}\right)$
(40)
$\displaystyle=\widehat{\mathbf{p}}_{a}^{[\Sigma_{\textsc{w}}]}\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}.$
(41)

Therefore, from the above Equation 31, we can get that

$\begin{cases}{\boldsymbol{\omega}}_{a}^{[\Sigma_{\textsc{w}}]}&=\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}\\\
\mathbf{v}_{a}^{[\Sigma_{\textsc{w}}]}&=\widehat{\mathbf{p}}_{a}^{[\Sigma_{\textsc{w}}]}\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}+\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\mathbf{v}_{a}^{[\Sigma_{a}]}.\end{cases}$
(42)

We can also represent the above Equation 42 with linear algebra
representation, which will illustrate the relationship between
${\boldsymbol{\nu}}_{a}^{[\Sigma_{a}]}$ and
${\boldsymbol{\nu}}_{a}^{[\Sigma_{\textsc{w}}]}$ as follows:

$\begin{bmatrix}{\boldsymbol{\omega}}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{v}_{a}^{[\Sigma_{\textsc{w}}]}\end{bmatrix}=\underbrace{\begin{bmatrix}\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}&\mathbf{0}\\\
\widehat{\mathbf{p}}_{a}^{[\Sigma_{\textsc{w}}]}\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}&\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\\\
\end{bmatrix}}_{\text{adjoint representation
}Ad({{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}})}\begin{bmatrix}{\boldsymbol{\omega}}_{a}^{[\Sigma_{a}]}\\\
\mathbf{v}_{a}^{[\Sigma_{a}]}\end{bmatrix}$ (43)

###### Definition 5

(Adjoint Representation): Given the homogeneous transformation matrix

$\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}=\begin{bmatrix}\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}&\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&1\\\ \end{bmatrix}$ (44)

We can represent its adjoint representation as follows:

$Ad({{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}})=\begin{bmatrix}\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}&\mathbf{0}\\\
\widehat{\mathbf{p}}_{a}^{[\Sigma_{\textsc{w}}]}\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}&\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\\\
\end{bmatrix}$ (45)

The adjoint representation of the homogeneous transformation matrix will
project the twist velocity vectors across different coordinate systems. For
instance, from the above analysis, we can get that

$\boldsymbol{\nu}_{a}^{[\Sigma_{\textsc{w}}]}=Ad({{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}})\boldsymbol{\nu}_{a}^{[\Sigma_{a}]}.$
(46)

With a similar process, we can also obtain that

$\boldsymbol{\nu}_{a}^{[\Sigma_{a}]}=Ad({{\mathbf{T}}^{[\Sigma_{\textsc{w}}\to\Sigma_{a}]}})\boldsymbol{\nu}_{a}^{[\Sigma_{\textsc{w}}]}.$
(47)

###  2.4 Screw

We have been discussing about the robot body motion and twist velocity vector
above. Some questions may naturally arise in readers’ mind: “why do we need to
study both ‘robot rotation’ and ‘robot motion’?”

To answer the question, we need to distinguish the differences between “robot
rotation” and “robot motion”. As indicated by the name, “robot rotation” as
introduced in [5] denotes the rotational movement of the robot body (or part
of the body). Meanwhile, “robot motion” involves both the rotational movement,
as well as the translational movement, so it will be much more complicated to
model than the the robot rotation.

####  2.4.1 A Toy Example

Some readers probably may propose another question “Can we project the ‘robot
motion’ as a type of ‘robot rotation’ movement, or combine them and model them
as one movement?” In stead of directly answering the question, we can use a
toy example below to illustrate our answer.

Figure 2: A Example of Robot Motion. (The motion of coordinate $\Sigma_{a}$ to
$\Sigma_{b}$ via both rotation and translation can be modeled as a rotation
around point $q$ with an angle of $\theta$ degrees.)

###### Example 4

As shown in Figure 3, in the 3D space, we provide three coordinate systems:
(1) the world coordinate system $\Sigma_{\textsc{w}}$ with origin $o$, (2) the
local coordinate system $\Sigma_{a}$ before motion with origin at point
$\mathbf{p}_{a}=(1,2,0)^{\top}$, and (3) the local coordinate system
$\Sigma_{b}$ after motion with origin at point
$\mathbf{p}_{a}=(2,1,0)^{\top}$. As to the orientation, compared with the
world coordinate system $\Sigma_{\textsc{w}}$, coordinate system $\Sigma_{a}$
rotates in the counter clockwise direction around axis $z$ with an angle of
$30^{\circ}$, and $\Sigma_{b}$ rotates in he counter clockwise direction with
an angle of $60^{\circ}$.

Based on the orientation and positions of these two local coordinate systems,
we can define the homogeneous transformation matrices from them to the world
coordinate system as

$\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}=\begin{bmatrix}\cos
30^{\circ}&-\sin 30^{\circ}&0&1\\\ \sin 30^{\circ}&\cos 30^{\circ}&0&2\\\
0&0&1&0\\\
0&0&0&1\end{bmatrix},\mathbf{T}^{[\Sigma_{b}\to\Sigma_{\textsc{w}}]}=\begin{bmatrix}\cos
60^{\circ}&-\sin 60^{\circ}&0&2\\\ \sin 60^{\circ}&\cos 60^{\circ}&0&1\\\
0&0&1&0\\\ 0&0&0&1\end{bmatrix}.$ (48)

Based on them, we can calculate the transformation matrix from $\Sigma_{a}$ to
$\Sigma_{b}$ as

$\displaystyle\mathbf{T}^{[\Sigma_{b}\to\Sigma_{a}]}$
$\displaystyle=(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}\mathbf{T}^{[\Sigma_{b}\to\Sigma_{\textsc{w}}]}$
(49) $\displaystyle=\begin{bmatrix}0.866&-0.5&0&2.134\\\ 0.5&0.866&0&-1.232\\\
0&0&1&0\\\ 0&0&0&1\\\ \end{bmatrix}$ (50) $\displaystyle=\begin{bmatrix}\cos
30^{\circ}&-\sin 30^{\circ}&0&0.366\\\ \sin 30^{\circ}&\cos
30^{\circ}&0&-1.366\\\ 0&0&1&0\\\ 0&0&0&1\\\ \end{bmatrix}.$ (51)

So, according to matrix $\mathbf{T}^{[\Sigma_{b}\to\Sigma_{a}]}$, looking from
$\Sigma_{a}$, the motion of the local coordinate system from $\Sigma_{a}$ to
$\Sigma_{b}$ involves both (1) a rotation in the counter clockwise direction
with an angle of $30^{\circ}$ around the $z$ axis, and (2) a translation with
a position vector $(0.366,-1.366,0)^{\top}$.

Meanwhile, looking from the world coordinate system $\Sigma_{\textsc{w}}$, it
can also be modeled as a pure rotational movement around the axis vector at
point $q$, i.e., $\mathbf{p}_{q}=(3.37,3.37,0)^{\top}$, in the counter
clockwise direction with an angle of $\theta$ degrees actually.

In the above example, the movement involves both a rotational movement and a
translational movement, which can be modeled as a pure rotational movement
around certain axis in the space. How about a pure translational movement
without any rotations? Actually, the translational movement can still be
modeled as a rotational movement with the rotation radius approaches
$+\infty$.

####  2.4.2 Screw Axis

To formally model the rigid robot motion as a rotational movement, in this
part, we will introduce the concept of screw axis and will also introduce two
operators to project screw axis with the transformation matrix.

If the readers still remember, when introducing the angular velocity vector
$\boldsymbol{\omega}$, we mention that it can be represented as
$\boldsymbol{\omega}=\dot{q}\mathbf{e}_{\boldsymbol{\omega}}$, where
$\mathbf{e}_{\boldsymbol{\omega}}$ is a unit vector indicating the direction
of the angular velocity vector and scalar $\dot{q}$ denotes the rate of ration
around the axis with the unit rad/s. It helps interpret the physical meaning
of the angular velocity vector. Meanwhile, when it comes to the twist velocity
vector $\boldsymbol{\nu}$, the readers probably may also wonder if it can also
be interpreted in a similar way or not. In this part, we aim to answer this
question and represent the twist velocity vector with the screw motion.

###### Example 5

Figure 3: An Example of Screw Rotational Movement. (Unit vector $\mathbf{e}$:
the direction of the screw axis; Scalar $\dot{q}$: rotation rate of screw;
Scalar $h$: pitch of screw and it equals to “linear velocity scalar along
screw axis/angular velocity scalar”; Vector $\mathbf{p}$: it denotes a point
on the screw axis.)

The twist velocity vector contains both angular velocity and linear velocity,
which may remind us about the screw rotational movement. In Figure 3, we show
an example of a screw rotating around the axis with direction indicated by a
unit vector $\mathbf{e}_{s}$. The screw rotating rate can be denoted by a
scalar $\dot{q}$ with unit rad/s, and the screw pitch is represented by
another scalar $h$ in this example, which denotes the forward movement
distance as the screw rotates an angle of $360^{\circ}$ degrees. One of the
point on the axis is denoted by position vector $\mathbf{p}$. The collection
of $\mathbf{s}=(\mathbf{p},\mathbf{e}_{s},h)$ together defines the screw axis
representation, and $\dot{q}$ denotes the angular velocity scalar around the
screw axis.

Screw Representation: Based on the above screw representation, given a twist
velocity vector $\boldsymbol{\nu}$, by choosing proper elements of the screw
axis, we can also represent it with the screw axis together with the
corresponding rotational angular velocity scalar $\dot{q}$ as follows:

$\boldsymbol{\nu}=\begin{bmatrix}\boldsymbol{\omega}\\\ \mathbf{v}\\\
\end{bmatrix}=\begin{bmatrix}\dot{q}\mathbf{e}_{s}\\\
-\dot{q}\mathbf{e}_{s}\times\mathbf{p}+h\mathbf{e}_{s}\dot{q}\\\
\end{bmatrix}.$ (52)

The physical meaning of $\dot{q}\mathbf{e}_{s}$ denotes the angular velocity
of the screw in rotation, As to the linear velocity representation
$-\dot{q}\mathbf{e}_{s}\times\mathbf{p}+h\mathbf{e}_{s}\dot{q}$, it has two
parts: (1) term $h\mathbf{e}_{s}\dot{q}$ denotes the translational linear
velocity that the screw moves forward as it rotates, and (2)
$-\dot{q}\mathbf{e}_{s}\times\mathbf{p}$ denotes the linear velocity at the
origin due to the rotational movement about the axis.

####  2.4.3 Screw Axis Representation Simplification

The above screw axis vector $\mathbf{s}$ and rotational velocity scalar
$\dot{q}$ representations carry concrete physical meanings. Meanwhile, given a
random twist velocity vector $\boldsymbol{\nu}$, calculating the
configurations of the screw axis vector $\mathbf{s}$ and rotational velocity
scalar $\dot{q}$ that can make the equation hold is not an easy task. In this
part, we propose to simplify the above calculation process.

Simplified Version 1: In application, for any twist velocity vector
$\mathbf{\nu}=\begin{bmatrix}\boldsymbol{\omega}\\\ \mathbf{v}\\\
\end{bmatrix}$ input,

  * •

if $\boldsymbol{\omega}\neq\mathbf{0}$, there also exist an equivalent
representation of the screw axis vector $\mathbf{s}$ and rotational velocity
scalar $\dot{q}$ that can make Equation 52 hold, where
$\mathbf{e}_{s}=\boldsymbol{\omega}/\left\|\boldsymbol{\omega}\right\|$,
$\dot{q}=\left\|\boldsymbol{\omega}\right\|$, and
$h=\mathbf{e}_{s}^{\top}\mathbf{v}/\dot{q}$. Meanwhile, the position vector
$\mathbf{p}$ is chosen so that the term
$-\dot{q}\mathbf{e}_{s}\times\mathbf{p}$ provides the portion of $\mathbf{v}$
orthogonal to the screw axis;

  * •

if $\boldsymbol{\omega}=\mathbf{0}$, then the pitch $h$ will be $+\infty$ and
$\mathbf{e}_{s}=\mathbf{v}/\left\|\mathbf{v}\right\|$ and $\dot{q}$ is
interpreted as the linear velocity $\left\|\mathbf{v}\right\|$ along
$\mathbf{e}_{s}$.

The readers may have also observe some potential problems with the above
simplified representation of the twist velocity vector as the screw axis and
angular velocity representation, e.g., the pitch $h$ can be $+\infty$, vector
$\mathbf{p}$ is not unique, and the calculation process is very cumbersome.
Therefore, we will introduce to define the screw axis vector $\mathbf{s}$ as a
normalized version of any twist velocity vector $\boldsymbol{\nu}$
corresponding to the motion along the screw.

Simplified Version 2: For any twist velocity vector
$\mathbf{\nu}=\begin{bmatrix}\boldsymbol{\omega}\\\ \mathbf{v}\\\
\end{bmatrix}$ input,

  * •

if $\boldsymbol{\omega}\neq\mathbf{0}$, we define the screw axis
$\mathbf{s}=\mathbf{\nu}/\left\|\boldsymbol{\omega}\right\|=\begin{bmatrix}\frac{\boldsymbol{\omega}}{\left\|\boldsymbol{\omega}\right\|}\\\
\frac{\mathbf{v}}{\left\|\boldsymbol{\omega}\right\|}\\\ \end{bmatrix}$, and
the angular velocity scalar about the axis as
$\dot{q}=\left\|\boldsymbol{\omega}\right\|$.

  * •

if $\boldsymbol{\omega}=\mathbf{0}$, we define the screw axis
$\mathbf{s}=\mathbf{\nu}/\left\|\mathbf{v}\right\|=\begin{bmatrix}\mathbf{0}\\\
\frac{\mathbf{v}}{\left\|\mathbf{v}\right\|}\\\ \end{bmatrix}$, and the
angular velocity scalar about the axis as $\dot{q}=\left\|\mathbf{v}\right\|$.

Based on the above simplified version, we can provide the formal definition of
screw axis as follows:

###### Definition 6

(Screw Axis): For a given coordinate system, a screw axis can be formally
defined as

$\mathbf{s}=\begin{bmatrix}\boldsymbol{\omega}\\\ \mathbf{v}\\\
\end{bmatrix}\in\mathbbm{R}^{6},$ (53)

where either (1) $\left\|\boldsymbol{\omega}\right\|=1$ or (2)
$\boldsymbol{\omega}=\mathbf{0}$ and $\left\|\mathbf{v}\right\|=1$.

According to the above definition, the screw axis vector is actually a
normalized version of the twist velocity vector, and we have the equation
$\boldsymbol{\nu}=\dot{q}\mathbf{s}$ hold. Also many of the properties,
operators and transformation that can be applied to the twist velocity vector
will also hold and applied to the screw axis vector, e.g., the $\land$
operator.

Given a screw axis representation
$\mathbf{s}=\begin{bmatrix}\boldsymbol{\omega}\\\ \mathbf{v}\\\
\end{bmatrix}$, we can represent its matrix representation subject to the
$\land$ operator as follows:

$\widehat{\mathbf{s}}=\begin{bmatrix}\widehat{\boldsymbol{\omega}}&\mathbf{v}\\\
\mathbf{0}&0\\\ \end{bmatrix},\text{ where
}\widehat{\boldsymbol{\omega}}=\begin{bmatrix}0&-\omega_{3}&\omega_{2}\\\
\omega_{3}&0&-\omega_{1}\\\ -\omega_{2}&\omega_{1}&0\\\ \end{bmatrix}.$ (54)

With the adjoint representation of the homogeneous transformation matrix, we
can also convert the screw axis vectors across coordinate systems, e.g.,

$\widehat{\mathbf{s}}^{[\Sigma_{\textsc{w}}]}=Ad(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})\widehat{\mathbf{s}}^{[\Sigma_{a}]}\text{,
and
}\widehat{\mathbf{s}}^{[\Sigma_{a}]}=Ad(\mathbf{T}^{[\Sigma_{\textsc{w}}\to\Sigma_{a}]})\widehat{\mathbf{s}}^{[\Sigma_{\textsc{w}}]},$
(55)

where $\widehat{\mathbf{s}}^{[\Sigma_{\textsc{w}}]}$ and
$\widehat{\mathbf{s}}^{[\Sigma_{a}]}$ denote two screw axis vectors in the
coordinate systems $\Sigma_{\textsc{w}}$ and $\Sigma_{a}$, respectively. Terms
$\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}$ and
$\mathbf{T}^{[\Sigma_{\textsc{w}}\to\Sigma_{a}]}$ represent the homogeneous
transformation matrices between these two coordinate systems.

###  2.5 Motion Exponential and Logarithm

In the previous tutorial article [5], we have introduced that the rotation
matrix $\mathbf{R}$ can be represented as the exponential of its corresponding
angular velocity vector $\boldsymbol{\omega}$, i.e.,

$\mathbf{R}(t)=\exp^{\widehat{\boldsymbol{\omega}}t}.$ (56)

Since we have modeled the rigid-body motion of robots as the rotational
movement in this section, the readers probably may also wonder if the
homogeneous transformation matrix $\mathbf{T}$ will also have similar
exponential representations of the screw axis representation $\mathbf{s}$ or
not? This is what we plan to introduce in this part.

####  2.5.1 Motion Exponential Representation

We have spent lots of space introducing the rigid-body robot motion, twist
velocity and the screw axis before already. Here, instead of showing the
detailed derivation steps, we will directly define the exponential operator
that will project the screw axis to the homogeneous transformation matrix.

###### Definition 7

(Exponential Operator): Given the screw axis
$\mathbf{s}=\begin{bmatrix}\boldsymbol{\omega}\\\ \mathbf{v}\\\
\end{bmatrix}\in\mathbbm{R}^{6}$ and the rotation angle $q$, we define a
matrix exponential operator to project the screw rotation to the corresponding
homogeneous transformation matrix $\mathbf{T}\in\mathbbm{R}^{4\times 4}$ as
follows:

$\exp:\widehat{\mathbf{s}}q\to\mathbf{T}.$ (57)

Specifically, for the screw axis
$\mathbf{s}=\begin{bmatrix}\boldsymbol{\omega}\\\ \mathbf{v}\\\
\end{bmatrix}$, depending on its element values, we can also provide the
closed-form representation of term $\exp^{\widehat{\mathbf{s}}q}$ illustrated
as follows:

  * •

if $\left\|\boldsymbol{\omega}\right\|=1$, we can also provide the closed-form
representation of the exponential term $\exp^{\widehat{\mathbf{s}}q}$ as
follows:

$\exp^{\widehat{\mathbf{s}}q}=\begin{bmatrix}\exp^{\widehat{\boldsymbol{\omega}}q}&\left(\mathbf{I}q+(1-\cos
q)\widehat{\boldsymbol{\omega}}+(q-\sin
q)\widehat{\boldsymbol{\omega}}^{2}\right)\mathbf{v}\\\ \mathbf{0}&1\\\
\end{bmatrix}.$ (58)

  * •

if the angular velocity vector $\boldsymbol{\omega}=\mathbf{0}$ and
$\left\|\mathbf{v}\right\|=1$, the above matrix representation can also be
simplified as

$\exp^{\widehat{\mathbf{s}}q}=\begin{bmatrix}\mathbf{i}&\mathbf{v}q\\\
\mathbf{0}&1\\\ \end{bmatrix}.$ (59)

Based on the exponential operator, given a screw axis vector, we can calculate
the corresponding homogeneous transformation matrix that can describe the
identical robot motion across coordinate systems.

####  2.5.2 Matrix Logarithm Representation

Let’s also look at the reversed direction. Given a homogeneous transformation
matrix $\mathbf{T}=\begin{bmatrix}\mathbf{R}&\mathbf{p}\\\
\mathbf{0}&0\end{bmatrix}$, can we also find a screw axis
$\mathbf{s}\in\mathbbm{R}^{6}$ and a rotation angle scalar $q\in\mathbbm{R}$
such that $\exp^{\widehat{\mathbf{s}}q}=\mathbf{T}$? Besides the exponential
operator, we will also introduce the a logarithm operator here to achieve such
an objective.

###### Definition 8

(Logarithm Operator): Given a homogeneous transformation matrix
$\mathbf{T}=\begin{bmatrix}\mathbf{R}&\mathbf{p}\\\
\mathbf{0}&0\end{bmatrix}$, via the logarithm operator, we can project
$\mathbf{T}$ to a screw axis $\mathbf{s}\in\mathbbm{R}^{6}$ and a rotation
angle scalar $q\in\mathbbm{R}$ shown as follows:

$\ln:\mathbf{T}\to\widehat{\mathbf{s}}q.$ (60)

Specifically, depending on the values in matrix $\mathbf{T}$, we can also
provide the closed-form representation of the corresponding screw axis
$\mathbf{s}\in\mathbbm{R}^{6}$ and rotation angle scalar $q\in\mathbbm{R}$:

  * •

if $\mathbf{R}=\mathbf{I}$, then we can set $\boldsymbol{\omega}=\mathbf{0}$,
$\mathbf{v}=\mathbf{p}/\left\|\mathbf{p}\right\|$, and
$q=\left\|\mathbf{p}\right\|$.

  * •

otherwise, we can use the logarithm operator defined before for rotation
matrix to calculate the angular velocity vector and the angle scalar $q$ as

$\displaystyle\boldsymbol{\omega}$
$\displaystyle=\left(\ln\mathbf{R}\right)^{\lor}$ (61) $\displaystyle q$
$\displaystyle=\text{atan2}\left(\left\|(\mathbf{R}_{32}-\mathbf{R}_{23},\mathbf{R}_{13}-\mathbf{R}_{31},\mathbf{R}_{21}-\mathbf{R}_{12})^{\top}\right\|,\mathbf{R}_{11}+\mathbf{R}_{22}+\mathbf{R}_{33}-1\right).$
(62)

As to the linear velocity vector $\mathbf{v}$, we can represent it as

$\mathbf{v}=\mathbf{G}^{-1}\mathbf{p}\text{, where matrix
}\mathbf{G}=\mathbf{I}q+(1-\cos q)\widehat{\boldsymbol{\omega}}+(q-\sin
q)\widehat{\boldsymbol{\omega}}^{2}.$ (63)

###  2.6 Wrench

At the end of this section, we also plan to provide a brief introduction about
the wrench of robot motion, which illustrate the relationship between forces
and torques. More detailed information about force and torque will be
discussed in the last section about robot dynamics for readers.

####  2.6.1 Wrench Definition

From basic knowledge we learn from physics, given a force denoted by vector
$\mathbf{f}_{e}$ acting on the end point $e$ of a robot body, whose position
can be denoted as vector $\mathbf{p}_{e}$, we can represent the generated
torque as

$\boldsymbol{\tau}_{e}=\mathbf{p}_{e}\times\mathbf{f}_{e}.$ (64)

Similar to twist velocity vector, the force and generated torque vectors can
also be organized into a vector of $6$ elements, which is formally defined as
the wrench vector as follows:

$\mathbf{w}_{e}=\begin{bmatrix}\boldsymbol{\tau}_{e}\\\ \mathbf{f}_{e}\\\
\end{bmatrix}\in\mathbbm{R}^{6}.$ (65)

For the multiple wrenches are acting on the same body, the total wrench is
defined as the sum of these individual wrench vectors. Readers probably have
also noticed that the above wrench vector is very similar to the twist
velocity vector, and the torque vector can also be expressed as the cross-
product of the force and the position vectors. Meanwhile, they also have minor
differences, e.g., (1) $\boldsymbol{\tau}=\mathbf{p}\times\mathbf{f}$ (vector
$\mathbf{p}$ is multiplied at the left side) but
$\mathbf{v}=\boldsymbol{\omega}\times\mathbf{p}$ (vector $\mathbf{p}$ at the
right side); and (2) in the wrench vector $\mathbf{w}$, term
$\boldsymbol{\tau}$ obtained via $\mathbf{p}\times\mathbf{f}$ is at the top,
but in the twist velocity vector $\boldsymbol{\nu}$, the linear velocity
$\mathbf{v}$ obtained by cross product term
$\boldsymbol{\omega}\times\mathbf{p}$ is at the bottom instead. Due to these
differences, we cannot simply apply the operators introduced before to the
wrench vector here.

####  2.6.2 Wrench Transformation

Actually, via the power quantity that drives the motion, we can further
illustrate the relationships between wrench and twist more clearly. The wrench
vectors in different coordinate systems can also be converted into each other
via the homogeneous transformation matrix. Given two coordinate systems
$\Sigma_{a}$ and $\Sigma_{b}$, we can represent two wrench vectors acting on
the end point $e$ defined in each of them as $\mathbf{w}_{e}^{[\Sigma_{a}]}$
and $\mathbf{w}_{e}^{[\Sigma_{b}]}$, respectively. At the same time, we can
also represent the twist vectors of the end point $e$ in these two coordinate
systems as vectors $\boldsymbol{\nu}_{e}^{[\Sigma_{a}]}$ and
$\boldsymbol{\nu}_{e}^{[\Sigma_{b}]}$. We know that the power that drives the
motion of the rigid-body robot can be represented as

$P=\mathbf{f}_{e}^{\top}\times\mathbf{v}_{e}=\boldsymbol{\tau}_{e}^{\top}\boldsymbol{\omega}_{e}.$
(66)

Also the power $P$ is scalar that is independent of the coordinate systems.
Viewed in such a perspective, we can get that

$\underbrace{(\boldsymbol{\nu}_{e}^{[\Sigma_{a}]})^{\top}\mathbf{w}_{e}^{[\Sigma_{a}]}}_{(\boldsymbol{\tau}_{e}^{[\Sigma_{a}]})^{\top}\boldsymbol{\omega}_{e}^{[\Sigma_{a}]}+(\mathbf{f}_{e}^{[\Sigma_{a}]})^{\top}\times\mathbf{v}_{e}^{[\Sigma_{a}]}=2P}=\underbrace{(\boldsymbol{\nu}_{e}^{[\Sigma_{b}]})^{\top}\mathbf{w}_{e}^{[\Sigma_{b}]}}_{(\boldsymbol{\tau}_{e}^{[\Sigma_{b}]})^{\top}\boldsymbol{\omega}_{e}^{[\Sigma_{b}]}+(\mathbf{f}_{e}^{[\Sigma_{b}]})^{\top}\times\mathbf{v}_{e}^{[\Sigma_{b}]}=2P}.$
(67)

According to the above Equation 46 and Equation 47, we know that

$\boldsymbol{\nu}_{e}^{[\Sigma_{b}]}=Ad(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{b}]})\boldsymbol{\nu}_{e}^{[\Sigma_{a}]}$
(68)

By replacing the above representation of $\boldsymbol{\nu}_{e}^{[\Sigma_{b}]}$
into Equation 67, we will have

$\displaystyle(\boldsymbol{\nu}_{e}^{[\Sigma_{a}]})^{\top}\mathbf{w}_{e}^{[\Sigma_{a}]}$
$\displaystyle=(\boldsymbol{\nu}_{e}^{[\Sigma_{b}]})^{\top}\mathbf{w}_{e}^{[\Sigma_{b}]}$
(69)
$\displaystyle=\left(Ad(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{b}]})\boldsymbol{\nu}_{e}^{[\Sigma_{a}]}\right)^{\top}\mathbf{w}_{e}^{[\Sigma_{b}]}$
(70)
$\displaystyle=(\boldsymbol{\nu}_{e}^{[\Sigma_{a}]})^{\top}Ad(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{b}]})^{\top}\mathbf{w}_{e}^{[\Sigma_{b}]}.$
(71)

Since the above equation should hold for any twist velocity vector
$\boldsymbol{\nu}_{e}^{[\Sigma_{a}]}\in\mathbbm{R}^{6}$, it can lead to

$\mathbf{w}_{e}^{[\Sigma_{a}]}=Ad(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{b}]})^{\top}\mathbf{w}_{e}^{[\Sigma_{b}]}.$
(72)

With the same method, we can also derive that

$\mathbf{w}_{e}^{[\Sigma_{b}]}=Ad(\mathbf{T}^{[\Sigma_{b}\to\Sigma_{a}]})^{\top}\mathbf{w}_{e}^{[\Sigma_{a}]}.$
(73)

##  3 Forward Kinematics

In this section, we will talk about the forward kinematics for readers on
robot motion. Based on the robot motion introduced in the previous article
[5], in this part, we will show readers how the robot body (involving multiple
joints and links) will move to the desired position and state.

###  3.1 What is Forward Kinematics?

The rigid robot body can actually be viewed as mapping, which projects the
input motor movement parameters (e.g., desired angles $\mathbf{q}$, velocity
$\dot{\mathbf{q}}$ and acceleration $\ddot{\mathbf{q}}$) to the desired robot
position and state (e.g., position $\mathbf{p}$, orientation $\mathbf{R}$,
velocity $\mathbf{v}$ and force $\mathbf{f}$ of certain robot body parts).

###### Definition 9

(Forward Kinematics): Formally, forward kinematics is a calculation to obtain
the desired robot position, orientation and other properties of links in the
body from a given input parameter.

If we represent the input parameters as a vector $\boldsymbol{\theta}(t)$,
subject the pre-defined rigid robot body, we can represent the desired robot
outputs as

$\mathbf{o}(t)=g\left(\boldsymbol{\theta}(t)\right).$ (74)

Since the input parameter and the desired output are both changing with time,
we specify that vectors $\boldsymbol{\theta}(t)$ and $\mathbf{o}(t)$ both have
time as their variable. Meanwhile, the calculation process of the mapping
$g\left(\cdot\right)$ to achieve the output is called the forward kinematics
process.

Robot forward kinematics serves as the basis for robot simulation and control.
It helps display the current state of the robot, calculate the center of mass
of the robot body and detect potential collisions of the robot with its body
parts and the environment. In this section, we will introduce the forward
kinematics of robots in terms of the velocity, orientation, velocity force and
torque for readers, respectively.

###  3.2 Pose in Forward Kinematics

We have been discussing about the position and orientation changes of robot
parts in both the previous article [5] and in the previous Section 2 already.
Here, we will just provide a very brief introduction about updating robot
pose, covering both position and orientation, in forward kinematics with the
chain rule and exponential representation, respectively.

Figure 4: The Robot Arm Example We Use Before.

####  3.2.1 Chain Rule

According to our previous discussions, readers should be able to understand
how the rotation matrix projects position vectors across coordinate systems.
In some sense, the rotation matrix also indicates the orientation (relative to
the world coordinate system) of robot actually within local or world
coordinate systems.

###### Definition 10

(Pose): The combination of both position and orientation defines the pose of
the robot, which can be represented as the position vector and rotation matrix
pair $\left(\mathbf{p},\mathbf{R}\right)$. Since the homogeneous
transformation matrix $\mathbf{T}=\begin{bmatrix}\mathbf{R}&\mathbf{p}\\\
\mathbf{0}&1\end{bmatrix}$ covers both $\mathbf{p}$ and $\mathbf{R}$, we can
also say the homogeneous transformation matrix $\mathbf{T}$ represents the
pose of robots.

As introduced in the previous section, for the robot arm shown in Figure 1,
the position and orientation of its end point can be calculated via the chain
rule, which will obtain the homogeneous transition matrix
$\mathbf{T}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}$:

$\displaystyle\mathbf{T}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}$ (75)
$\displaystyle=\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\mathbf{T}^{[\Sigma_{b}\to\Sigma_{a}]}\mathbf{T}^{[\Sigma_{c}\to\Sigma_{b}]}$
(76)
$\displaystyle=\begin{bmatrix}\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}&\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&1\\\
\end{bmatrix}\begin{bmatrix}\mathbf{R}^{[\Sigma_{b}\to\Sigma_{a}]}&\mathbf{p}_{a\to
b}^{[\Sigma_{a}]}\\\ \mathbf{0}&1\\\
\end{bmatrix}\begin{bmatrix}\mathbf{R}^{[\Sigma_{c}\to\Sigma_{b}]}&\mathbf{p}_{b\to
c}^{[\Sigma_{b}]}\\\ \mathbf{0}&1\\\ \end{bmatrix}$ (77)
$\displaystyle=\begin{bmatrix}\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\mathbf{R}^{[\Sigma_{b}\to\Sigma_{a}]}\mathbf{R}^{[\Sigma_{c}\to\Sigma_{b}]}&\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}+\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\mathbf{p}_{a\to
b}^{[\Sigma_{a}]}+\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\mathbf{R}^{[\Sigma_{b}\to\Sigma_{a}]}\mathbf{p}_{b\to
c}^{[\Sigma_{b}]}\\\ \mathbf{0}&1\\\ \end{bmatrix}$ (78)

Since the rigid-body robot arm is normally straight, the orientation of the
end point is actually the same as the orientation of the joint that controls
the last arm link. From the above representation of the homogeneous
transformation matrix $\mathbf{T}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}$, we
can denote the orientation of the end point of the arm with the rotation
matrix

$\mathbf{R}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}=\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\mathbf{R}^{[\Sigma_{b}\to\Sigma_{a}]}\mathbf{R}^{[\Sigma_{c}\to\Sigma_{b}]}.$
(79)

As introduced in the previous section, based on the end point’s current
position vector within the local coordinate $\Sigma_{c}$, we can calculate its
end point’s position within the world coordinate as

$\begin{bmatrix}\mathbf{p}_{e}^{[\Sigma_{\textsc{w}}]}\\\
1\end{bmatrix}=\mathbf{T}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}\begin{bmatrix}\mathbf{p}_{c\to
e}^{[\Sigma_{c}]}\\\ 1\end{bmatrix}.$ (80)

The vector $\mathbf{p}_{e}^{[\Sigma_{\textsc{w}}]}$ indicating the robot arm
end point’s position can also be represented as the following summation
formula:

$\displaystyle\mathbf{p}_{e}^{[\Sigma_{\textsc{w}}]}=$
$\displaystyle\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}+\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\mathbf{p}_{a\to
b}^{[\Sigma_{a}]}+\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\mathbf{R}^{[\Sigma_{b}\to\Sigma_{a}]}\mathbf{p}_{b\to
c}^{[\Sigma_{b}]}$ (81)
$\displaystyle+\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\mathbf{R}^{[\Sigma_{b}\to\Sigma_{a}]}\mathbf{R}^{[\Sigma_{c}\to\Sigma_{b}]}\mathbf{p}_{c\to
e}^{[\Sigma_{c}]}.$ (82)

####  3.2.2 Product of Exponentials

In the above derivation, we assume the rotation matrix for each joint is known
and can be used for calculating both the orientation and position of the robot
arm end point. Meanwhile, when the rotation matrices are unknown, we can still
calculate the end point orientation and position via the exponential operator
we introduce in the previous article [5] based on the rotation angles or the
joints.

As introduced in [5], the homogeneous transformation matrix of the robot arm
joint can be represented with its angular velocity vector with the exponential
operator as follows:

$\displaystyle\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}=\begin{bmatrix}\mathbf{R}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}&\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&1\\\
\end{bmatrix}=\begin{bmatrix}\exp^{\widehat{\mathbf{e}}_{a}^{[\Sigma_{\textsc{w}}]}\dot{q}_{a}}&\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}\\\
\mathbf{0}&1\\\ \end{bmatrix},$ (83)

where vector ${\mathbf{e}}_{a}^{[\Sigma_{\textsc{w}}]}$ and scalar
$\dot{q}_{a}$ denote the rotation axis and rotation velocity, respectively.

In a similar way, we can also represent the rotation matrix and homogeneous
transformation matrix with origins at other joints with their corresponding
angular velocity vector as follows:

$\displaystyle\mathbf{T}^{[\Sigma_{b}\to\Sigma_{a}]}$
$\displaystyle=\begin{bmatrix}\mathbf{R}^{[\Sigma_{b}\to\Sigma_{a}]}&\mathbf{p}_{a\to
b}^{[\Sigma_{a}]}\\\ \mathbf{0}&1\\\
\end{bmatrix}=\begin{bmatrix}\exp^{\widehat{\mathbf{e}}_{b}^{[\Sigma_{a}]}\dot{q}_{b}}&\mathbf{p}_{a\to
b}^{[\Sigma_{a}]}\\\ \mathbf{0}&1\\\ \end{bmatrix},$ (84)
$\displaystyle\mathbf{T}^{[\Sigma_{c}\to\Sigma_{b}]}$
$\displaystyle=\begin{bmatrix}\mathbf{R}^{[\Sigma_{c}\to\Sigma_{b}]}&\mathbf{p}_{b\to
c}^{[\Sigma_{b}]}\\\ \mathbf{0}&1\\\
\end{bmatrix}=\begin{bmatrix}\exp^{\widehat{\mathbf{e}}_{c}^{[\Sigma_{b}]}\dot{q}_{c}}&\mathbf{p}_{b\to
c}^{[\Sigma_{b}]}\\\ \mathbf{0}&1\\\ \end{bmatrix}.$ (85)

By replacing them into the rotation matrix and end point position vector
calculation, we can obtain their representations as follows:

$\displaystyle\mathbf{R}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}$
$\displaystyle=\left(\exp^{\widehat{\mathbf{e}}_{a}^{[\Sigma_{\textsc{w}}]}\dot{q}_{a}}\exp^{\widehat{\mathbf{e}}_{b}^{[\Sigma_{a}]}\dot{q}_{b}}\exp^{\widehat{\mathbf{e}}_{c}^{[\Sigma_{b}]}\dot{q}_{c}}\right),$
(86) $\displaystyle\mathbf{p}_{e}^{[\Sigma_{\textsc{w}}]}$
$\displaystyle=\mathbf{p}_{a}^{[\Sigma_{\textsc{w}}]}+\left(\exp^{\widehat{\mathbf{e}}_{a}^{[\Sigma_{\textsc{w}}]}\dot{q}_{a}}\mathbf{p}_{a\to
b}^{[\Sigma_{a}]}\right)+\left(\exp^{\widehat{\mathbf{e}}_{a}^{[\Sigma_{\textsc{w}}]}\dot{q}_{a}}\exp^{\widehat{\mathbf{e}}_{b}^{[\Sigma_{a}]}\dot{q}_{b}}\mathbf{p}_{b\to
c}^{[\Sigma_{b}]}\right)$ (87)
$\displaystyle+\left(\exp^{\widehat{\mathbf{e}}_{a}^{[\Sigma_{\textsc{w}}]}\dot{q}_{a}}\exp^{\widehat{\mathbf{e}}_{b}^{[\Sigma_{a}]}\dot{q}_{b}}\exp^{\widehat{\mathbf{e}}_{c}^{[\Sigma_{b}]}\dot{q}_{c}}\mathbf{p}_{c\to
e}^{[\Sigma_{c}]}\right).$ (88)

The above representations of rotation matrix and end point position vector is
also named as the product of exponentials of the joint angular velocity
vectors.

###  3.3 Velocity in Forward Kinematics

Let’s revisit the Equaton 74 provided at the beginning of this section, if we
take the desired output vector as the position vector $\mathbf{p}(t)$ and the
input as the angles of the joints $\mathbf{q}(t)$, we can represent the
desired output as follows:

$\mathbf{p}(t)=g\left(\mathbf{q}(t)\right).$ (89)

####  3.3.1 Jacobian Matrix

By taking the derivatives of both size of the above equation with regard to
the time variable $t$, we can obtain

$\displaystyle\dot{\mathbf{p}}(t)$ $\displaystyle=\frac{\partial
g\left(\mathbf{q}\right)}{\partial\mathbf{q}}\frac{\partial\mathbf{q}(t)}{\partial
t}=\frac{\partial
g\left(\mathbf{q}\right)}{\partial\mathbf{q}}\dot{\mathbf{q}}(t)$ (90)
$\displaystyle=\mathbf{J}\dot{\mathbf{q}}(t),$ (91)

where term $\dot{\mathbf{p}}(t)$ denotes the linear velocity vector and the
matrix $\mathbf{J}=\frac{\partial
g\left(\mathbf{q}\right)}{\partial\mathbf{q}}$ about the joint variables
$\mathbf{q}$ is called the Jacobian matrix.

###### Example 6

For instance, let’s assume the length of the robot arm links starting from the
origin $o$, joints $a$, $b$ and $c$ are $l_{1}$, $l_{2}$, $l_{3}$ and $l_{4}$,
respectively. Based on the rotation angles of these links, we can provide the
analytic representation of the end point position in the world coordinate
system as

$\displaystyle x_{e}$ $\displaystyle=0,$ (92) $\displaystyle y_{e}$
$\displaystyle=l_{2}\sin\theta+l_{3}\sin(\theta+\phi)+l_{4}\sin(\theta+\phi+\psi),$
(93) $\displaystyle z_{e}$
$\displaystyle=l_{1}+l_{2}\cos\theta+l_{3}\cos(\theta+\phi)+l_{4}\cos(\theta+\phi+\psi).$
(94)

By taking derivatives of both sides of the above equations, we can obtain

$\displaystyle\dot{x}_{e}$ $\displaystyle=0,$ (95) $\displaystyle\dot{y}_{e}$
$\displaystyle=l_{2}\dot{\theta}\cos\theta+l_{3}(\dot{\theta}+\dot{\phi})\cos(\theta+\phi)+l_{4}(\dot{\theta}+\dot{\phi}+\dot{\psi})\cos(\theta+\phi+\psi),$
(96) $\displaystyle\dot{z}_{e}$ $\displaystyle=-l_{2}\dot{\theta}\sin\theta-
l_{3}(\dot{\theta}+\dot{\phi})\sin(\theta+\phi)-l_{4}(\dot{\theta}+\dot{\phi}+\dot{\psi})\sin(\theta+\phi+\psi).$
(97)

The above equations can also be represented in the form of
$\dot{\mathbf{p}}=\mathbf{J}\dot{\mathbf{q}}$ as follows:

$\underbrace{\begin{bmatrix}\dot{x}_{e}\\\ \dot{y}_{e}\\\ \dot{z}_{e}\\\
\end{bmatrix}}_{\dot{\mathbf{p}}}=\underbrace{\begin{bmatrix}0&0&0\\\
\mathbf{J}_{2,1}&\mathbf{J}_{2,2}&\mathbf{J}_{2,3}\\\
\mathbf{J}_{3,1}&\mathbf{J}_{3,2}&\mathbf{J}_{3,3}\\\
\end{bmatrix}}_{\mathbf{J}}\underbrace{\begin{bmatrix}\dot{\theta}\\\
\dot{\phi}\\\ \dot{\psi}\\\ \end{bmatrix}}_{\dot{\mathbf{q}}},$ (98)

where the matrix elements

$\displaystyle\mathbf{J}_{2,1}$
$\displaystyle=l_{2}\cos\theta+l_{3}\cos(\theta+\phi)+l_{4}\cos(\theta+\phi+\psi),$
(99) $\displaystyle\mathbf{J}_{2,2}$
$\displaystyle=l_{3}\cos(\theta+\phi)+l_{4}\cos(\theta+\phi+\psi),$ (100)
$\displaystyle\mathbf{J}_{2,3}$ $\displaystyle=l_{4}\cos(\theta+\phi+\psi),$
(101) $\displaystyle\mathbf{J}_{3,1}$ $\displaystyle=-l_{2}\sin\theta-
l_{3}\sin(\theta+\phi)-l_{4}\sin(\theta+\phi+\psi),$ (102)
$\displaystyle\mathbf{J}_{3,2}$
$\displaystyle=-l_{3}\sin(\theta+\phi)-l_{4}\sin(\theta+\phi+\psi),$ (103)
$\displaystyle\mathbf{J}_{3,3}$ $\displaystyle=-l_{4}\sin(\theta+\phi+\psi).$
(104)

###### Example 7

For instance, if the arm link length $l_{1}=l_{2}=l_{3}=l_{4}=1$, and the arm
joint angles $\theta=\frac{\pi}{4}$, $\phi=\frac{\pi}{2}$ and
$\psi=\frac{3\pi}{4}$, we can represent the corresponding Jacobian matrix as
follows:

$\displaystyle\mathbf{J}$ $\displaystyle=\begin{bmatrix}0&0&0\\\
\left(\cos\frac{\pi}{4}+\cos\frac{3\pi}{4}+\cos\frac{3\pi}{2}\right)&\left(\cos\frac{3\pi}{4}+\cos\frac{3\pi}{2}\right)&\left(\cos\frac{3\pi}{2}\right)\\\
\left(-\sin\frac{\pi}{4}-\sin\frac{3\pi}{4}-\sin\frac{3\pi}{2}\right)&\left(-\sin\frac{3\pi}{4}-\sin\frac{3\pi}{2}\right)&\left(-\sin\frac{3\pi}{2}\right)\\\
\end{bmatrix}$ (105) $\displaystyle=\begin{bmatrix}0&0&0\\\
0&-\frac{\sqrt{2}}{2}&0\\\ 1-\sqrt{2}&1-\frac{\sqrt{2}}{2}&1\\\
\end{bmatrix}.$ (106)

####  3.3.2 Velocity Kinematics in World Coordinate System

As introduced in Section 2.2.1, the linear velocity and angular velocity of
robots can be described with its twist velocity, which can be represented with
the homogeneous transformation matrix effectively. In this part, we will
discuss about the robot velocity within the world coordinate system, whereas
the velocity kinematics within the local coordinate system will be introduced
in the following subsection.

According to Equation 13 introduced in the previous Section 2.2.1, the twist
velocity vector of the robot end point can be represented as

$\widehat{\boldsymbol{\nu}}_{e}^{[\Sigma_{\textsc{w}}]}=\dot{\mathbf{T}}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}\left(\mathbf{T}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}\right)^{-1}.$
(107)

Meanwhile, as introduced in the previous Section 2.5, the homogeneous
transformation matrix can also be effectively represented as the screw axis
and rotation angle with the exponential operator. Viewed in such a
perspective, we can represent the matrix
${\mathbf{T}}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}$ as follows:

$\displaystyle{\mathbf{T}}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}$
$\displaystyle=\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\mathbf{T}^{[\Sigma_{b}\to\Sigma_{a}]}\mathbf{T}^{[\Sigma_{c}\to\Sigma_{b}]}$
(108)
$\displaystyle=\exp^{\widehat{\mathbf{s}}_{a}q_{a}}\exp^{\widehat{\mathbf{s}}_{b}q_{b}}\exp^{\widehat{\mathbf{s}}_{c}q_{c}}\mathbf{T}_{0},$
(109)

where the constant matrix $\mathbf{T}_{0}$ to represent the initial state of
the robot when $q_{a}=q_{b}=q_{c}=0$.

With the above exponential representation of matrix
${\mathbf{T}}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}$, we can represent its
derivative with respect to the time as

$\displaystyle\dot{\mathbf{T}}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}$
$\displaystyle=\left(\frac{{d}}{{d}t}\exp^{\widehat{\mathbf{s}}_{a}q_{a}}\right)\exp^{\widehat{\mathbf{s}}_{b}q_{b}}\exp^{\widehat{\mathbf{s}}_{c}q_{c}}\mathbf{T}_{0}+\exp^{\widehat{\mathbf{s}}_{a}q_{a}}\left(\frac{{d}}{{d}t}\exp^{\widehat{\mathbf{s}}_{b}q_{b}}\right)\exp^{\widehat{\mathbf{s}}_{c}q_{c}}\mathbf{T}_{0}$
(110)
$\displaystyle+\exp^{\widehat{\mathbf{s}}_{a}q_{a}}\exp^{\widehat{\mathbf{s}}_{b}q_{b}}\left(\frac{{d}}{{d}t}\exp^{\widehat{\mathbf{s}}_{c}q_{c}}\right)\mathbf{T}_{0}$
(111)
$\displaystyle=\Big{(}\exp^{\widehat{\mathbf{s}}_{a}q_{a}}\widehat{\mathbf{s}}_{a}\dot{q}_{a}\exp^{\widehat{\mathbf{s}}_{b}q_{b}}\exp^{\widehat{\mathbf{s}}_{c}q_{c}}+\exp^{\widehat{\mathbf{s}}_{a}q_{a}}\exp^{\widehat{\mathbf{s}}_{b}q_{b}}\widehat{\mathbf{s}}_{b}\dot{q}_{b}\exp^{\widehat{\mathbf{s}}_{c}q_{c}}$
(112)
$\displaystyle+\exp^{\widehat{\mathbf{s}}_{a}q_{a}}\exp^{\widehat{\mathbf{s}}_{b}q_{b}}\exp^{\widehat{\mathbf{s}}_{c}q_{c}}\widehat{\mathbf{s}}_{c}\dot{q}_{c}\Big{)}\mathbf{T}_{0}.$
(113)

On the other hand, we can also represent the inverse of matrix
$\left(\mathbf{T}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}\right)^{-1}$ as
follows:

$\displaystyle\left(\mathbf{T}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}\right)^{-1}$
$\displaystyle=\left(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\mathbf{T}^{[\Sigma_{b}\to\Sigma_{a}]}\mathbf{T}^{[\Sigma_{c}\to\Sigma_{b}]}\right)^{-1}$
(114)
$\displaystyle=\left(\mathbf{T}^{[\Sigma_{c}\to\Sigma_{b}]}\right)^{-1}\left(\mathbf{T}^{[\Sigma_{b}\to\Sigma_{a}]}\right)^{-1}\left(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\right)^{-1}$
(115)
$\displaystyle=\mathbf{T}_{0}^{-1}\exp^{-\widehat{\mathbf{s}}_{c}q_{c}}\exp^{-\widehat{\mathbf{s}}_{b}q_{b}}\exp^{-\widehat{\mathbf{s}}_{a}q_{a}}.$
(116)

Based on the above derivation, we can formally represent the twist velocity
vector with the above exponential representation as follows:

$\displaystyle\widehat{\boldsymbol{\nu}}_{e}^{[\Sigma_{\textsc{w}}]}$
$\displaystyle=\dot{\mathbf{T}}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}\left(\mathbf{T}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}\right)^{-1}$
(117)
$\displaystyle=\exp^{\widehat{\mathbf{s}}_{a}q_{a}}\widehat{\mathbf{s}}_{a}\dot{q}_{a}\exp^{-\widehat{\mathbf{s}}_{a}q_{a}}+\exp^{\widehat{\mathbf{s}}_{a}q_{a}}\exp^{\widehat{\mathbf{s}}_{b}q_{b}}\widehat{\mathbf{s}}_{b}\dot{q}_{b}\exp^{-\widehat{\mathbf{s}}_{b}q_{b}}\exp^{-\widehat{\mathbf{s}}_{a}q_{a}}$
(118)
$\displaystyle+\exp^{\widehat{\mathbf{s}}_{a}q_{a}}\exp^{\widehat{\mathbf{s}}_{b}q_{b}}\exp^{\widehat{\mathbf{s}}_{c}q_{c}}\widehat{\mathbf{s}}_{c}\dot{q}_{c}\exp^{-\widehat{\mathbf{s}}_{c}q_{c}}\exp^{-\widehat{\mathbf{s}}_{b}q_{b}}\exp^{-\widehat{\mathbf{s}}_{a}q_{a}}.$
(119)

From the previous Section 2.3.1 and Section 2.3.2, we already know that for
$\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{\textsc{w}}]}=\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\widehat{\boldsymbol{\nu}}_{a}^{[\Sigma_{a}]}(\mathbf{T}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]})^{-1}$,
we can get
$\boldsymbol{\nu}_{a}^{[\Sigma_{\textsc{w}}]}=Ad({{\mathbf{T}}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}})\boldsymbol{\nu}_{a}^{[\Sigma_{a}]}$.
Since the screw axis is also defined as a normalized twist velocity, with the
$\lor$ operator be applied to both sides of the above equation, we can
simplify the equation as

$\displaystyle{\boldsymbol{\nu}}_{e}^{[\Sigma_{\textsc{w}}]}$
$\displaystyle=\underbrace{Ad(\exp^{\widehat{\mathbf{s}}_{a}q_{a}}){\mathbf{s}}_{a}}_{\mathbf{j}_{a}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}}\dot{q}_{a}+\underbrace{Ad(\exp^{\widehat{\mathbf{s}}_{a}q_{a}}\exp^{\widehat{\mathbf{s}}_{b}q_{b}}){\mathbf{s}}_{b}}_{\mathbf{j}_{b}^{[\Sigma_{b}\to\Sigma_{\textsc{w}}]}}\dot{q}_{b}+\underbrace{Ad(\exp^{\widehat{\mathbf{s}}_{a}q_{a}}\exp^{\widehat{\mathbf{s}}_{b}q_{b}}\exp^{\widehat{\mathbf{s}}_{c}q_{c}}){\mathbf{s}}_{c}}_{\mathbf{j}_{c}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}}\dot{q}_{c}$
(120)
$\displaystyle=\mathbf{j}_{a}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]}\dot{q}_{a}+\mathbf{j}_{b}^{[\Sigma_{b}\to\Sigma_{\textsc{w}}]}\dot{q}_{b}+\mathbf{j}_{c}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}\dot{q}_{c}$
(121)
$\displaystyle=\underbrace{\left[\mathbf{j}_{a}^{[\Sigma_{a}\to\Sigma_{\textsc{w}}]},\mathbf{j}_{b}^{[\Sigma_{b}\to\Sigma_{\textsc{w}}]},\mathbf{j}_{c}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}\right]}_{\mathbf{J}^{[\Sigma_{\textsc{w}}]}}\begin{bmatrix}\dot{q}_{a}\\\
\dot{q}_{b}\\\ \dot{q}_{c}\\\ \end{bmatrix}$ (122)
$\displaystyle=\mathbf{J}^{[\Sigma_{\textsc{w}}]}\dot{\mathbf{q}}.$ (123)

###### Definition 11

(Space Jacobian): Formally, the matrix

$\mathbf{J}^{[\Sigma_{\textsc{w}}]}=\begin{bmatrix}Ad(\exp^{\widehat{\mathbf{s}}_{a}q_{a}})&Ad(\exp^{\widehat{\mathbf{s}}_{a}q_{a}}\exp^{\widehat{\mathbf{s}}_{b}q_{b}}){\mathbf{s}}_{b}&Ad(\exp^{\widehat{\mathbf{s}}_{a}q_{a}}\exp^{\widehat{\mathbf{s}}_{b}q_{b}}\exp^{\widehat{\mathbf{s}}_{c}q_{c}}){\mathbf{s}}_{c}\end{bmatrix}$
(124)

we introduce above is also named as the space Jacobian matrix in the world
coordinate system. The column vectors in matrix
$\mathbf{J}^{[\Sigma_{\textsc{w}}]}$ actually denote the screw axes of
different joints of the robot body in the world coordinate, respectively.

####  3.3.3 Velocity Kinematics in Local Coordinate System

Meanwhile, according to Equation 24 introduced in the previous Section 2.2.2,
we can also obtain the twist velocity vector representation of the end point
within a local coordinate system $\Sigma_{c}$, which can be denoted as
follows:

$\displaystyle\widehat{\boldsymbol{\nu}}_{e}^{[\Sigma_{c}]}$
$\displaystyle=\left(\mathbf{T}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}\right)^{-1}\dot{\mathbf{T}}^{[\Sigma_{c}\to\Sigma_{\textsc{w}}]}$
(125)
$\displaystyle=\left(\exp^{-\widehat{\mathbf{s}}_{c}q_{c}}\exp^{-\widehat{\mathbf{s}}_{b}q_{b}}\widehat{\mathbf{s}}_{a}\exp^{\widehat{\mathbf{s}}_{b}q_{b}}\exp^{\widehat{\mathbf{s}}_{c}q_{c}}\right)\dot{q}_{a}+\left(\exp^{-\widehat{\mathbf{s}}_{c}q_{c}}\widehat{\mathbf{s}}_{b}\exp^{\widehat{\mathbf{s}}_{c}q_{c}}\right)\dot{q}_{b}+\left(\widehat{\mathbf{s}}_{c}\right)\dot{q}_{c}$
(126)
$\displaystyle=\underbrace{Ad\left(\exp^{-\widehat{\mathbf{s}}_{c}q_{c}}\exp^{-\widehat{\mathbf{s}}_{b}q_{b}}\right)\widehat{\mathbf{s}}_{a}}_{\mathbf{j}_{a}^{[\Sigma_{a}\to\Sigma_{c}]}}\dot{q}_{a}+\underbrace{Ad\left(\exp^{-\widehat{\mathbf{s}}_{c}q_{c}}\right)\widehat{\mathbf{s}}_{b}}_{\mathbf{j}_{b}^{[\Sigma_{b}\to\Sigma_{c}]}}\dot{q}_{b}+\underbrace{\left(\widehat{\mathbf{s}}_{c}\right)}_{\mathbf{j}_{c}^{[\Sigma_{c}]}}\dot{q}_{c}$
(127)
$\displaystyle=\underbrace{\left[\mathbf{j}_{a}^{[\Sigma_{a}\to\Sigma_{c}]},\mathbf{j}_{b}^{[\Sigma_{b}\to\Sigma_{c}]},\mathbf{j}_{c}^{[\Sigma_{c}]}\right]}_{\mathbf{J}^{[\Sigma_{c}]}}\begin{bmatrix}\dot{q}_{a}\\\
\dot{q}_{b}\\\ \dot{q}_{c}\\\ \end{bmatrix}$ (128)
$\displaystyle=\mathbf{J}^{[\Sigma_{c}]}\dot{\mathbf{q}}.$ (129)

###### Definition 12

(Body Jacobian): Formally, the matrix

$\mathbf{J}^{[\Sigma_{c}]}=\begin{bmatrix}Ad\left(\exp^{-\widehat{\mathbf{s}}_{c}q_{c}}\exp^{-\widehat{\mathbf{s}}_{b}q_{b}}\right)&Ad\left(\exp^{-\widehat{\mathbf{s}}_{c}q_{c}}\right)&\left(\widehat{\mathbf{s}}_{c}\right)\end{bmatrix}$
(130)

we introduce above is also named as the body Jacobian matrix in the local
coordinate system $\Sigma_{c}$ that the end point lies in. The column vectors
in matrix $\mathbf{J}^{[\Sigma_{c}]}$ actually denote the screw axes of
different joints of the robot body in the local coordinate $\Sigma_{c}$,
respectively.

###  3.4 Torque in Forward Kinematics

The Jacobian matrix also plays an important role in robot static analysis.
Formally, at the end of the robot arm, we can represent the generated force
vector as $\mathbf{f}_{e}$ and the joint torque vector as
$\boldsymbol{\tau}_{e}$.

####  3.4.1 Static Analysis

According to the physics, in the rotational system, the system power equals to
the product of torque vector $\boldsymbol{\tau}_{e}$ and the angular velocity
vector $\boldsymbol{\omega}_{e}$ of the end point, i.e.,

$P=\boldsymbol{\tau}_{e}^{\top}\boldsymbol{\omega}_{e}.$ (131)

Meanwhile, the power of the end point can also be represented with its end
point force vector $\mathbf{f}_{e}$ and its linear velocity vector
$\mathbf{v}_{e}$, i.e.,

$P=\mathbf{f}_{e}^{\top}\mathbf{v}_{e}.$ (132)

These above two equations will derive that

$\mathbf{f}_{e}^{\top}\mathbf{v}_{e}=\boldsymbol{\tau}_{e}^{\top}\boldsymbol{\omega}_{e}.$
(133)

####  3.4.2 Force-Torque Relationship

Meanwhile, according to the Equation 90 we present before when defining the
Jacobian Matrix before, we know that the linear velocity vector
$\mathbf{v}_{e}=\mathbf{J}\boldsymbol{\omega}_{e}$, so we can have the
following equation,

$\mathbf{f}_{e}^{\top}\mathbf{J}\boldsymbol{\omega}_{e}=\boldsymbol{\tau}_{e}^{\top}\boldsymbol{\omega}_{e}.$
(134)

The above equation holds for any angular velocity vector
$\boldsymbol{\omega}_{e}$ if

$\boldsymbol{\tau}_{e}=\left(\mathbf{f}_{e}^{\top}\mathbf{J}\right)^{\top}=\mathbf{J}^{\top}\mathbf{f}_{e}.$
(135)

The above equation calculates the required torque $\boldsymbol{\tau}_{e}$
needed to generate the desired force $\mathbf{f}_{e}$ at the end point.
Meanwhile, when the Jacobian Matrix is not singular and its transpose is
invertible, we can also obtain

$\mathbf{f}_{e}=(\mathbf{J}^{\top})^{-1}\boldsymbol{\tau}_{e}=\mathbf{J}^{-\top}\boldsymbol{\tau}_{e}.$
(136)

Such an equation indicates that, under the static equilibrium state, the force
that can be generated at the end point given the torques of the robot arm.
This equation is very important, and will be used in many scenarios. For
instance, to pick up a box of certain weight $w$, we need to decide the needed
torques of each joint motors so the robot can hold the box. More discussions
about the robot torque and generated forces will be provided in the following
sections when talking about the robot dynamics.

##  4 Inverse Kinematics

In this section, we will introduce another important topic about robotic for
readers, the inverse kinematics. Just from the name, we can know that it is a
reversed process of forward kinematics introduced in the previous section. In
this section, we will introduce two different methods to address the inverse
kinematics problem for readers, and also provide detailed analysis of the
Jacobian singularity.

###  4.1 What is Inverse Kinematics?

We can still take the Equation 74 we use at the beginning of the previous
Section 3 on forward kinematics to explain “what is inverse kinematics” for
readers.

  * •

Forward Kinematics: Given the parameter inputs $\mathbf{q}(t)$, the process of
calculating the desired output $\mathbf{o}(t)$ of the robot body is called
forward kinematics, i.e.,

$\mathbf{o}(t)=g\left(\mathbf{q}(t)\right).$ (137)

  * •

Inverse Kinematics: Given the desired output $\mathbf{O}(t)$, the problem to
find the parameter solution $\mathbf{q}(t)$ that can satisfy

$\arg_{\mathbf{q}(t)}g\left(\mathbf{q}(t)\right)=\mathbf{O}(t),\forall
t\in\\{1,2,\cdots\\},$ (138)

is named as the inverse kinematics problem.

Compared with forward kinematics that give clear “instructions” (denoted by
parameters $\mathbf{q}(t)$) to robots that will perform accordingly to
illustrate the outputs, inverse kinematics is more frequently used in real-
world robot control, e.g., standing, walking, picking up items. In this part,
we will try to introduce how to find the parameter vector $\mathbf{q}(t)$ that
will generate the output satisfying the constraint
$g\left(\mathbf{q}(t)\right)=\mathbf{O}(t)$.

###  4.2 Analytic Solution

Before we introduce the numerical solution to the inverse kinematics problem,
we would like to use a toy example to illustrate how to solve the problem
analytically.

###### Example 8

Figure 5: An Example of 2-Link Robot Ram.

As shown in Figure 5, we illustrate a robot arm in the 3D coordinate system
with two links of length $l_{1}$ and $l_{2}$, respectively. For the first arm
link, it forms an angle of $\theta$ degrees with respect to the $y$ axis.
Meanwhile, the second arm link further forms an angle of $\phi$ degrees with
the first arm link. We aim to configure the parameter values of the angles
$\theta$ and $\phi$ to make the end point reaches position denoted by
coordinate pair $(x_{e},y_{e},0)$ in the space.

Analytic Solution: Based on the robot arm example shown in Figure 5, we will
show below about how to use the analytic solution to calculate the desired
angles $\theta$ and $\phi$.

First of all, for the triangle $\triangle oae$, according to the law of
cosine, we know that

$\displaystyle oe^{2}=oa^{2}+ae^{2}-2(oa)(ae)\cos\beta$ (139)
$\displaystyle\implies$ $\displaystyle
y_{e}^{2}+z_{e}^{2}=l_{1}^{2}+l_{2}^{2}-2l_{1}l_{2}\cos\beta$ (140)
$\displaystyle\implies$
$\displaystyle\beta=\cos^{-1}\left(\frac{l_{1}^{2}+l_{2}^{2}-y_{e}^{2}-z_{e}^{2}}{2l_{1}l_{2}}\right).$
(141)

Following the same law, we can also obtain that

$\displaystyle ae^{2}=oa^{2}+oe^{2}-2(oa)(oe)\cos\alpha$ (142)
$\displaystyle\implies$ $\displaystyle
l_{2}^{2}=l_{1}^{2}+y_{e}^{2}+z_{e}^{2}-2l_{1}\sqrt{y_{e}^{2}+z_{e}^{2}}\cos\alpha$
(143) $\displaystyle\implies$
$\displaystyle\alpha=\cos^{-1}\left(\frac{l_{1}^{2}+y_{e}^{2}+z_{e}^{2}-l_{2}^{2}}{2l_{1}\sqrt{y_{e}^{2}+z_{e}^{2}}}\right).$
(144)

Meanwhile, according to the coordinate of end point $e$, i.e.,
$(0,y_{e},z_{e})$, we can get that

$\displaystyle\tan 2(\alpha+\theta)=(z_{e},y_{e})$ (145)
$\displaystyle\implies$ $\displaystyle(\alpha+\theta)=atan2(z_{e},y_{e}).$
(146)

Based on the above analysis, we can easily obtain the representations of
angles $\theta$ and $\alpha$ as follows:

$\displaystyle\theta$
$\displaystyle=(\alpha+\theta)-\alpha=atan2(z_{e},y_{e})-\cos^{-1}\left(\frac{l_{1}^{2}+y_{e}^{2}+z_{e}^{2}-l_{2}^{2}}{2l_{1}\sqrt{y_{e}^{2}+z_{e}^{2}}}\right),$
(147) $\displaystyle\phi$
$\displaystyle=\pi-\beta=\pi-\cos^{-1}\left(\frac{l_{1}^{2}+l_{2}^{2}-y_{e}^{2}-z_{e}^{2}}{2l_{1}l_{2}}\right).$
(148)

According to the above analysis, via the analytic method shown in the example,
we can obtain the closed-form solution of the inverse kinematics problem. The
readers may have also notice that the analytic method calculation process is
already very cumbersome for the 2-link robot arm. When it comes to more
complicated robot structures, like a 6-link robot arm, it will become much
more challenging to solve the problem analytically.

###  4.3 Inverse Position and Orientation Kinematics

Compared with the inverse kinematics, we have noticed that the forward
kinematics calculation process we introduce in the previous section is much
simpler and easier. Readers may also wonder if we can use forward kinematics
introduced before to help identify the solution to the inverse kinematics
problem. This is also the problem we plan to study in this part, and we will
introduce a numerical solution that doesn’t need to analyze the

####  4.3.1 Numerical Solution Method

Before we introduce the detailed information about the numerical solution, we
would like to first provide the general framework of the numerical solution
algorithm.

  * •

Step 1: Define the position vector $\mathbf{p}_{start}$ and rotation matrix
$\mathbf{R}_{start}$ of the robot’s start position and orientation.

  * •

Step 2: Define the position vector $\mathbf{p}_{target}$ and rotation matrix
$\mathbf{R}_{target}$ of the robot’s target position and orientation.

  * •

Step 3: Initialize a variable vector $\mathbf{q}$ that denotes the angles of
the robot’s link angles.

  * •

Step 4: Given the variable vector $\mathbf{q}$ and the current robot position
and orientation, calculate the robot position vector $\mathbf{p}$ and
orientation matrix $\mathbf{R}$ outputs with forward kinematics.

  * •

Step 5: Calculate the introduced error (or loss) term of the forward
kinematics outputs $\mathbf{p}$, $\mathbf{R}$ compared with the target outputs
$\mathbf{p}_{target}$, $\mathbf{R}_{target}$, i.e.,
$\delta\mathbf{p}=\mathbf{p}-\mathbf{p}_{target}$ and
$\delta\mathbf{R}=\mathbf{R}-\mathbf{R}_{target}$.

  * •

Step 6: If $\delta\mathbf{p}$ and $\delta\mathbf{R}$ are sufficiently small
(less than the pre-defined threshold), stop the calculation and return the
current variable vector $\mathbf{q}$ as the output.

  * •

Step 7: Otherwise, calculate the term $\delta\mathbf{q}$ with the introduced
error terms $\delta\mathbf{p}$ and $\delta\mathbf{R}$, and update the current
variable vector $\mathbf{q}=\mathbf{q}+\delta\mathbf{q}$ and go to Step 4.

Figure 6: Outline of the Numerical Solution in Updating Variable Vector via
Forward Kinematics.

An outline of the above algorithm is also illustrated in Figure 6. Such an
iterative adjustment process continues until the stop criteria is met.
Meanwhile, the above algorithm also create several new open questions:

  1. 1.

Error Function: How to define “whether $\delta\mathbf{p}$ and
$\delta\mathbf{R}$ are sufficiently small”?

  2. 2.

Adjustment Term: How to calculate the “$\delta\mathbf{q}$” term to be used for
updating $\mathbf{q}$?

To address the first problem, a loss function is introduced to quantify
“whether $\delta\mathbf{p}$ and $\delta\mathbf{R}$ are sufficiently small” or
not. Formally, given $\delta\mathbf{p}=\mathbf{p}-\mathbf{p}_{target}$ and
$\delta\mathbf{R}=\mathbf{R}-\mathbf{R}_{target}$, we can represent the
introduced loss term to be

$\ell(\delta\mathbf{p},\delta\mathbf{R})=\left\|\delta\mathbf{p}\right\|^{2}+\left\|\left(\ln\delta\mathbf{R}\right)^{\lor}\right\|^{2}$
(149)

According to the previous tutorial article, the “$\ln$” and “$\lor$” operators
will project the rotation matrix to the corresponding angular velocity vector,
i.e., $\delta(\boldsymbol{\omega}t)=(\ln\delta\mathbf{R})^{\lor}$. The term
$\boldsymbol{\omega}$ denotes the angular velocity vector of the interested
point of the robot body. For the loss term
$\ell(\delta\mathbf{p},\delta\mathbf{R})\leq\gamma$ (where $\gamma=1e^{-6}$ is
a pre-defined threshold parameter), then the updating process will stop and we
can also generally guarantee that the robot position vector and orientation
matrix approaches to the targets.

As to the second problem, we will introduce its solution in the following
subsection for readers with the Newton-Raphson Method.

####  4.3.2 Newton-Raphson Method

Let’s assume we change the parameter vector $\mathbf{q}$ with a very minor
term $\delta\mathbf{q}$, where $\delta\mathbf{q}\to\mathbf{0}$. Its impact on
the position vector $\mathbf{q}$ and orientation matrix $\mathbf{R}$ can be
represented as

$\displaystyle\delta\mathbf{p}$
$\displaystyle=g_{\mathbf{p}}(\mathbf{q}+\delta\mathbf{q})-g_{p}(\mathbf{q}),$
(150) $\displaystyle\delta(\boldsymbol{\omega}t)$
$\displaystyle=g_{\boldsymbol{\omega}}(\mathbf{q}+\delta\mathbf{q})-g_{\boldsymbol{\omega}}(\mathbf{q}).$
(151)

In the above equation, we already replace the term $\delta\mathbf{R}$ with the
$\delta\boldsymbol{\omega}$ just to simplify the representations below.

As introduced before in Equation 90 on velocity kinematics, if the term
$\delta\mathbf{q}$ is small enough and approaches to $\mathbf{0}$, we can
actually combine the above two equations and rewrite them as follows:

$\begin{bmatrix}\delta{\mathbf{p}}\\\ \delta({\boldsymbol{\omega}}t)\\\
\end{bmatrix}=\begin{bmatrix}\mathbf{v}\\\ \boldsymbol{\omega}\\\
\end{bmatrix}=\mathbf{J}\delta{\mathbf{q}},$ (152)

where $\mathbf{J}\in\mathbbm{R}^{6\times n}$ is the Jacobian matrix we
introduce before and vectors $\mathbf{v}=\dot{\mathbf{p}}$ and
$\boldsymbol{\omega}$ denote the linear and angular velocity, respectively.
The notation $n$ denotes the number of links in the robot, which is equal to
the dimension of the variable vector $\mathbf{q}$. The combined vector
$\boldsymbol{\nu}=\begin{bmatrix}\mathbf{v}\\\ \boldsymbol{\omega}\\\
\end{bmatrix}$ is also the twist velocity vector we define before.

If matrix $\mathbf{J}$ is invertible, we can represent the term
$\delta\mathbf{q}$ as follows:

$\delta{\mathbf{q}}=\mathbf{J}^{-1}\begin{bmatrix}\mathbf{v}\\\
\boldsymbol{\omega}\\\ \end{bmatrix}=\mathbf{J}^{-1}\boldsymbol{\nu}.$ (153)

The above equation answer the second problem on “how to calculate the
‘$\delta\mathbf{q}$’ term to be used for updating $\mathbf{q}$”. In
application, instead of directly applying the calculated term
$\delta{\mathbf{q}}$ to update the variable vector $\mathbf{q}$, we usually
also includes a parameter $\lambda\in(0,1)$ to control the variable updating
speed, i.e.,

$\mathbf{q}=\mathbf{q}+\lambda\cdot\delta{\mathbf{q}}.$ (154)

The introduced parameter $\lambda$ can stabilize the updating process and
avoid drastic changes to the variables, and this method is formally named as
the Newton-Raphson method.

If the readers are familiar with mathematical optimization or gradient descent
algorithms, the Newton-Raphson method has been widely applied in parameter
updating to optimize the model performance. Meanwhile, in the above
calculation process, we have a strong assumption that “the Jacobian matrix
$\mathbf{J}$ is invertible”. At the end of this section, we will have a
detailed analysis about the Jacobian matrix and discuss about how to update
the variables when $\mathbf{J}$ is singular.

###  4.4 Inverse Velocity and Torque Kinematics

In the previous Section 3.3 and Section 3.4, we discuss about the robot
forward velocity kinematics and forward torque kinematics, which calculates
the robot part twist velocity vector and generated force vector based on the
inputs. In this part, we will talk about the reversed process, i.e., the
inverse velocity kinematics and inverse torque kinematics.

####  4.4.1 Inverse Velocity Kinematics

From the name, we can know that inverse velocity kinematics literally denotes
the process to figure out the desired joint angular velocity to achieve the
desired twist velocity vector. From Equation 90 we provide in the previous
section on defining the Jacobian matrix, we already know the relationship
between twist velocity vector and the joint angular velocity. Furthermore,
from Equation 152 in the previous subsection, we also know that if the
Jacobian matrix $\mathbf{J}$ is invertible, the joint angular velocity can be
calculated from its twist velocity as follows:

$\dot{\mathbf{q}}=\mathbf{J}^{-1}\boldsymbol{\nu}.$ (155)

Depending on the coordinate chosen to study the inverse velocity kinematics,
the twist velocity vector $\boldsymbol{\nu}$ can be defined as either
$\mathbf{T}^{-1}\dot{\mathbf{T}}$ (the twist velocity in the local coordinate
system) or $\dot{\mathbf{T}}\mathbf{T}^{-1}$ (the twist velocity within the
world coordinate system) as we introduce before. In the case if the Jacobian
matrix $\mathbf{J}$ is not invertible, we will introduce the solution in the
following part at the end of this section for readers.

####  4.4.2 Inverse Torque Kinematics

In the previous Section 3.4, we have also discussed about the relationship
between the robot torque and the force. In Equation 135 introduced before, via
the Jacobian Matrix, we have already calculate the required joint torque to
generate the desired force for the end point, i.e.,

$\boldsymbol{\tau}_{e}=\mathbf{J}^{\top}\mathbf{f}_{e}.$ (156)

The above equation is derived based on pure static analysis, and no kinetic
energy or potential energy are considered actually. In the following section
on robot dynamics, we will further discuss about the robot velocity, torque
and force for readers.

###  4.5 Singular Jacobian Matrix

In this part, we will talk about an important problem about the Jacobian
matrix. According to the previous discussion, the Jacobian matrix plays an
important role in calculating (1) twist velocity vector and force vector of
the end point in forward kinematics, (2) needed joint angle vector and torque
vector to generate the twist velocity and force in inverse kinematics. We make
lots of assumption on the Jacobian matrix before in the derivation, e.g., it
is full rank and invertible. In this part, we will discuss about some
potential problems with Jacobian matrix, i.e., the singularity problem.

####  4.5.1 Singularity and Gimbal Lock

We can first use the classic “gimbal lock” problem to describe “what is
singularity”. The “gimbal lock” term becomes famous in the movie Apollo 13,
which happens when the rotational axis of the middle term in the sequence
becomes parallel to the rotational axis of the first or the third term.

Figure 7: An Illustration of the IMU (Inertial Measure Unit) used in Apollo
Lunar Module.

###### Example 9

As illustrated in Figure 7, we show the IMU (inertial measurement unit) used
in the Apollo Lunar Module and its schematic representation, which is a
mechanical gyroscope used for spacecraft navigation. This IMU has three
orthogonal gyroscopes that hold it at a constant orientation with respect to
the universe. According to the orientations of these three axes and the right-
hand rule, we can mark them as the $x$, $y$ and $z$ axis, respectively.

Figure 8: An Illustration of the IMU Gimbal Lock Problem.

As the spacecraft pose changes in the universe, the stable member of the IMU
will remain relatively stable in terms of the orientation, but the outer,
middle and inner gimbal will rotate around their axes with certain angles. As
shown in Figure 8, we can represent the rotation angles around these three
axes as $\theta_{x}$, $\theta_{y}$ and $\theta_{z}$, respectively. From these
rotation angles, we will be able to infer the orientation of the spacecraft.

Meanwhile, when the middle gimbal rotates around the $y$ axis with an angle
$\theta_{y}=\pi/2$, we observe that the axis of the inner and outer gimbal
(i.e., the $x$ axis and $z$ axis) will be aligned and share the same rotation
axis. For such a case, the IMU will have only two degrees of freedom and one
degree of freedom will be lost.

Such a degree of freedom lose in the above example can also be illustrated via
the mathematical representations of the rotation matrix about the IMU. For the
rotation around the $x$, $y$ and $z$ axis with angles of $\theta_{x}$,
$\theta_{y}$ and $\theta_{z}$ degrees, we can represent the corresponding
rotation matrix of the IMU to be

$\mathbf{R}=\mathbf{R}_{x}(\theta_{x})\mathbf{R}_{y}(\theta_{y})\mathbf{R}_{z}(\theta_{z}).$
(157)

Such rotation matrix follows the cyclic rotation rules, i.e.,

$\displaystyle\mathbf{R}_{x}(\pi/2)\mathbf{R}_{y}(\theta)\mathbf{R}_{x}(\pi/2)^{\top}=\mathbf{R}_{z}(\theta),$
(158)
$\displaystyle\mathbf{R}_{y}(\pi/2)\mathbf{R}_{z}(\theta)\mathbf{R}_{y}(\pi/2)^{\top}=\mathbf{R}_{x}(\theta),$
(159)
$\displaystyle\mathbf{R}_{z}(\pi/2)\mathbf{R}_{x}(\theta)\mathbf{R}_{z}(\pi/2)^{\top}=\mathbf{R}_{y}(\theta);$
(160)

as well as the anti-cyclic rotation rules, i.e.,

$\displaystyle\mathbf{R}_{y}(\pi/2)^{\top}\mathbf{R}_{x}(\theta)\mathbf{R}_{y}(\pi/2)=\mathbf{R}_{z}(\theta),$
(161)
$\displaystyle\mathbf{R}_{z}(\pi/2)^{\top}\mathbf{R}_{y}(\theta)\mathbf{R}_{z}(\pi/2)=\mathbf{R}_{x}(\theta),$
(162)
$\displaystyle\mathbf{R}_{x}(\pi/2)^{\top}\mathbf{R}_{z}(\theta)\mathbf{R}_{x}(\pi/2)=\mathbf{R}_{y}(\theta).$
(163)

We will not provide the proofs of the above rules, and readers can easily
prove them with the concrete representations of rotation matrices
$\mathbf{R}_{x}$, $\mathbf{R}_{y}$ and $\mathbf{R}_{z}$ to calculate their
product result representations by yourselves.

According to the above rules as well as the orthogonality of rotation matrix,
when the middle gimbal rotation angle $\theta_{y}=\pi/2$, we can rewrite the
rotation matrix of the IMU as follows:

$\displaystyle\mathbf{R}$
$\displaystyle=\mathbf{R}_{x}(\theta_{x})\mathbf{R}_{y}(\pi/2)\mathbf{R}_{z}(\theta_{z})$
(164)
$\displaystyle=\mathbf{R}_{x}(\theta_{x})\mathbf{R}_{y}(\pi/2)\mathbf{R}_{z}(\theta_{z})\left(\mathbf{R}_{y}(\pi/2)^{\top}\mathbf{R}_{y}(\pi/2)\right)$
(165)
$\displaystyle=\mathbf{R}_{x}(\theta_{x})\left(\mathbf{R}_{y}(\pi/2)\mathbf{R}_{z}(\theta_{z})\mathbf{R}_{y}(\pi/2)^{\top}\right)\mathbf{R}_{y}(\pi/2)$
(166)
$\displaystyle=\mathbf{R}_{x}(\theta_{x})\mathbf{R}_{x}(\theta_{z})\mathbf{R}_{y}(\pi/2)$
(167)
$\displaystyle=\mathbf{R}_{x}(\theta_{x}+\theta_{z})\mathbf{R}_{y}(\pi/2).$
(168)

The above representation of the rotation matrix also indicates that the IMU
will not be able to represent the rotation in the $z$ axis anymore and render
the IMU fail to work. Such a problem happens only at certain special
circumstances, e.g., $\theta_{y}=\pi/2$ as show above.

####  4.5.2 Jacobian Singularity

A robot singularity is a physical blockage, not some kind of abstract
mathematical problem.

###### Definition 13

(Robot Singularity): At a singularity, a robot loses one or more degrees of
freedom.

In the previous subsection, we have illustrate the singularity of a simple
“robot”, i.e., the IMU with three degrees of freedom, which will lose one
degree of freedom when the middle gimbal rotates with an angle of
$\theta_{y}=\pi/2$ degrees.

Besides the simple IMU, singularity widely exists in many robots, like a robot
arm or a legged robot. Readers may also wonder what are the causes of the
robot singularity. In this part, we will illustrate that robot singularity is
closely related to the singularity of the corresponding Jacobian matrix.

###### Example 10

To illustrate the relationship of robot singularity with the Jacobian matrix,
we provide an example of robot arm in Figure 9, whose links are connected via
the $6$ joints, and the DH parameters are illustrated in the figure as well.

A Note: We haven’t introduced the robot DH parameters for readers yet. For
now, you just need to know robot DH parameters can describe the pose and
structure of rigid-body robots. Once the DH parameters are specified, then we
can build the mathematical model to recover the current pose and the full
structure of the robot. More detailed information about DH parameter will be
described in detail in the follow-up tutorial articles instead.

In the DH parameter representation, the local coordinates are attached to the
links on the far-away side of the joints with the $z$ axis collinear with the
joint rotation axis. For some of the axes not clearly indicated, readers can
infer their orientation with the right-hand rule, i.e., thumb: $x$ axis, index
finger: $y$ axis, and middle finger: $z$ axis.

Figure 9: An Example of $6$-DOF Robot Arm and Its DH Parameter Table. (DOF:
Degree of Freedom).

According to the DH table, we can define its corresponding Jacobian matrix as
follows:

$\mathbf{J}=\begin{bmatrix}X&X&X&X&X&X\\\ X&X&X&X&X&X\\\ 0&X&X&X&X&X\\\
0&s_{1}&-c_{1}s_{2}&-c_{3}s_{1}-c_{123}&c_{1}s_{24}-c_{4}s_{13}+c_{1234}&X\\\
0&-c_{1}&-s_{12}&c_{13}-c_{2}s_{13}&s_{124}+c_{14}s_{3}+c_{234}s_{1}&X\\\
1&0&c_{2}&-s_{23}&c_{34}s_{2}-c_{2}s_{4}&c_{24}s_{5}-c_{5}s_{23}+c_{3}s_{245}\\\
\end{bmatrix},$ (169)

where the simplified notations of terms $c_{i}$ and $s_{i}$ denote $\cos
q_{i}$ and $\sin q_{i}$. Meanwhile, terms $s_{ijk\cdots}$ and $c_{ijk\cdots}$
represent $\sin(q_{i}+q_{j}+q_{k}+\cdots)$ and
$\cos(q_{i}+q_{j}+q_{k}+\cdots)$ instead.

Via symbolic computation, we can calculate the determinant of the Jacobian
matrix as

$\det(\mathbf{J})=-\cos(q_{5})\sin(q_{2})\Gamma(q_{4})\text{, where
}\Gamma(q_{4})=(d_{3}^{2}d_{5})/2+d_{3}^{2}d_{5}\cos(2q_{4})/2.$ (170)

In the derivation of inverse kinematics we introduce before, the inverse of
the Jacobian matrix play a critical role. We assumed the Jacobian matrix is
invertible, whose inverse will help calculate the desired joint angles,
angular velocity and torque vector parameters. Meanwhile, the Jacobian matrix
is invertible when Jacobian is not singular, and a matrix is singular iff its
determinant is $0$. Therefore, we can derive the singularity condition to be

$\det(\mathbf{J})=0\Longleftrightarrow\begin{cases}&\text{Condition
1}:\cos(q_{5})=0\\\ &\text{Condition 2}:\sin(q_{2})=0\\\ &\text{Condition
3}:\Gamma(q_{4})=0\\\
\end{cases}\Longleftrightarrow\begin{cases}&q_{5}=\pm\pi/2\\\ &q_{2}=0\\\
&q_{4}=-\pi/2\\\ \end{cases},$ (171)

where assume the joint angles are all within the range $[-\pi/2,\pi/2]$ and
some cases like $q_{2}=\pi$ have been pruned from the results.

Based on the above solutions of the singularity conditions, as shown in Figure
11, we can categorize the robot singularity into two main types:

  * •

Boundary Singularity: The condition that $q_{4}=-\pi/2$ will cause the robot
arm manipulator in a straight pose, as shown in Figure 11, where the end point
of the arm will actually reach the boundary of its workspace. Therefore, we
also call such singularity as the boundary singularity or workspace
singularity. It is usually caused by a full extension of a joint, and asking
the manipulator to move beyond where it can be positioned. An illustration of
robot arm boundary singularity is also shown in Figure 10.

Figure 10: An Illustration of Robot Arm Boundary Singularity and Singular
Region.

  * •

Internal Singularity: Except the condition $q_{4}=-\pi/2$, the other
conditions do not yield a straight posture, and we name them as the internal
singularity. The internal singularity (also known as joint space singularity)
are generally caused by an alignment of the robots axes in space. The
conditions derived above can introduce three different internal singularity as
follows:

    * –

Case 1: $q_{2}=0$, $q_{4}\neq-\pi/2$ and $q_{5}\neq\pm\pi/2$. When $q_{2}=0$,
the axes of joints $q_{1}$ and $q_{3}$ will be collinear so the rotation of
these two joints correspond to the directions of the coordinate system is
coupled in the $y_{1}$-direction and the $z_{1}$-direction as illustrated in
Figure 10.

    * –

Case 2: $q_{2}\neq 0$, $q_{4}\neq-\pi/2$ and $q_{5}=\pm\pi/2$. The axes joints
$q_{1}$ and $q_{6}$ (blue color), as well as those of joints $q_{3}$ and
$1_{6}$ (red color) will be collinear.

    * –

Case 3: $q_{2}=0$, $q_{4}\neq-\pi/2$ and $q_{5}=\pm\pi/2$. The collinear joint
axes will include $q_{1}$, $q_{3}$, and $q_{6}$, or $q_{1}$ and $q_{3}$.

Figure 11: An Illustration of Robot Arm Boundary Singularity and Internal
Singularity.

A Remark: The above derivation about the singularity conditions are obtained
based on the robot arm shown in Figure 9. For other robot arms with different
structures, their DH parameter table will be different, and we will also
obtain very different derivation results for the singularity conditions as
well.

####  4.5.3 Robot Singular Pose in Application

Figure 12: Velocity Inverse Kinematics of Legged Robot [2].

Besides the robot arm, singularity also exist in other types of robots in
real-world applications, like the legged robots.

###### Example 11

As shown in Figure 12, we show a biped legged robot, where each leg has 6 DOF
and the trunk is just simply represented as a cuboid. In Plot (a), the robot
right leg is slightly raised and the angles of each joint are
$(0,0,-\pi/6,\pi/3,-\pi/6,0)$, respectively. We aim to calculate the desired
angular velocity of the robot leg joints so that the robot right foot (i.e.,
the end point) can lift vertically with a linear velocity of $0.1m/s$.

According to our previous discussion, we know that the robot joint angular
velocity can be represented as

$\dot{\mathbf{q}}=\mathbf{J}^{-1}\boldsymbol{\nu},$ (172)

where vector $\boldsymbol{\nu}=[0,0,0.1,0,0,0]^{\top}$ denotes the twist
velocity vector of the right foot end point and the Jacobian matrix can be
defined based on the current leg pose and its joint angles
$(0,0,-\pi/6,\pi/3,-\pi/6,0)$.

With the above equation, we can calculate the vector $\dot{\mathbf{q}}$ to be

$\dot{\mathbf{q}}=\mathbf{J}^{-1}\begin{bmatrix}0\\\ 0\\\ 0.1\\\ 0\\\ 0\\\
0\\\ \end{bmatrix}=\begin{bmatrix}0\\\ 0\\\ -0.3333\\\ 0.6667\\\ -0.3333\\\
0\\\ \end{bmatrix}.$ (173)

In other words, the angular velocity of the hip, knee and ankle joint should
be $-0.3333$ rad/s, $0.6667$ rad/s and $-0.3333$ rad/s, respectively.

Meanwhile, when it comes to the robot in the Plot (b) of Figure 12, we observe
that its right leg is fully stretched and the angles of the current joints are
all zeros. If we calculate the inverse of the corresponding Jacobian matrix
and the angular velocity vector $\dot{\mathbf{q}}$ with the above equation, we
can get

$\mathbf{J}^{-1}=\begin{bmatrix}\inf&\inf&\inf&\inf&\inf&\inf\\\
\inf&\inf&\inf&\inf&\inf&\inf\\\ \inf&\inf&\inf&\inf&\inf&\inf\\\
\inf&\inf&\inf&\inf&\inf&\inf\\\ \inf&\inf&\inf&\inf&\inf&\inf\\\
\inf&\inf&\inf&\inf&\inf&\inf\\\ \end{bmatrix}\text{, and
}\dot{\mathbf{q}}=\begin{bmatrix}\text{NaN}\\\ \text{NaN}\\\ \text{NaN}\\\
-\inf\\\ \inf\\\ 0\\\ \end{bmatrix}.$ (174)

From the result, we observe that the robot right leg is at its singular pose.
The corresponding Jacobian at the current pose is singular and its inverse
doesn’t exist. If we still use it to calculate the angular velocity vector
$\dot{\mathbf{q}}$ to achieve a vertical linear velocity of $0.1$m/s, its
result will consists of either NaN (Not a Number) of $\inf$ (Infinity). It is
also the reason why the legged robots will always have their legs bended
slightly to avoid be stretched too much and suffer from the singularity.

In addition to such a singular pose, in Figure 13, we also show several other
singular poses of the legged robot.

  * •

Case (a): As discussed above, the legs are fully stretched and cannot move
vertically due to singularity of the Jacobian matrix.

  * •

Case (b): The hip yaw axis and the ankle roll axis are collinear, and the leg
is also in a singular pose.

  * •

Case (c): The robot hip roll axis and ankle roll axis are collinear and it is
at a singular pose as well.

Figure 13: Legged Robot Singularity Poses [2].

###  4.6 Singularity-Robust Inverse of Jacobian

By now, readers should know that the Jacobian matrix and its inverse plays an
important role in robot inverse kinematics. When the Jacobian matrix is
singular, we will suffer from lots of problems in calculating its inverse
kinematics parameters. Meanwhile, in applications, people has almost zero
tolerance for wired performance of the robots due to the singularity problem.
At the end of this section, we will introduce a singularity robust inverse of
the Jacobian matrix, which allows the robots still be able to work with no
worries about the singularity problem.

To illustrate the effectiveness, we also use an example in this part to
compare the performance of the robot using conventional Jacobian inverse in
Newton Raphson Method introduced in Section 4.3.2 and the new method to
calculate the singular robust inverse of Jacobian matrix.

Figure 14: An Illustration of Legged Robot Suffering from Singularity [2].

###### Example 12

As illustrated in Figure 14, we aim to calculate the robot joint angularity
velocity to move the robot right foot forward from a non-singular pose with
the Newton-Raphson method. The right plot shows the angles of the hip joint,
keen joint and ankle joint at the given target foot position. The solid line
denote the solution obtained by the numerical method. For comparison, we also
provide the analytic solution in the plot, which is denoted by the dotted line
on the right-hand side when the robot reaches its singular pose.

We observe that the robot will reach the singularity, since the axes of the
hip pitch, knee pitch and ankle pitch will collinear. On the right plot, we
observe that the angles of the hip, knee and ankle angles vibrate and go off
the chart.

Let’s take a look at the equation illustrating the relationship between the
twist velocity vector of the end point and the robot joint angular velocity:

$\boldsymbol{\nu}=\mathbf{J}\dot{\mathbf{q}}.$ (175)

We aim to find the desired vector $\dot{\mathbf{q}}$ to make the above
equation hold. When the Jacobian matrix $\mathbf{J}$ is not singular, we can
easily solve the problem with
$\dot{\mathbf{q}}=\mathbf{J}^{-1}\boldsymbol{\nu}$. However, when matrix
$\mathbf{J}$ is singular, we have to find another way to calculate the optimal
$\dot{\mathbf{q}}$ vector.

From the above description, some readers probably already get a sense that
“can we solve the problem as an mathematical optimization problem?”. Finding
the solution to equation $\boldsymbol{\nu}=\mathbf{J}\dot{\mathbf{q}}$ is
actually equivalent to find the vector $\dot{\mathbf{q}}$ that can make the
error vector $\mathbf{e}$ equal to zero, where

$\mathbf{e}=\boldsymbol{\nu}-\mathbf{J}\dot{\mathbf{q}}.$ (176)

Making the error vector $\mathbf{e}=\mathbf{0}$ is not an easy task.
Sometimes, we will not be able to find such a solution actually, which will
render the robot failing to work. Then, can we slightly relax our expectation?
Instead of find the error vector $\dot{\mathbf{q}}$ to make
$\mathbf{e}=\mathbf{0}$, we propose to find the optimal vector
$\dot{\mathbf{q}}$ to make the error vector $\mathbf{e}$ as close to the zero
vector $\mathbf{0}$ as possible, i.e.,

$\min_{\dot{\mathbf{q}}}\mathbf{E}(\dot{\mathbf{q}}),$ (177)

where we can introduce a loss function notation
$\mathbf{E}(\dot{\mathbf{q}})=\frac{1}{2}\mathbf{e}^{\top}\mathbf{e}$ to
represent the optimization objective function. If the function
$\mathbf{E}(\dot{\mathbf{q}})$ approaches $0$, the error vector $\mathbf{e}$
will be close to the zero vector as well, and vice versa.

The optimal variable $\dot{\mathbf{q}}$ can be obtained by making the
derivative of $\mathbf{E}(\dot{\mathbf{q}})$ with respect to variable
$\dot{\mathbf{q}}$ equal to zero, i.e.,

$\displaystyle\frac{\partial\mathbf{E}(\dot{\mathbf{q}})}{\partial\dot{\mathbf{q}}}=\mathbf{0}$
(178) $\displaystyle\Longrightarrow$ $\displaystyle\ \
-\mathbf{J}^{\top}\boldsymbol{\nu}+\mathbf{J}^{\top}\mathbf{J}\dot{\mathbf{q}}=\mathbf{0}$
(179) $\displaystyle\Longrightarrow$ $\displaystyle\ \
\dot{\mathbf{q}}=\left(\mathbf{J}^{\top}\mathbf{J}\right)^{-1}\mathbf{J}^{\top}\boldsymbol{\nu}.$
(180)

Meanwhile, the matrix $\mathbf{J}^{\top}\mathbf{J}$ is actually positive
semidefinite, and it can still be singular. Since
$\det(\mathbf{J}^{\top}\mathbf{J})=\det(\mathbf{J}^{\top})\det(\mathbf{J})$,
if the Jacobian matrix $\mathbf{J}$ is singular, i.e., $\det(\mathbf{J})=0$,
we can easily get that $\det(\mathbf{J}^{\top}\mathbf{J})=0$, i.e., matrix
product $\mathbf{J}^{\top}\mathbf{J}$ is singular as well. In other words, if
$\mathbf{J}$ is singular, the above representation doesn’t exist neither.

If the readers have any experiences with mathematical optimization, especially
with variable regularizations, you probably can propose different definition
of the loss function $\mathbf{E}(\dot{\mathbf{q}})$ definition. To address the
above solution non-existence problem when $\mathbf{J}$ is singular, we propose
to modify the loss function slightly by incorporating a regularization term on
variable $\dot{\mathbf{q}}$ as follows:

$\mathbf{E}(\dot{\mathbf{q}})=\frac{1}{2}\mathbf{e}^{\top}\mathbf{e}+\frac{\lambda}{2}\dot{\mathbf{q}}^{\top}\dot{\mathbf{q}}.$
(181)

By solving the equation
$\frac{\partial\mathbf{E}(\dot{\mathbf{q}})}{\partial\dot{\mathbf{q}}}=\mathbf{0}$,
we can obtain the representation of the solution to be

$\displaystyle\frac{\partial\mathbf{E}(\dot{\mathbf{q}})}{\partial\dot{\mathbf{q}}}=\mathbf{0}$
(182) $\displaystyle\Longrightarrow$ $\displaystyle\ \
-\mathbf{J}^{\top}\boldsymbol{\nu}+\left(\mathbf{J}^{\top}\mathbf{J}+\lambda\mathbf{I}\right)\dot{\mathbf{q}}=\mathbf{0}$
(183) $\displaystyle\Longrightarrow$ $\displaystyle\ \
\dot{\mathbf{q}}=\left(\mathbf{J}^{\top}\mathbf{J}+\lambda\mathbf{I}\right)^{-1}\mathbf{J}^{\top}\boldsymbol{\nu}.$
(184)

We know $\mathbf{J}^{\top}\mathbf{J}$ is positive semidefinite and if
$\lambda\neq 0$, their summation
$\mathbf{J}^{\top}\mathbf{J}+\lambda\mathbf{I}$ will be positive definite and
its inverse always exists, which ensures we can find a solution
$\dot{\mathbf{q}}$ no matter what the Jacobian matrix $\mathbf{J}$ is
actually.

We can introduce a new notation for the term
$\left(\mathbf{J}^{\top}\mathbf{J}+\lambda\mathbf{I}\right)^{-1}\mathbf{J}^{\top}$
as follows

$\mathbf{J}^{\dagger}=\left(\mathbf{J}^{\top}\mathbf{J}+\lambda\mathbf{I}\right)^{-1}\mathbf{J}^{\top},$
(185)

and the solution can be represented with the new notation as

$\dot{\mathbf{q}}=\mathbf{J}^{\dagger}\boldsymbol{\nu}.$ (186)

We also name $\mathbf{J}^{\dagger}$ as the singularity-robust inverse of the
Jacobian matrix $\mathbf{J}$.

###### Example 13

Let’s come back to the example we show at the beginning of this part. As shown
in Figure 15, to move the robot right foot forward, with the new singularity-
robust inverse of the Jacobian matrix $\mathbf{J}$, we can get rid of the
singular pose of the robot perfectly in the movement. As shown in the right
plot of the joint angles, we can still control the robot pretty without
singularity problem at all.

Figure 15: An Illustration of Legged Robot that Gets Rid of Singularity [2].

##  5 Robot Dynamics

We have been introducing the robot joint rotation, robot motion, forward
kinematics and inverse kinematics for readers already, which covers the
transformation of robot position, orientation, linear velocity and angular
velocity within as well as across coordinate systems. Meanwhile, we didn’t
discuss much about the driven forces that lead to such changes yet, which will
be the main topic to be studied in this section for readers, i.e., the robot
dynamics.

###  5.1 What is Robot Dynamics?

Formally, robot dynamics aims to study the relationship between the forces (or
torques) that act on a robot and the accelerations that they produce. For the
robot studied in this article, the forces covers both the internal forces,
e.g., the driving forces of joint motors, as well as external forces, e.g.,
the gravity of robot links and body, as well as the items the robot end points
trying to pickup. Meanwhile, the generated accelerations overtime will create
velocity and further change the pose of robot parts and body.

There exist different formulation methods of robot dynamics. In this section,
we will introduce two dynamics formulation methods for readers that are
initially proposed in Physics but can also be used for formulating robot
dynamics as well, i.e.,

  * •

Lagrangian Dynamics Formulation: In the Lagrangian dynamics, motion is
described by energies as scalars and it is based on the principle of least
action. Generalized coordinates are used in Lagrangian dynamics instead of
constraint forces, and conservation laws can be derived easily. Lagrangian
dynamics is not idea for non-conservative forces, e.g., friction, and widely
used in all areas of physics.

  * •

Newton-Euler Dynamics Formulation: In the Newtonian dynamics, motion is
described by forces with vectors and it is based on the Newton’s laws of
motion. Newtonian dynamics involves constraint forces, and doesn’t have a
systematic methods for deriving conservation laws. Lagrangian dynamics handles
non-conservative forces very well, and mainly applicable to classic physics.

There are two main problems that are studied in robot dynamics, which include

  * •

Forward Dynamics: Given the known forces, calculate the acceleration (as well
as velocity and pose) of the robot generated by the forces.

  * •

Inverse Dynamics: Given the desired acceleration (as well as velocity and
pose), figure out the needed forces to generate them.

Similar to the forward kinematics and inverse kinematics discussed before, the
forward dynamics is normally used for simulation, while the inverse dynamics
has very diverse usage in robot control.

In this section, we first will introduce Lagrangian dynamics formulation and
Newton-Euler dynamics formulation for readers. At the end, we will use a
concrete example of motor driven robot link to discuss about the forward
dynamics and inverse dynamics for readers, and the following subsections are
also organized according to these topics.

###  5.2 Lagrangian Dynamics Formulation

In this part, we will introduce the Lagrangian formulation of dynamics and
discuss about how to use it to model the dynamics of robot arms.

####  5.2.1 Euler-Lagrange Equation

To use Lagrangian formulation of robot dynamics, the first step is to choose a
set of independent coordinates $\mathbf{q}\in\mathbbm{R}^{n}$ that defines the
robot configuration. The chosen coordinates can not only be the Cartesian
coordinate but also other configurations of the robot, e.g., the angles of the
joints, which are also named as the generalized coordinates. The set of all
potential configurations $\mathbf{q}$ will define the configuration space
$\mathbbm{C}$.

###### Definition 14

(Lagrangian Dynamics): Formally, the Lagrangian dynamics formulates a dynamics
system as a pair $(\mathbbm{C},L)$, where $\mathbbm{C}$ denotes the
configuration space and $L$ is the Lagrangian function defined as follows:

$L(\mathbf{q},\dot{\mathbf{q}})=K(\mathbf{q},\dot{\mathbf{q}})-P(\mathbf{q})\text{,
where }\mathbf{q}\in\mathbbm{C}.$ (187)

In the above equation, terms $K(\mathbf{q},\dot{\mathbf{q}})$ and
$P(\mathbf{q})$ denote the kinetic energy and potential energy of the system,
respectively. Notation $\dot{\mathbf{q}}$ denotes the first-order derivative
of term $\mathbf{q}\in\mathbbm{C}$.

After choosing the generalized coordinates, we can also further define the
generalized forces $\mathbf{f}\in\mathbbm{R}^{n}$ subject to the constraint
that $\mathbf{f}^{\top}\mathbf{q}$ corresponds to power. Depending on the
chosen generalized coordinates, the generalized forces $\mathbf{f}$ can not
only represent regular forces, but can also represent other properties of the
robot, e.g., the torque of joints. For instance, if generalized coordinates
$\mathbf{q}$ denotes the position in Cartesian coordinate, then $\mathbf{f}$
will be regular forces, since the product of forces and moving distance will
denote the power. Meanwhile, if $\mathbf{q}$ denotes the rotation angles, then
$\mathbf{f}$ should represent the torques, since the product of torque and
rotation angles denotes the power.

Based on the Lagrangian function defined above, any equations of motion can
now be expressed in terms of the Lagrangian as follows:

$\mathbf{f}=\frac{d}{dt}\frac{\partial
L}{\partial\dot{\mathbf{q}}}-\frac{\partial L}{\partial\mathbf{q}},$ (188)

where $\mathbf{f}$ denotes the generalized forces. Such an equation is also
referred to as the Euler-Lagrange equation.

Instead of forces, Lagrangian dynamics uses energy in the system formulation,
and the Lagrangian function defined above summarizes the dynamics of the
entire system. Some readers may wonder “why the Lagrangian function is defined
as the kinetic energy minus the potential energy?” According to Physics,
defining Lagrangian function in this way will allow us to generate the correct
equations of motion in agreement with classic physical laws, e.g., the
“Newton’s laws of motion”.

###### Example 14

To illustrate that the above Lagrangian function can be used to define the
equations of motion, we also provide an example of a particle of mass $m$ that
is moving within a 1D space along a vertical line. We represent the position
(i.e., the height) of the particle as a variable $q$, while its speed and
acceleration can be denoted as $\dot{q}$ and $\ddot{q}$, respectively. The
gravitational force acting on the particle will generate an acceleration
represented by $g$. According to what we learn from high-school Physics
course, we can represent the motion of the particle as

$f-mg=m\ddot{q},$ (189)

where $f$ is the external force acting on the particle to drive its motion. If
readers still remember, the above equation is derived according to the classic
Newton’s second law of motion.

Meanwhile, the Euler-Lagrange equation introduced above will also help derive
the identical representation of the external force. For the particle, we can
represent its kinetic energy and potential energy as follows:

$K(q,\dot{q})=\frac{1}{2}m\dot{q}^{2}\text{, }P(q)=mgq,$ (190)

which will define the Lagrangian function

$L(q,\dot{q})=K(q,\dot{q})-P(q)=\frac{1}{2}m\dot{q}^{2}-mgq.$ (191)

According to Equation 211, we can represent the equation of motion as

$\displaystyle f$ $\displaystyle=\frac{d}{dt}\frac{\partial
L}{\partial\dot{{q}}}-\frac{\partial L}{\partial{q}}$ (192)
$\displaystyle=m\ddot{q}+mg,$ (193)

which is identical to Equation 189 derived from Newton’s second law of motion.

####  5.2.2 Lagrangian Dynamics of 2-Link Robot Arm

In the previous subsection, we introduce the Lagrangian formulation about the
dynamics of a particle, which can derive the identical dynamic equation as
Newton’s second law of motion.

Figure 16: Dynamics of Two-Link Robot Arm.

###### Example 15

As illustrated in Figure 16, we provide a 2-link robot arm in a 2D space. The
positive $x$ and $y$ axis orientations are provided by the coordinate system
with origin located at the root of the arm. The length and mass of these two
arm links are $L_{1}$, $m_{1}$ and $L_{2}$, $m_{2}$, respectively. In this
example, we assume the mass of these two links are concentrated at the links’
end points, i.e., the orange and green dots in the figure. The first link
rotates in the counter clockwise direction with an angle of $\theta_{1}$
degrees, and the second link further rotates with another angle of
$\theta_{2}$ degrees.

According to the link length and rotation angles, we can represent the
position of the these two arm links’ end points in the coordinate system as

$\begin{bmatrix}x_{1}\\\ y_{1}\\\
\end{bmatrix}=\begin{bmatrix}L_{1}\cos(\theta_{1})\\\ L_{1}\sin(\theta_{1})\\\
\end{bmatrix}\text{, and }\begin{bmatrix}x_{2}\\\ y_{2}\\\
\end{bmatrix}=\begin{bmatrix}L_{1}\cos(\theta_{1})+L_{2}\cos(\theta_{1}+\theta_{2})\\\
L_{1}\sin(\theta_{1})+L_{2}\sin(\theta_{1}+\theta_{2})\\\ \end{bmatrix}.$
(194)

The velocity of these two arm links’ end points can be represented as the
derivative of the position with respect to time, i.e.,

$\displaystyle\begin{bmatrix}\dot{x}_{1}\\\ \dot{y}_{1}\\\
\end{bmatrix}=\begin{bmatrix}-L_{1}\sin(\theta_{1})\\\
L_{1}\cos(\theta_{1})\\\ \end{bmatrix}\dot{\theta}_{1},$ (195)
$\displaystyle\begin{bmatrix}\dot{x}_{2}\\\ \dot{y}_{2}\\\
\end{bmatrix}=\begin{bmatrix}-L_{1}\sin(\theta_{1})-L_{2}\sin(\theta_{1}+\theta_{2})&-L_{2}\sin(\theta_{1}+\theta_{2})\\\
L_{1}\cos(\theta_{1})\dot{\theta}_{1}+L_{2}\cos(\theta_{1}+\theta_{2})&L_{2}\cos(\theta_{1}+\theta_{2})\\\
\end{bmatrix}\begin{bmatrix}\dot{\theta}_{1}\\\ \dot{\theta}_{2}\\\
\end{bmatrix},$ (196)

where $\dot{\theta}_{1}=\frac{d\theta_{1}}{dt}$ and
$\dot{\theta}_{2}=\frac{d\theta_{2}}{dt}$ denote the derivatives of
$\theta_{1}$ and $\theta_{2}$ with respect to the time.

Meanwhile, based on the above derivations, we can also calculate the current
kinetic energy and potential energy of the robot arm as

$\displaystyle K_{1}$
$\displaystyle=\frac{1}{2}m_{1}(\dot{x}_{1}^{2}+\dot{y}_{1}^{2})=\frac{1}{2}m_{1}L_{1}\dot{\theta}_{1}^{2},$
(197) $\displaystyle K_{2}$
$\displaystyle=\frac{1}{2}m_{2}(\dot{x}_{2}^{2}+\dot{y}_{2}^{2})$ (198)
$\displaystyle=\frac{1}{2}m_{2}\left((L_{1}^{2}+2L_{1}L_{2}\cos(\theta_{2})+L_{2}^{2})\dot{\theta}_{1}^{2}+2(L_{2}^{2}+L_{1}L_{2}\cos(\theta_{2}))\dot{\theta}_{1}\dot{\theta}_{2}+L_{2}^{2}\dot{\theta}_{2}^{2}\right),$
(199) $\displaystyle P_{1}$
$\displaystyle=m_{1}gy_{1}=m_{1}gL_{1}\sin(\theta_{1}),$ (200) $\displaystyle
P_{2}$
$\displaystyle=m_{1}gy_{1}=m_{2}g\left(L_{1}\sin(\theta_{1})+L_{2}\sin(\theta_{1}+\theta_{2})\right).$
(201)

By combining them together, we will be able to define the Lagrangian functoin

$L(\boldsymbol{\theta},\dot{\boldsymbol{\theta}})=\sum_{i=1}^{2}(K_{i}-P_{i}).$
(202)

By choosing the joint angles $\boldsymbol{\theta}=\begin{bmatrix}\theta_{1}\\\
\theta_{2}\\\ \end{bmatrix}$ as the generalized coordinates, as discussed in
the previous Section 5.2.1, then the corresponding generalized forces derived
in Equation 211 will actually be the torques
$\boldsymbol{\tau}=\begin{bmatrix}\boldsymbol{\tau}_{1}\\\
\boldsymbol{\tau}_{2}\\\ \end{bmatrix}$:

$\displaystyle\boldsymbol{\tau}_{1}=\frac{d}{dt}\frac{\partial
L}{\partial\dot{\theta}_{1}}-\frac{\partial L}{\partial\theta_{1}}\text{, and
}\boldsymbol{\tau}_{2}=\frac{d}{dt}\frac{\partial