+++
title = "Parallel robots : mechanics and control"
author = ["Thomas Dehaeze"]
draft = false
+++

Tags
: [Stewart Platforms]({{< relref "stewart_platforms" >}}), [Reference Books]({{< relref "reference_books" >}})

Reference
: <sup id="1a9683d2a60e031fa9b8ed16f4944c9f"><a href="#taghirad13_paral" title="Taghirad, Parallel robots : mechanics and control, CRC Press (2013).">(Taghirad, 2013)</a></sup>

Author(s)
: Taghirad, H.

Year
: 2013


## Introduction {#introduction}

<a id="orgb4a81bf"></a>

This book is intended to give some analysis and design tools for the increase number of engineers and researchers who are interested in the design and implementation of parallel robots.
A systematic approach is presented to analyze the kinematics, dynamics and control of parallel robots.
To define the motion characteristics of such robots, it is necessary to represent 3D motion of the robot moving platform with respect to a fixed coordinate.
This issue leads to the requirements for 3D representation of position, orientation and motion of bodies in space.
In chapter [sec:motion_representation](#sec:motion_representation), such representation are introduced with emphasis on screw coordinates, which makes the representation of the general motion of the robot much easier to follow.<br />

Kinematic analysis refers to the study of robot motion geometry without considering the forces and torques that cause the motion.
In this analysis (chapter [sec:kinematics](#sec:kinematics)), the relation between the geometrical parameters of the manipulator and the final motion of the moving platform is derived and analyzed.<br />

In Chapter [sec:jacobian](#sec:jacobian), the kinematics analysis of robot manipulators is further examined beyond static positioning.
Jacobian analysis not only reveals the relation between the joint variable velocities of a parallel manipulator and the moving platform linear and angular velocities, but also constructs the transformation needed to find the actuator forces from the forces and moments acting on the moving platform.
A systematic means to perform Jacobian analysis of parallel manipulators is given in this chapter.<br />

Dynamic analysis of parallel manipulators presents an inherent complexity due to their closed-loop structure and kinematic constraints.
Nevertheless, dynamic modeling is quite important for the control, in particular because parallel manipulators are preferred in applications where precise positioning and suitable dynamic performance under high loads are the prime requirements.
In Chapter [sec:dynamics](#sec:dynamics), the dynamic analysis of such robots is examined through three methods, namely the Newton-Euler principle of virtual work and Lagrange formations.
Furthermore, a method is presented in this chapter to formulate the dynamic equation of parallel robots into closed form, by which the dynamic matrices are more tractable, and dynamics verification becomes feasible.<br />

The control of parallel robots is elaborated in the last two chapters, in which both the motion and force control are covered.


## Motion Representation {#motion-representation}

<a id="org9be8358"></a>


### Spatial Motion Representation {#spatial-motion-representation}

Six independent parameters are sufficient to fully describe the spatial location of a rigid body.

Consider a rigid body in a spatial motion as represented in Figure [fig:rigid_body_motion](#fig:rigid_body_motion).
Let us define:

-   A **fixed reference coordinate system** \\((x, y, z)\\) denoted by frame \\(\\{\bm{A}\\}\\) whose origin is located at point \\(O\_A\\)
-   A **moving coordinate system** \\((u, v, z)\\) denoted by frame \\(\\{\bm{B}\\}\\) attached to the rigid body at point \\(O\_B\\)

The absolute position of point \\(P\\) of the rigid body can be constructed from the relative position of that point with respect to the moving frame \\(\\{\bm{B}\\}\\), and the **position and orientation** of the moving frame \\(\\{\bm{B}\\}\\) with respect to the fixed frame \\(\\{\bm{A}\\}\\).

<a id="org221cb93"></a>

{{< figure src="/ox-hugo/taghirad13_rigid_body_motion.png" caption="Figure 1: Representation of a rigid body spatial motion" >}}


#### Position of a point {#position-of-a-point}

The position of a point \\(P\\) with respect to a frame \\(\\{\bm{A}\\}\\) can be described by a \\(3 \times 1\\) position vector.
The name of the frame is usually added as a leading superscript: \\({}^A\bm{P}\\) which reads as vector \\(\bm{P}\\) in frame \\(\\{\bm{A}\\}\\).

\begin{equation}
  \tcmbox{{}^A\bm{P} = \begin{bmatrix} P\_x\\ P\_y\\ P\_z \end{bmatrix}}
\end{equation}


#### Orientation of a Rigid Body {#orientation-of-a-rigid-body}

The orientation of the whole rigid body is the same for all its points (by definition).
Hence, representation of the orientation of a rigid body can be viewed as that for the orientation of a moving frame attached to the rigid body.
It can be **represented in several different ways**: the rotation matrix, the screw axis representation and Euler angles are common descriptions.


##### Rotation Matrix {#rotation-matrix}

We consider a rigid body that has been exposed to a pure rotation.
Its orientation has changed from a state represented by frame \\(\\{\bm{A}\\}\\) to its current orientation represented by frame \\(\\{\bm{B}\\}\\) (Figure [2](#org1448d5b)).

A \\(3 \times 3\\) rotation matrix \\({}^A\bm{R}\_B\\) is defined by

\begin{equation}
  \tcmbox{{}^A\bm{R}\_B = \left[ {}^A\hat{\bm{x}}\_B | {}^A\hat{\bm{y}}\_B | {}^A\hat{\bm{z}}\_B \right] = \begin{bmatrix}
    u\_{x} & v\_{x} & z\_{x} \\\\\\
    u\_{y} & v\_{y} & z\_{y} \\\\\\
    u\_{z} & v\_{z} & z\_{z}
  \end{bmatrix}}
\end{equation}

in which \\({}^A\hat{\bm{x}}\_B, {}^A\hat{\bm{y}}\_B\\) and \\({}^A\hat{\bm{z}}\_B\\) are the Cartesian unit vectors of frame \\(\\{\bm{B}\\}\\) represented in frame \\(\\{\bm{A}\\}\\).

\begin{align\*}
  {}^A\hat{\bm{x}}\_B &= {}^A\hat{u} = u\_x \hat{i} + u\_y \hat{j} + u\_z \hat{k} \\\\\\
  {}^A\hat{\bm{y}}\_B &= {}^A\hat{v} = v\_x \hat{i} + v\_y \hat{j} + v\_z \hat{k} \\\\\\
  {}^A\hat{\bm{z}}\_B &= {}^A\hat{w} = w\_x \hat{i} + w\_y \hat{j} + w\_z \hat{k}
\end{align\*}

The nine elements of the rotation matrix can be simply represented as the projections of the Cartesian unit vectors of frame \\(\\{\bm{B}\\}\\) on the unit vectors of frame \\(\\{\bm{A}\\}\\).

<a id="org1448d5b"></a>

{{< figure src="/ox-hugo/taghirad13_rotation_matrix.png" caption="Figure 2: Pure rotation of a rigid body" >}}

The rotation matrix has a number of properties linking each of its nine elements:

-   **Orthonormality**: the rotation matrix is an orthonormal matrix
-   **Transposition**: \\({}^B\bm{R}\_A = {}^A\bm{R}\_B^{T}\\)
-   **Inverse**: \\({}^B\bm{R}\_A = {}^A\bm{R}\_B^{-1} = {}^A\bm{R}\_B^{T}\\)
-   **Pure Rotation Mapping**: Suppose that the point of a rigid body with respect to the moving frame \\(\\{\bm{B}\\}\\) is given and denoted by \\({}^B\bm{P}\\) and we wish to express the position of this point with respect to the fixed frame \\(\\{\bm{A}\\}\\). Consider that the rigid body has been exposed to a pure rotation (\\(\\{\bm{A}\\}\\) and \\(\\{\bm{B}\\}\\) are coincident at their origins). Then
    \\[ \tcmbox{{}^A\bm{P} = {}^A\bm{R}\_B {}^B\bm{P}} \\]
-   **Determinant**: \\(\det({}^A\bm{R}\_B) = 1\\)
-   **Eigenvalues**: The eigenvalues of a rotation matrix \\({}^A\bm{R}\_B\\) are equal to \\(1\\), \\(e^{i\theta}\\) and \\(e^{-i\theta}\\) where \\(\theta\\) is calculated from \\(\theta = \cos^{-1}\frac{\text{tr}({}^A\bm{R}\_B) - 1}{2}\\).


##### Screw Axis Representation {#screw-axis-representation}

As seen previously, there exist an **invariant angle** \\(\theta\\) corresponding to the rotation matrix. This angle is an equivalent angle of rotation.
The rotation is a spatial change of orientation about an axis which is called the **screw axis**.
It can be shown that this screw axis is also an invariant of the rotation matrix, it is the eigenvector corresponding to the eigenvalue \\(\lambda = 1\\).

The term screw axis for this axis of rotation has the benefit that a general motion of a rigid body, which is composed as a pure translation and a pure rotation, can be further represented by the same axis of rotation.

The screw axis representation has the benefit of **using only four parameters** to describe a pure rotation.
These parameters are the angle of rotation \\(\theta\\) and the axis of rotation which is a unit vector \\({}^A\hat{\bm{s}} = [s\_x, s\_y, s\_z]^T\\).

<a id="org778c755"></a>

{{< figure src="/ox-hugo/taghirad13_screw_axis_representation.png" caption="Figure 3: Pure rotation about a screw axis" >}}

The Rodrigue's rotation formula for spatial rotation of a rigid body gives us the new position \\(\bm{P}\_2\\) of point \\(\bm{P}\_1\\) after a rotation represented by the screw axis \\(\hat{\bm{s}}\\) and the angle \\(\theta\\):

\begin{equation}
  \tcmbox{\bm{P}\_2 = \bm{P}\_1 \cos \theta + (\hat{\bm{s}} \times \bm{P}\_1)\sin\theta + (\bm{P}\_1 \cdot \hat{\bm{s}})\hat{\bm{s}}}
\end{equation}


##### Euler Angles {#euler-angles}

Since rotation in space is a motion with three-degrees-of-freedom, a set of three independent parameters is sufficient to represent the orientation.

In an Euler angle representation, three **successive** rotations about the coordinate system of either **fixed** or **moving** frame are used to describe the orientation of the rigid body.

One type of Euler angle corresponds to rotations considered with respect to the fixed frame. The representation is called pitch-roll-yaw, or fixed X-Y-Z Euler angles.

Three other types of Euler angles are consider with respect to a moving frame: they are denoted \\(w-v-u\\), \\(w-v-w\\) and \\(w-u-w\\) Euler angles.


#### Pitch-Roll-Yaw Euler Angles {#pitch-roll-yaw-euler-angles}

The pitch, roll and yaw angles are defined for a moving object in space as the rotations along the lateral, longitudinal and vertical axes attached to the moving object.

<a id="orga52af18"></a>

{{< figure src="/ox-hugo/taghirad13_pitch-roll-yaw.png" caption="Figure 4: Definition of pitch, roll and yaw angles on an air plain" >}}

Since all three rotations take place about the axes of a **fixed coordinate frame**, the resulting rotation matrix is obtained by multiplying the three basic rotation matrices as follows:
\\[ \tcmbox{\bm{R}\_{PRY}(\alpha, \beta, \gamma) = \bm{R}\_z(\gamma) \bm{R}\_y(\beta) \bm{R}\_x(\alpha)} \\]

To go from rotation matrix to Pitch-Roll-Yaw angles, the following set of equations can be used:

\begin{align\*}
  \alpha &= \atan2\left( \frac{r\_{32}}{\cos \beta}, \frac{r\_{33}}{\cos \beta} \right) \\\\\\
  \beta  &= \atan2\left( -r\_{31}, \pm \sqrt{r\_{11}^2 + r\_{21}^2} \right) \\\\\\
  \gamma &= \atan2\left( \frac{r\_{21}}{\cos \beta}, \frac{r\_{11}}{\cos \beta} \right)
\end{align\*}


#### u-v-w Euler Angles {#u-v-w-euler-angles}

Another way to describe the orientation of a moving object is to consider three successive rotations about the **coordinate axes of the moving frame**.
Since the rotations do not occur about fixed axes, pre-multiplications of the individual rotation matrices fails to give the correct solution. It can be shown that the resulting matrix can be derived by post-multiplication of the individual rotation matrices as follows:
\\[ {}^A\bm{R}\_B(\alpha, \beta, \gamma) = \bm{R}\_u(\alpha) \bm{R}\_v(\beta) \bm{R}\_w(\gamma) \\]

The inverse solution for the u-v-w Euler angles is the following (for \\(\cos \beta \ne 0\\)):

\begin{align\*}
  \alpha &= \atan2\left( -\frac{r\_{23}}{\cos \beta}, \frac{r\_{33}}{\cos \beta} \right) \\\\\\
  \beta  &= \atan2\left( r\_{13}, \pm \sqrt{r\_{11}^2 + r\_{12}^2} \right) \\\\\\
  \gamma &= \atan2\left( -\frac{r\_{12}}{\cos \beta}, \frac{r\_{11}}{\cos \beta} \right)
\end{align\*}


#### w-v-w Euler Angles {#w-v-w-euler-angles}

Similarly:
\\[ \bm{R}\_{wvw}(\alpha, \beta, \gamma) = \bm{R}\_w(\alpha) \bm{R}\_v(\beta) \bm{R}\_w(\gamma) \\]

And for \\(\sin\beta\ne0\\):

\begin{align\*}
  \alpha &= \atan2\left( \frac{r\_{23}}{\sin\beta}, \frac{r\_{13}}{\sin\beta} \right) \\\\\\
  \beta  &= \atan2\left( \pm \sqrt{r\_{31}^2 + r\_{32}^2}, r\_{33} \right) \\\\\\
  \gamma &= \atan2\left( \frac{r\_{32}}{\sin\beta}, -\frac{r\_{31}}{\sin\beta} \right)
\end{align\*}


#### w-u-w Euler Angles {#w-u-w-euler-angles}

Here, the second rotation is about the \\(u\\) axis:
\\[ \bm{R}\_{wuw}(\alpha, \beta, \gamma) = \bm{R}\_w(\alpha) \bm{R}\_u(\beta) \bm{R}\_w(\gamma) \\]

And for \\(\sin\beta\ne0\\):

\begin{align\*}
  \alpha &= \atan2\left( \frac{r\_{13}}{\sin\beta}, -\frac{r\_{23}}{\sin\beta} \right) \\\\\\
  \beta  &= \atan2\left( \pm \sqrt{r\_{31}^2 + r\_{32}^2}, r\_{33} \right) \\\\\\
  \gamma &= \atan2\left( \frac{r\_{31}}{\sin\beta}, \frac{r\_{32}}{\sin\beta} \right)
\end{align\*}


#### Notes about Euler Angles {#notes-about-euler-angles}

If the Euler angle is given, a **unique rotation matrix** is determined for the orientation of the rigid body.
However, the inverse map is not one-to-one, and at least two Euler angle sets can be found for each orientation.

If the Euler angle is chosen for the representation of the orientation, extra care should be taken. From the continuity of the motion, a suitable solution may be chosen, such that no abrupt changes are seen in the variation of the Euler angles in a typical maneuver.

The use of rotation matrix to represent the orientation of a rigid body is then generally preferred although there are nine parameters for that description.


### Motion of a Rigid Body {#motion-of-a-rigid-body}

Since the relative positions of a rigid body with respect to a moving frame \\(\\{\bm{B}\\}\\) attached to it is fixed for all time, it is sufficient to know the **position of the origin of the frame** \\(O\_B\\) and the **orientation of the frame** \\(\\{\bm{B}\\}\\) with respect to the fixed frame \\(\\{\bm{A}\\}\\), to represent the position of any point \\(P\\) in the space.<br />

Representation of the position of \\(O\_B\\) is uniquely given by the position vector, while orientation of the rigid body is represented in different forms.
However, for all possible orientation representation, a rotation matrix \\({}^A\bm{R}\_B\\) can be derived.<br />

<div class="cbox">
  <div></div>

Therefore, the location or **pose** of a rigid body, can be **fully determined** by:

1.  The **position vector** of point \\(O\_B\\) with respect to frame \\(\\{\bm{A}\\}\\) which is denoted \\({}^A\bm{P}\_{O\_B}\\)
2.  The **orientation of the rigid body**, or the moving frame \\(\\{\bm{B}\\}\\) attached to it with respect to the fixed frame \\(\\{\bm{A}\\}\\), that is represented by \\({}^A\bm{R}\_B\\).

</div>

The position of any point \\(P\\) of the rigid body with respect to the fixed frame \\(\\{\bm{A}\\}\\), which is denoted \\({}^A\bm{P}\\) may be determined thanks to the Chasles' theorem.

<div class="cbox">
  <div></div>

If the pose of a rigid body \\(\\{{}^A\bm{R}\_B, {}^A\bm{P}\_{O\_B}\\}\\) is given, then the position of any point \\(P\\) of this rigid body with respect to \\(\\{\bm{A}\\}\\) is given by:

\begin{equation}
  {}^A\bm{P} = {}^A\bm{R}\_B {}^B\bm{P} + {}^A\bm{P}\_{O\_B}
\end{equation}

</div>


### Homogeneous Transformations {#homogeneous-transformations}

To describe general transformations, we introduce the \\(4\times1\\) **homogeneous coordinates**, and Eq. \eqref{eq:chasles_therorem} is generalized to

\begin{equation}
  \tcmbox{{}^A\bm{P} = {}^A\bm{T}\_B {}^B\bm{P}}
\end{equation}

in which \\({}^A\bm{T}\_B\\) is a \\(4\times4\\) **homogeneous transformation matrix**.


#### Homogeneous Coordinates {#homogeneous-coordinates}

There are two basic classes of vector quantities, the generalization to homogeneous coordinates of which are different.<br />

The first type is called **line vector**. Line vectors refer to a vector of which its value depends on the line of action, or the position of where it is applied. Examples are the position vector, linear velocity, force vector.<br />

On the contrary, there exist quantities likes orientation that **hold for the whole rigid body** and do not correspond to a point. They can be positioned freely throughout the whole rigid body, without any change in their quantity. These types of vectors are called **free vectors**.<br />

For line vectors, both orientation and translation of the moving frame contribute to their value.
Homogeneous coordinate of such vectors is generated by appending \\(1\\) to the three components of that vector:

\begin{equation}
  \tcmbox{\bm{V} = \begin{bmatrix} v\_x \\ v\_y \\ v\_z \\ 1 \end{bmatrix}}
\end{equation}

For free vectors, only the orientation of the moving frame contributes to their value.
The homogeneous coordinate is then

\begin{equation}
  \tcmbox{\bm{\omega} = \begin{bmatrix} \omega\_x \\ \omega\_y \\ \omega\_z \\ 0 \end{bmatrix}}
\end{equation}


#### Homogeneous Transformation Matrix {#homogeneous-transformation-matrix}

<div class="cbox">
  <div></div>

The **homogeneous transformation matrix** is a \\(4\times4\\) matrix, defined for the purpose of transformation mapping of a vector in a homogeneous coordinate from one frame to another in a compact form.
The matrix is composed of the rotation matrix \\({}^A\bm{R}\_B\\) representing the orientation and the position vector \\({}^A\bm{P}\_{O\_B}\\) representing the translation.
It is partitioned as follows:

\begin{equation}
  {}^A\bm{T}\_B =
  \left[ \begin{array}{ccc|c}
      &   &   & \\\\\\
      & {}^A\bm{R}\_B  &   & {}^A\bm{P}\_{O\_B} \\\\\\
      &   &   & \\\\\\
    \hline
    0 & 0 & 0 & 1 \\\\\\
  \end{array} \right]
\end{equation}

</div>

The homogeneous transformation matrix \\({}^A\bm{T}\_B\\) is a \\(4\times4\\) matrix operator mapping **vector valued** quantities represented by \\(4\times1\\) homogeneous coordinates.:

\begin{align\*}
  \left[ \begin{array}{c} \\ {}^A\bm{P} \\ \\ \hline 1 \end{array} \right]
  & =
  \left[ \begin{array}{ccc|c}
      &   &   & \\\\\\
      & {}^A\bm{R}\_B  &   & {}^A\bm{P}\_{O\_B} \\\\\\
      &   &   & \\\\\\
    \hline
    0 & 0 & 0 & 1 \\\\\\
  \end{array} \right]
  \left[ \begin{array}{c} \\ {}^B\bm{P} \\ \\ \hline 1 \end{array} \right] \\\\\\
  {}^A\bm{P} &= {}^A\bm{R}\_B {}^B\bm{P} + {}^A\bm{P}\_{O\_B}
\end{align\*}

Using homogeneous coordinate for a **free vector** like angular velocity of a rigid body:

\begin{align\*}
  \left[ \begin{array}{c} \\ {}^A\bm{\omega} \\ \\ \hline 0 \end{array} \right]
  & =
  \left[ \begin{array}{ccc|c}
      &   &   & \\\\\\
      & {}^A\bm{R}\_B  &   & {}^A\bm{P}\_{O\_B} \\\\\\
      &   &   & \\\\\\
    \hline
    0 & 0 & 0 & 1 \\\\\\
  \end{array} \right]
  \left[ \begin{array}{c} \\ {}^B\bm{\omega} \\ \\ \hline 0 \end{array} \right] \\\\\\
  {}^A\bm{P} &= {}^A\bm{R}\_B {}^B\bm{P}
\end{align\*}


#### Screw Displacement {#screw-displacement}

The most general rigid body displacement can be produced by a **translation along a line followed by a rotation about the same line**.
The line is called the **screw axis**.

There exist transformations to from screw displacement notation to the transformation matrix.


#### Transformation Arithmetics {#transformation-arithmetics}


##### Consecutive transformations {#consecutive-transformations}

Let us consider the motion of a rigid body described at three locations (Figure [fig:consecutive_transformations](#fig:consecutive_transformations)).
Frame \\(\\{\bm{A}\\}\\) represents the initial location, frame \\(\\{\bm{B}\\}\\) is an intermediate location, and frame \\(\\{\bm{C}\\}\\) represents the rigid body at its final location.

<a id="org49a98a7"></a>

{{< figure src="/ox-hugo/taghirad13_consecutive_transformations.png" caption="Figure 5: Motion of a rigid body represented at three locations by frame \\(\\{\bm{A}\\}\\), \\(\\{\bm{B}\\}\\) and \\(\\{\bm{C}\\}\\)" >}}

Furthermore, suppose the position vector of a point \\(P\\) of the rigid body is given in the final location, that is \\({}^C\bm{P}\\) is given, and the position of this point is to be found in the fixed frame \\(\\{\bm{A}\\}\\), that is \\({}^A\bm{P}\\).
Since the locations of the rigid body is known relative to each other, \\({}^C\bm{P}\\) can be transformed to \\({}^B\bm{P}\\) using \\({}^B\bm{T}\_C\\):
\\[{}^B\bm{P} = {}^B\bm{T}\_C {}^C\bm{P}\\]

Now, \\({}^B\bm{P}\\) can be transformed into \\({}^A\bm{P}\\):
\\[ {}^A\bm{P} = {}^A\bm{T}\_B {}^B\bm{P} \\]

And we have:
\\[ {}^A\bm{P} = {}^A\bm{T}\_B {}^B\bm{T}\_C {}^C\bm{P} \\]

From which, the consecutive transformation can be defined as follows:

\begin{equation}
  \tcmbox{{}^A\bm{T}\_C = {}^A\bm{T}\_B {}^B\bm{T}\_C}
\end{equation}


##### Inverse transformation {#inverse-transformation}

Direct inversion of the \\(4\times4\\) homogeneous transfer matrix \\({}^A\bm{T}\_B\\) to obtain \\({}^B\bm{T}\_A\\) might be computationally intensive.
It is much easier to use the specific structure of the transfer matrix for inversion.

To obtain \\({}^B\bm{T}\_A\\), it is necessary to compute \\({}^B\bm{R}\_A\\) and \\({}^B\bm{P}\_{O\_A}\\) from the known \\({}^A\bm{R}\_B\\) and \\({}^A\bm{P}\_{O\_B}\\), then

\begin{equation\*}
  {}^B\bm{T}\_A =
  \left[ \begin{array}{ccc|c}
      &   &   & \\\\\\
      & {}^B\bm{R}\_A  &   & {}^B\bm{P}\_{O\_A} \\\\\\
      &   &   & \\\\\\
    \hline
    0 & 0 & 0 & 1 \\\\\\
  \end{array} \right]
\end{equation\*}

Moreover

\begin{align\*}
  {}^B\bm{R}\_A     &= {}^A\bm{R}\_B^T \\\\\\
  {}^B\bm{P}\_{O\_A} &= {}^B\bm{R}\_A {}^A\bm{P}\_{O\_A} = - {}^B\bm{R}\_A {}^A\bm{P}\_{O\_B} \\\\\\
                   &= -{}^A\bm{R}\_B^T {}^A\bm{P}\_{O\_B}
\end{align\*}

Hence, the **inverse of the transformation matrix** can be obtain by

\begin{equation}
  {}^B\bm{T}\_A = {}^A\bm{T}\_B^{-1} =
  \left[ \begin{array}{ccc|c}
      &   &   & \\\\\\
      & {}^A\bm{R}\_B^T  &   & -{}^A \bm{R}\_B^T {}^A\bm{P}\_{O\_B} \\\\\\
      &   &   & \\\\\\
    \hline
    0 & 0 & 0 & 1 \\\\\\
  \end{array} \right]
\end{equation}


## Kinematics {#kinematics}

<a id="org56e6a14"></a>


### Introduction {#introduction}

<div class="cbox">
  <div></div>

Kinematic analysis refers to the study of the geometry of motion of a robot, without considering the forces an torques that cause the motion.
In this analysis, the relation between the geometrical parameters of the manipulator with the final motion of the moving platform is derived and analyzed.

</div>

A **parallel robot** is a mechanism with a number of **closed kinematic chains**, and its moving platform is linked to the base by several independent kinematic chains.
Parallel robots for which the number of kinematic chains is equal to the number of degrees-of-freedom of the moving platform are called **fully parallel robots**.<br />

If in addition to this condition, if the type and number of joints at each limb, and the number and location of the actuated joints are identical in all the limbs, such a parallel robot is called **symmetric**.<br />

There are three main cases for fully parallel manipulators.
Planar robots with two translation and one rotational degree-of-freedom in the plane.
Spatial orientation manipulators with three rotational degrees-of-freedom in space.
And a general spatial robot with three translational and three rotational degrees-of-freedom in space.<br />

It is known that unlike serial manipulators, **inverse kinematic analysis of parallel robots is usually simple and straightforward**.
In most cases, limb variable may be computed independently using the given pose of the moving platform, and the solution in most cases even for redundant manipulators is uniquely determined.
However, **forward kinematics of parallel manipulators is generally very complicated**, and its solution usually involves systems of nonlinear equations, which are highly coupled and in general have no closed form and unique solution.


### Loop Closure Method {#loop-closure-method}

A typical parallel manipulator consists of two main bodies.
Body \\(A\\) is arbitrary designated as fixed and is called the **base**, while body \\(B\\) is designated to be movable and is called the **moving platform**.<br />
These two bodies are coupled via \\(n\\) **limbs**, each attached to points \\(A\_i\\) and \\(B\_i\\) and called fixed and moving attachment points of the limb \\(i\\).<br />

At the **displacement** level, the **forward kinematic** problem permits the determination of the actual location or pose of the moving platform relative to the base from a set of joint-position readouts.

At the **velocity** level, the **forward kinematic** problem refers to the determination of the translational and angular velocities of the moving platform relative to the base, from a set of joint-velocity readouts and for a known configuration.<br />

To describe the motion of the moving platform relative to the base, frame \\(\\{\bm{A}\\}\\) is attached to body \\(A\\) and frame \\(\\{\bm{B}\\}\\) to body \\(B\\).
The pose of the moving platform relative to the base is thus defined by:

-   A position vector \\(\bm{p}\\) which denotes the position vector of the origin of \\(\\{\bm{B}\\}\\) with respect to frame \\(\\{\bm{A}\\}\\)
-   A \\(3\times3\\) rotation matrix \\(R\\) which denotes the rotation of \\(\\{\bm{B}\\}\\) with respect to \\(\\{\bm{A}\\}\\)

Each limb of a parallel manipulator defines a kinematic loop passing through the origins of frames \\(\\{\bm{A}\\}\\) and \\(\\{\bm{B}\\}\\), and through the two limb attachment points \\(A\_i\\) and \\(B\_i\\).

At the displacement level, the **closure of each kinematic loop** can be express in the vector form as
\\[ \vec{AB} = \vec{AA\_i} + \vec{A\_iB\_i} - \vec{BB\_i} \quad \text{for } i = 1,2,\dots,n \\]
in which \\(\vec{AA\_i}\\) and \\(\vec{BB\_i}\\) can be easily obtained from the geometry of the attachment points in the base and in the moving platform.

Let us defined \\(\bm{a}\_i = \vec{AA\_i}\\) in the fixed frame \\(\\{\bm{A}\\}\\), and \\(\bm{b}\_i = \vec{BB\_i}\\) in the moving frame \\(\\{\bm{B}\\}\\).
Furthermore, \\(\bm{q}\_i = \vec{A\_iB\_i}\\) is defined as the **limb variable**, which indicated the geometry of the limb.

<div class="cbox">
  <div></div>

The **loop closure** can be written as the unknown pose variables \\(\bm{p}\\) and \\(\bm{R}\\), the position vectors describing the known geometry of the base and of the moving platform, \\(\bm{a}\_i\\) and \\(\bm{b}\_i\\), and the limb vector \\(\bm{q}\_i\\)

\begin{equation}
  \bm{p} = \bm{a}\_i + \bm{q}\_i - \bm{R} \bm{b}\_i \quad \text{for } i=1,2,\dots,n
\end{equation}

</div>

For an **inverse kinematic problem**, it is assumed that the moving platform position \\(\bm{p}\\) and orientation \\(\bm{R}\\) are given and the problem is to solve the active limb variables.
This analysis is usually straightforward and results in unique solution for the limb variables.

However, the inverse solution is not straightforward, and usually numerical methods are used for forward kinematic solution.


### Kinematic Analysis of a Stewart-Gough Platform {#kinematic-analysis-of-a-stewart-gough-platform}


#### Mechanism Description {#mechanism-description}

One frame \\(\\{\bm{A}\\}\\) is attached to the fixed base and frame \\(\\{\bm{B}\\}\\) is attached to the moving platform at points \\(O\_A\\) and \\(O\_B\\) respectively.

The number of actuators is equal to the degrees-of-freedom of the manipulator and hence the manipulator is **fully parallel**.

<div class="cbox">
  <div></div>

Since all the limbs are connected to the moving platform and to the base by spherical joints, **no twisting torque** can be transmitted and the force acting on each limb is directed along the longitudinal axis of the limb.

</div>


#### Geometry of the Manipulator {#geometry-of-the-manipulator}

<div class="cbox">
  <div></div>

The position of the attachment points on the fixed base are denoted by the vectors \\(\bm{a}\_i\\) and the position of moving attachment points are denoted by the vectors \\(\bm{b}\_i\\).
The geometry of each limb is described by its length \\(l\_i\\) and its direction is denoted by a unit vector \\(\hat{\bm{s}}\_i\\).

</div>

The position of the point \\(O\_B\\) of the moving platform is described by the **position vector** \\({}^A\bm{P} = [p\_x\ p\_y\ p\_z]^T\\) and orientation of the moving platform is described by the **rotation matrix** \\({}^A\bm{R}\_B\\) which can by represented by the components of the unit vectors \\(\hat{\bm{u}}\\), \\(\hat{\bm{v}}\\), \\(\hat{\bm{z}}\\) as follows:

\begin{equation}
  ^A\bm{R}\_B = \begin{bmatrix}
    u\_x & v\_x & w\_x \\\\\\
    u\_y & v\_y & w\_y \\\\\\
    u\_z & v\_z & w\_z \\\\\\
  \end{bmatrix}
\end{equation}

<a id="org10ade7e"></a>

{{< figure src="/ox-hugo/taghirad13_stewart_schematic.png" caption="Figure 6: Geometry of a Stewart-Gough platform" >}}

The geometry of the manipulator is shown Figure [fig:stewart_schematic](#fig:stewart_schematic).


#### Inverse Kinematics {#inverse-kinematics}

<div class="cbox">
  <div></div>

For **inverse kinematic analysis**, it is assumed that the position \\({}^A\bm{P}\\) and orientation of the moving platform \\({}^A\bm{R}\_B\\) are given and the problem is to obtain the joint variables \\(\bm{L} = \left[ l\_1, l\_2, l\_3, l\_4, l\_5, l\_6 \right]^T\\).

</div>

From the geometry of the manipulator, one can write:

\begin{equation}
  {}^A \bm{a}\_i + l\_i {}^A \hat{\bm{s}}\_i = {}^A\bm{P} + {}^A\bm{b}\_i
\end{equation}

Then, we can find \\(l\_i\\) given \\({}^A\bm{P}\\) and \\({}^A\bm{R}\_B\\):

\begin{equation}
  \begin{aligned}
    l\_i = &\Big[ {}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}\_i^T {}^B\bm{b}\_i + {}^A\bm{a}\_i^T {}^A\bm{a}\_i - 2 {}^A\bm{P}^T {}^A\bm{a}\_i + \dots\\\\\\
          &2 {}^A\bm{P}^T \left[{}^A\bm{R}\_B {}^B\bm{b}\_i\right] - 2 \left[{}^A\bm{R}\_B {}^B\bm{b}\_i\right]^T {}^A\bm{a}\_i \Big]^{1/2}
  \end{aligned}
\end{equation}

If the position and orientation of the platform lie in the feasible workspace, the solution is unique.
Otherwise, the solution gives complex numbers.


#### Forward Kinematics {#forward-kinematics}

<div class="cbox">
  <div></div>

In **forward kinematic analysis**, it is assumed that the vector of limb lengths \\(\bm{L}\\) is given and the problem is to find the position \\({}^A\bm{P}\\) and the orientation \\({}^A\bm{R}\_B\\).

</div>

The size of the problem depends of the representation used for orientation (rotation matrix, Euler angles, ...).

The forward kinematic problem is then to solve many **highly nonlinear equations** that are extremely difficult to solve.

The complexity of the problem depends widely on the manipulator architecture and geometry.


## Jacobian: Velocities and Static Forces {#jacobian-velocities-and-static-forces}

<a id="org150c88f"></a>


### Introduction {#introduction}

<div class="cbox">
  <div></div>

The Jacobian matrix not only reveals the **relation between the joint variable velocities of a parallel manipulator to the moving platform linear and angular velocities**, it also constructs the transformation needed to find the **actuator forces from the forces and moments acting on the moving platform**.

</div>

For specific configurations, **local degeneracy** can occur and leads to:

1.  An instantaneous change in the degrees-of-freedom of the system and hence a **loss of controllability**
2.  An important **degradation of the natural stiffness** that may lead to very high joint forces or torques

Therefore, it is very important to **identify singular configurations** at the design stage to improve the performance.


### Angular and Linear Velocities {#angular-and-linear-velocities}

To determine the absolute linear velocity of a point, the derivative must be calculated relative to a **fixed** frame.
Differentiation of a position vector with respect to a moving frame results in a relative velocity.
Therefore, it is necessary to define the arithmetics to transform the relative velocities to the absolute ones.


#### Angular Velocity of a Rigid Body {#angular-velocity-of-a-rigid-body}

Angular velocity is an attribute of a rigid body that describes the rotational motion of the frame \\(\\{\bm{B}\\}\\) that is attached to the rigid body.

<div class="cbox">
  <div></div>

The **angular velocity vector** \\(\bm{\Omega}\\) describes the instantaneous rotation of frame \\(\\{\bm{B}\\}\\) with respect to the fixed frame \\(\\{\bm{A}\\}\\).
The direction of \\(\bm{\Omega}\\) indicates the instantaneous axis of rotation and its magnitude indicates the speed of rotation.

</div>

The angular velocity vector is related to the screw formalism by equation \eqref{eq:angular_velocity_vector}.

\begin{equation}
  \tcmbox{\bm{\Omega} \triangleq \dot{\theta} \hat{\bm{s}}}
\end{equation}

The angular velocity can be expressed in any frame. For example \\({}^A\bm{\Omega}\\) denotes the angular velocity of the rigid body expressed in the frame \\(\\{\bm{A}\\}\\) and we have:

\begin{equation}
  \begin{aligned}
    ^A \bm{\Omega} &= \Omega\_x \hat{\bm{x}} + \Omega\_y \hat{\bm{y}} + \Omega\_z \hat{\bm{z}} \\\\\\
                   &= \dot{\theta}\left( s\_x \hat{\bm{x}} + s\_y \hat{\bm{y}} + s\_z \hat{\bm{z}} \right)
  \end{aligned}
\end{equation}

in which \\(\Omega\_x\\), \\(\Omega\_y\\) and \\(\Omega\_z\\) are the three components of angular velocity of a rigid body expressed in frame \\(\\{\bm{A}\\}\\).


#### Linear Velocity of a Point {#linear-velocity-of-a-point}

Linear velocity of a point P can be easily determined by the time derivative of the position vector \\(p\\) of that point with respect to a fixed frame:

\begin{equation}
  v\_p = \dot{p} = \left( \dv{p}{t} \right)\_{\text{fix}}
\end{equation}

If the variation of the position vector is determined with respect to a moving frame, we obtain the relative velocity:

\begin{equation}
  v\_{\text{rel}} = \left( \pdv{p}{t} \right)\_{\text{mov}}
\end{equation}

In classical mechanics, it is shown that the relation between absolute derivative of any vector to its relative derivative is given by:

\begin{equation}
  \left( \dv{(\cdot)}{t} \right)\_{\text{fix}} = \left( \pdv{(\cdot)}{t} \right)\_{\text{mov}} + \bm{\Omega} \times (\cdot)
\end{equation}

in which \\(\bm{\Omega}\\) denotes the angular velocity of the moving frame with respect to the fixed frame.

The term \\(\bm{\Omega}\times(\cdot)\\) can be written in matrix form:

\begin{equation}
  \tcmbox{\left( \dv{(\cdot)}{t} \right)\_{\text{fix}} = \left( \pdv{(\cdot)}{t} \right)\_{\text{mov}} + \bm{\Omega}^\times(\cdot)}
\end{equation}

The matrix \\(\bm{\Omega}^\times\\) denotes a **skew-symmetric matrix** defined by:

\begin{equation}
  \tcmbox{\bm{\Omega}^\times = \begin{bmatrix}
    0         & -\Omega\_z & \Omega\_y  \\\\\\
    \Omega\_z  & 0         & -\Omega\_x \\\\\\
    -\Omega\_y & \Omega\_x  & 0
  \end{bmatrix}}
\end{equation}

Now consider the general motion of a rigid body shown in Figure [fig:general_motion](#fig:general_motion), in which a moving frame \\(\\{\bm{B}\\}\\) is attached to the rigid body and **the problem is to find the absolute velocity** of point \\(P\\) with respect to a fixed frame \\(\\{\bm{A}\\}\\).

<a id="org29fa342"></a>

{{< figure src="/ox-hugo/taghirad13_general_motion.png" caption="Figure 7: Instantaneous velocity of a point \\(P\\) with respect to a moving frame \\(\\{\bm{B}\\}\\)" >}}

The rigid body perform a general motion which is a combination of a translation, denoted by the vector \\({}^A\bm{P}\_{O\_B}\\), and an instantaneous rotation.
To determine the velocity of point \\(P\\), we start with the relation between absolute and relative position vectors:
\\[ ^A\bm{P} = {}^A\bm{P}\_{O\_B} + {}^A\bm{R}\_B {}^B\bm{P} \\]

To derive the velocity of point \\(P\\), we differentiate with respect to time:
\\[ {}^A\bm{v}\_P = {}^A\bm{v}\_{O\_B} + {}^A\dot{\bm{R}}\_B{}^B\bm{P} + {}^A\bm{R}\_B\underbrace{{}^B\bm{v}\_P}\_{=0} \\]

The time derivative of the rotation matrix \\({}^A\dot{\bm{R}}\_B\\) is:

\begin{equation}
  \tcmbox{{}^A\dot{\bm{R}}\_B = {}^A\bm{\Omega}^\times \ {}^A\bm{R}\_B}
\end{equation}

And we finally obtain equation \eqref{eq:absolute_velocity_formula}.

<div class="cbox">
  <div></div>

\begin{equation}
  {}^A\bm{v}\_P = {}^A\bm{v}\_{O\_B} + {}^A\bm{\Omega}^\times \  {}^A\bm{R}\_B {}^B\bm{P}
\end{equation}

</div>


#### Screw Coordinates {#screw-coordinates}

Finite rotation of a rigid body can be expressed as a rotation \\(\theta\\) about a screw axis \\(\hat{\bm{s}}\\).
Furthermore, it is shown that the angular velocity of a rigid body is also defined as the rate of instantaneous rotation angle \\(\dot{\theta}\\) about the same screw axis \\(\hat{\bm{s}}\\).

<div class="cbox">
  <div></div>

The most general rigid-body displacement can be produced by a translation along a line followed by a rotation about the same line. Since this displacement is reminiscent of the displacement of a screw, it is called a **screw displacement**, and the line of axis is called the **screw axis**.

</div>


### Jacobian Matrices of a Parallel Manipulator {#jacobian-matrices-of-a-parallel-manipulator}

Let \\(\bm{q} = \left[ q\_1, q\_2, \ldots, q\_m \right]^T\\) denote the vector of actuated joint coordinates (linear displacement of an actuator prismatic joint or angular rotation of an actuated revolute joint) and \\(\bm{\mathcal{X}} = \left[ x\_1, x\_2, \ldots, x\_n \right]^T\\) denote the vector of moving platform motion variables (position or orientation).

\\(m\\) denotes the number of actuated joints in the manipulator, \\(n\\) denotes the number of degrees-of-freedom of the manipulator.

Generally \\(m \geq n\\), in which for a fully parallel manipulator \\(m=n\\) and for redundant manipulator \\(m>n\\).<br />

\\(\bm{q}\\) and \\(\bm{\mathcal{X}}\\) are related through a system of **nonlinear algebraic equations** representing the **kinematic constraints imposed by the limbs**, which can be generally written as

\begin{equation}
  f(\bm{q}, \bm{\mathcal{X}}) = 0
\end{equation}

We can differentiate this equation with respect to time and obtain:

\begin{equation}
  \tcmbox{\bm{J}\_x \dot{\bm{\mathcal{X}}} = \bm{J}\_q \dot{\bm{q}}}
\end{equation}

where

\begin{equation}
  \bm{J}\_x = \pdv{f}{\bm{\mathcal{X}}} \quad \text{and} \quad \bm{J}\_q = -\pdv{f}{\bm{q}}
\end{equation}

<div class="cbox">
  <div></div>

The **general Jacobian matrix** is defined as:

\begin{equation}
  \dot{\bm{q}} = \bm{J} \dot{\bm{\mathcal{X}}}
\end{equation}

From equation \eqref{eq:jacobians}, we have:

\begin{equation}
  \bm{J} = {\bm{J}\_q}^{-1} \bm{J}\_x
\end{equation}

</div>


### Velocity Loop Closure {#velocity-loop-closure}

The **velocity loop closures** are used for **obtaining the Jacobian matrices** in a straightforward manner.
Velocity loop closures are derived by direct differentiation of kinematic loop closures.<br />

Kinematic loop closures are:
\\[ \bm{p} = \bm{a}\_i + \bm{d}\_i - \bm{R} \bm{b}\_i \quad \text{for}\ i = 1, \ldots, m \\]

with

-   \\(\bm{p}\\) the position vector of the moving platform w.r.t. frame \\(\\{\bm{A}\\}\\)
-   \\(\bm{R}\\) the rotation matrix of the moving platform
-   \\(\bm{a}\_i\\) the position vector of the \\(i\\)'th limb of the fixed platform w.r.t. frame \\(\\{\bm{A}\\}\\)
-   \\(\bm{b}\_i\\) the position vector of the \\(i\\)'th limb of the moving platform w.r.t. frame \\(\\{\bm{B}\\}\\)
-   \\(\bm{d}\_i\\) the limb vector

By taking the time derivative, we obtain the following **Velocity Loop Closure**:

\begin{equation}
  \tcmbox{\dot{\bm{p}} = \dot{\bm{d}\_i} - \bm{\omega} \times \bm{R} \bm{b}\_i \quad \text{for}\ i = 1, \ldots, m}
\end{equation}


### Singularity Analysis of Parallel Manipulators {#singularity-analysis-of-parallel-manipulators}

The singularities occur when:

-   \\(\bm{J}\_q\\) is rank deficient (Inverse kinematic singularity)
-   \\(\bm{J}\_x\\) is rank deficient (Forward kinematic singularity)


#### Inverse Kinematic Singularity {#inverse-kinematic-singularity}

Inverse kinematic singularity happens when \\(\bm{J}\_q\\) (\\(m \times m\\) matrix) is rank deficient (\\(\det \bm{J}\_q = 0\\)).

The corresponding configurations are located at the boundary of the manipulator workspace or on the internal boundaries between sub-regions of the workspace.

In such cases, there exist nonzero vectors \\(\dot{\bm{q}}\\) which correspond to a null Cartesian twist vector \\(\dot{\bm{\mathcal{X}}}\\). In other words, infinitesimal motion of the moving platform along certain directions cannot be accomplished.
**The manipulator looses one or more degrees-of-freedom**.


#### Forward Kinematic Singularity {#forward-kinematic-singularity}

Forward kinematic singularity happens when \\(\bm{J}\_x\\) (\\(m \times n\\) matrix) is rank deficient (\\(\det({\bm{J}\_x}^T \bm{J}\_x) = 0\\)).
If the manipulator is not redundantly actuated (\\(m=n\\)), then the Jacobian matrix \\(\bm{J}\_x\\) is square and the forward kinematic singularity happens when \\(\det \bm{J}\_x  = 0\\).<br />

The degeneracy occur inside the manipulator's Cartesian workspace and corresponds to the set of configurations for which two different branches of forward kinematic problem meet.<br />

There exist nonzero cartesian twist vectors \\(\dot{\bm{\mathcal{X}}}\\) that are mapped into a vanishing actuator velocity vector \\(\dot{\bm{\mathcal{q}}}\\).

The corresponding configuration will be one in which an infinitesimal motion of the platform is possible even if the actuator are locked. **The manipulator gains one or several degrees-of-freedom and its stiffness vanishes in the corresponding direction(s)**.


### Jacobian Analysis of the Stewart-Gough Platform {#jacobian-analysis-of-the-stewart-gough-platform}


#### Velocity Loop Closure {#velocity-loop-closure}

The input joint rate is denoted by \\(\dot{\bm{\mathcal{L}}} = [ \dot{l}\_1, \dot{l}\_2, \dot{l}\_3, \dot{l}\_4, \dot{l}\_5, \dot{l}\_6 ]^T\\), and the output twist vector is denoted by \\(\dot{\bm{\mathcal{X}}} =&nbsp;[^A\bm{v}\_p, {}^A\bm{\omega}]^T\\).<br />

The jacobian matrix can be derived by formulating a velocity loop closure equation of each limb.
The loop closure equations for each limb are:

\begin{equation}
  {}^A\bm{P} + {}^A\bm{R}\_B {}^B\bm{b}\_i = l\_i {}^A\hat{\bm{s}}\_i + {}^A\bm{a}\_i
\end{equation}

By differentiate this with respect to time:

\begin{equation}
  {}^A\bm{v}\_p + {}^A \dot{\bm{R}}\_B {}^B\bm{b}\_i = \dot{l}\_i {}^A\hat{\bm{s}}\_i + l\_i {}^A\dot{\hat{\bm{s}}}\_i
\end{equation}

Moreover, we have:

-   \\({}^A\dot{\bm{R}}\_B {}^B\bm{b}\_i = {}^A\bm{\omega} \times {}^A\bm{R}\_B {}^B\bm{b}\_i = {}^A\bm{\omega} \times {}^A\bm{b}\_i\\) in which \\({}^A\bm{\omega}\\) denotes the angular velocity of the moving platform expressed in the fixed frame \\(\\{\bm{A}\\}\\).
-   \\(l\_i {}^A\dot{\hat{\bm{s}}}\_i = l\_i \left( {}^A\bm{\omega}\_i \times \hat{\bm{s}}\_i \right)\\) in which \\({}^A\bm{\omega}\_i\\) is the angular velocity of limb \\(i\\) express in fixed frame \\(\\{\bm{A}\\}\\).

Then, the velocity loop closure \eqref{eq:loop_closure_limb_diff} simplifies to
\\[ {}^A\bm{v}\_p + {}^A\bm{\omega} \times {}^A\bm{b}\_i = \dot{l}\_i {}^A\hat{\bm{s}}\_i + l\_i ({}^A\bm{\omega}\_i \times \hat{\bm{s}}\_i) \\]

By dot multiply both side of the equation by \\(\hat{\bm{s}}\_i\\):
\\[ \hat{\bm{s}}\_i {}^A\bm{v}\_p + ({}^A\bm{b}\_i \times \hat{\bm{s}}\_i) {}^A\bm{\omega} = \dot{l}\_i \\]

We then omit the superscript \\(A\\) and we can rearrange the 6 equations into a matrix form

\begin{equation}
  \tcmbox{\dot{\bm{\mathcal{L}}} = \bm{J} \dot{\bm{\mathcal{X}}}}
\end{equation}

<div class="cbox">
  <div></div>

\begin{equation}
  \bm{J} = \begin{bmatrix}
    {\hat{\bm{s}}\_1}^T & (\bm{b}\_1 \times \hat{\bm{s}}\_1)^T \\\\\\
    {\hat{\bm{s}}\_2}^T & (\bm{b}\_2 \times \hat{\bm{s}}\_2)^T \\\\\\
    {\hat{\bm{s}}\_3}^T & (\bm{b}\_3 \times \hat{\bm{s}}\_3)^T \\\\\\
    {\hat{\bm{s}}\_4}^T & (\bm{b}\_4 \times \hat{\bm{s}}\_4)^T \\\\\\
    {\hat{\bm{s}}\_5}^T & (\bm{b}\_5 \times \hat{\bm{s}}\_5)^T \\\\\\
    {\hat{\bm{s}}\_6}^T & (\bm{b}\_6 \times \hat{\bm{s}}\_6)^T
  \end{bmatrix}
\end{equation}

\\(\bm{J}\\) then **depends only** on:

-   \\(\hat{\bm{s}}\_i\\) the orientation of the limbs
-   \\(\bm{b}\_i\\) the position of the joints with respect to \\(O\_B\\) and express in \\(\\{\bm{A}\\}\\).

</div>


#### Singularity Analysis {#singularity-analysis}

It is of primary importance to avoid singularities in a given workspace.
To study the singularity configurations of the Stewart-Gough platform, we consider the Jacobian matrix determined with the equation \eqref{eq:jacobian_formula_stewart}.<br />

From equation \eqref{eq:jacobians}, it is clear that for the Stewart-Gough platform, \\(\bm{J}\_q = \bm{I}\\) and \\(\bm{J}\_x = \bm{J}\\).
Hence the manipulator has **no inverse kinematic singularities** within the manipulator workspace, but **may possess forward kinematic singularity** when \\(\bm{J}\\) becomes rank deficient. This may occur when
\\[ \det \bm{J} = 0 \\]


### Static Forces in Parallel Manipulators {#static-forces-in-parallel-manipulators}

The relation between the forces/moments applied to the environment at the point of contact and the actuator forces/torques is determined and analyzed in the study of static force analysis.<br />

It is assumed that the manipulator is at a static equilibrium, and that the actuator forces required to produce the desired contact forces are determined.<br />

Two methods are usually applied: the **free-body diagram** and the **principle of virtual work**.<br />

In the **free-body diagram approach**, the actuator forces are determined to produce desired contact forces/moments as well as the internal and interacting forces/torques applied at the limbs. The analysis of such forces is essential in the design of a manipulator to determine the stresses and deflection of each link and joint.<br />

However, if only the actuator forces are desired to be determined, the **principle of virtual work** is more efficient and computationally less expensive.


#### Virtual Work Approach {#virtual-work-approach}

A virtual displacement for a parallel manipulator refers to an infinitesimal change in the general displacement of the moving platform as a result of any arbitrary infinitesimal changes in the joint variables at a given instant of time.<br />

The virtual displacement of the joints can be written as \\(\delta \bm{q} = [\delta q\_1, \delta q\_2, \cdots, \delta q\_m]^T\\) and \\(\delta \bm{\mathcal{X}} = [\delta x, \delta y, \delta z, \delta \theta\_x, \delta \theta\_y, \delta \theta\_z ]^T\\) denotes the virtual displacement of a contacting point of the moving platform.
\\([\delta \theta\_x, \delta \theta\_y, \delta \theta\_z ]^T = \delta \theta \hat{\bm{s}}\\) are the orientation variables represented by screw coordinates.<br />

Let the vector of actuator forces be denoted by \\(\bm{\tau} = [\tau\_1, \tau\_2, \cdots, \tau\_m]^T\\), and the external forces/torque acting on the contact point of the moving platform denoted by a wrench in a screw coordinate as \\(\bm{\mathcal{F}} = [\bm{f}, \bm{n}]^T\\) in which \\(\bm{f} = [f\_x, f\_y, f\_z]^T\\) denotes the external forces, and \\(\bm{n} = [n\_x, n\_y, n\_z]^T\\) denotes the external torque action on the moving platform at the point of contact to the environment.<br />

We assume that the frictional forces acting on the joints are negligible, and also that the gravitational forces of the limb links are much smaller than the interacting force of the moving platform to the environment.
The principle of virtual work states that the total virtual work, \\(\delta W\\), done by all actuators and external forces is equal to zero:

\begin{equation}
  \tcmbox{\delta W = \bm{\tau}^T \delta \bm{q} - \bm{\mathcal{F}}^T \delta \bm{\mathcal{X}} = 0}
\end{equation}

Furthermore, from the definition of the Jacobian, the virtual displacements \\(\delta \bm{q}\\) and \\(\delta \bm{\mathcal{X}}\\) are related by the Jacobian:
\\[ \delta \bm{q} = \bm{J} \cdot \delta \bm{\mathcal{X}} \\]

We then have \\(\left( \bm{\tau}^T \bm{J} - \bm{\mathcal{F}}^T \right) \delta \bm{\mathcal{X}} = 0\\) that holds for any arbitrary virtual displacement \\(\delta \bm{\mathcal{X}}\\), hence
\\[ \bm{\tau}^T \bm{J} - \bm{\mathcal{F}}^T = 0 \\]

<div class="cbox">
  <div></div>

We obtain that the **Jacobian matrix** constructs the **transformation needed to find the actuator forces** \\(\bm{\tau}\\) **from the wrench acting on the moving platform** \\(\bm{\mathcal{F}}\\):

\begin{equation}
  \bm{\mathcal{F}} = \bm{J}^T \bm{\tau}
\end{equation}

</div>


#### Static Forces of the Stewart-Gough Platform {#static-forces-of-the-stewart-gough-platform}

As shown in Figure [fig:stewart_static_forces](#fig:stewart_static_forces), the twist of moving platform is described by a 6D vector \\(\dot{\bm{\mathcal{X}}} = \left[ {}^A\bm{v}\_P \ {}^A\bm{\omega} \right]^T\\), in which \\({}^A\bm{v}\_P\\) is the velocity of point \\(O\_B\\), and \\({}^A\bm{\omega}\\) is the angular velocity of moving platform.<br />

<a id="org55b47c8"></a>

{{< figure src="/ox-hugo/taghirad13_stewart_static_forces.png" caption="Figure 8: Free-body diagram of forces and moments action on the moving platform and each limb of the Stewart-Gough platform" >}}

Consider an external wrench generated by the manipulator and applied to the environment at the point \\(O\_B\\) denoted by \\(\bm{\mathcal{F}} = [\bm{f} \ \bm{n}]^T\\).

It is assumed that no external forces are applied to the limbs except the actuator forces.
Therefore, the static force can be assumed to be **along the limb axis** \\(\hat{\bm{s}}\_i\\), and the limb is subject to a tension/compression force \\(f\_i\\).

At static equilibrium, the summation of all acting forces on the moving platform shall be zero, therefore

\begin{equation}
  -\bm{f} + \sum\_{i=1}^6 f\_i \hat{\bm{s}}\_i = 0
\end{equation}

in which \\(-\bm{f}\\) if the **external force** applied to the moving platform from the environment.<br />

The summation of moments contributed by all forces acting on the moving platform about \\(O\_B\\) is as follows:

\begin{equation}
  -\bm{n} + \sum\_{i=1}^6 b\_i \times f\_i \hat{\bm{s}}\_i = 0
\end{equation}

in which \\(-n\\) is the **external moment** applied to the moving platform by the environment, and \\(b\_i\\) is the position vector from the point \\(O\_B\\) to the attached point \\(B\_i\\) on the moving platform.<br />

Writing the two equations together in a matrix form results in

\begin{equation}
  \begin{bmatrix}
    \hat{\bm{s}}\_1                 & \hat{\bm{s}}\_2                 & \cdots & \hat{\bm{s}}\_6 \\\\\\
    \bm{b}\_1 \times \hat{\bm{s}}\_1 & \bm{b}\_2 \times \hat{\bm{s}}\_2 & \cdots & \bm{b}\_6 \times \hat{\bm{s}}\_6
    \end{bmatrix} \cdot \begin{bmatrix}
    f\_1 \\ f\_2 \\ \vdots \\ f\_6
    \end{bmatrix} = \begin{bmatrix}
    \bm{f} \\ \bm{n}
  \end{bmatrix}
\end{equation}

There we can recognize the transpose of the Jacobian matrix:

\begin{equation}
  \tcmbox{\bm{\mathcal{F}} = \bm{J}^T \bm{\tau}}
\end{equation}

in which \\(\bm{\tau} = [f\_1, f\_2, \cdots, f\_6]^T\\) is the vector of actuator forces, and \\(\bm{\mathcal{F}} = [\bm{f}, \bm{n}]^T\\) is the 6D wrench applied by the manipulator to the environment.


### Stiffness Analysis of Parallel Manipulators {#stiffness-analysis-of-parallel-manipulators}

Here, we focus on the deflections of the manipulator moving platform that are the result of the applied wrench to the environment.
The amount of these deflections are a function of the applied wrench as well as the manipulator **structural stiffness**.
Thus, the stiffness of a manipulator has a direct impact on its overall positioning accuracy if the manipulator is in contact with a stiff environment.


#### Stiffness and Compliance Matrices {#stiffness-and-compliance-matrices}

The relation between the applied actuator force \\(\tau\_i\\) and the corresponding small deflection \\(\Delta q\_i\\) along the applied force axis can be approximated as a **linear function**:

\begin{equation}
  \tcmbox{\tau\_i = k\_i \cdot \Delta q\_i}
\end{equation}

in which \\(k\_i\\) denotes the **stiffness constant of the actuator**.<br />

Re-writing the equation \eqref{eq:stiffness_actuator} for all limbs in a matrix form result in

\begin{equation}
  \tcmbox{\bm{\tau} = \mathcal{K} \cdot \Delta \bm{q}}
\end{equation}

in which \\(\bm{\tau}\\) is the vector of actuator forces, and \\(\Delta \bm{q}\\) corresponds to the actuator deflections.
\\(\mathcal{K} = \text{diag}\left[ k\_1 \ k\_2 \dots k\_m \right]\\) is an \\(m \times m\\) diagonal matrix composed of the actuator stiffness constants.<br />

Writing the Jacobian relation given in equation \eqref{eq:jacobian_disp} for infinitesimal deflection read

\begin{equation}
  \Delta \bm{q} = \bm{J} \cdot \Delta \bm{\mathcal{X}}
\end{equation}

in which \\(\Delta \bm{\mathcal{X}} = [\Delta x\ \Delta y\ \Delta z\ \Delta\theta x\ \Delta\theta y\ \Delta\theta z]\\) is the infinitesimal linear and angular deflection of the moving platform.

Furthermore, rewriting the Jacobian as the projection of actuator forces to the moving platform \eqref{eq:jacobian_forces} gives

\begin{equation}
  \bm{\mathcal{F}} = \bm{J}^T \bm{\tau}
\end{equation}

Hence, by substituting \eqref{eq:stiffness_matrix_relation} and \eqref{eq:jacobian_disp_inf} in \eqref{eq:jacobian_force_inf}, we obtain:

\begin{equation}
  \tcmbox{\bm{\mathcal{F}} = \underbrace{\bm{J}^T \mathcal{K} \bm{J}}\_{\bm{K}} \cdot \Delta \bm{\mathcal{X}}}
\end{equation}

Equation \eqref{eq:stiffness_jacobian} implies that the moving platform output wrench is related to its deflection by the **stiffness matrix** \\(K\\).

<div class="cbox">
  <div></div>

\begin{equation}
  \bm{K} = \bm{J}^T \mathcal{K} \bm{J}
\end{equation}

</div>

The stiffness matrix has desirable characteristics for analysis:

-   It is a **symmetric positive definite matrix**, however, it is configuration dependent
-   If the manipulator actuators have all the same stiffness constants \\(k\\), the stiffness matrix is reduced to the form \\(\bm{K} = k \bm{J}^T \bm{J}\\)

If the stiffness matrix is inversible (\\(\det( \bm{J}^T \bm{J}) \ne 0\\)), the **compliance matrix** of the manipulator is defined as

\begin{equation}
  \tcmbox{\bm{C} = \bm{K}^{-1} = (\bm{J}^T \mathcal{K} \bm{J})^{-1}}
\end{equation}

The compliance matrix of a manipulator shows the mapping of the moving platform wrench to its deflection by

\begin{equation}
  \Delta \bm{\mathcal{X}} = \bm{C} \cdot \bm{\mathcal{F}}
\end{equation}


#### Transformation Ellipsoid {#transformation-ellipsoid}

As seen previously, the Jacobian matrix \\(\bm{J}\\) transforms n-dimensional moving platform velocity vector \\(\dot{\bm{\mathcal{X}}}\\) into m-dimensional actuated joint velocity \\(\dot{\bm{q}}\\).
Also, the Jacobian transpose \\(\bm{J}^T\\) maps m-dimensional actuated joint forces \\(\bm{\tau}\\) into n-dimensional applied wrench \\(\bm{\mathcal{F}}\\).<br />

One way to **characterize these transformation** is to compare the amplitude and direction of the moving platform velocity generated by a **unit** actuator joint velocity.
To achieve this goal, we confine the actuator joint velocity vector on a m-dimensional unit sphere
\\[ \dot{\bm{q}}^T \dot{\bm{q}} = 1 \\]
and compare the resulting moving platform velocity in n-dimensional space:
\\[ \dot{\bm{\mathcal{X}}}^T \bm{J}^T \bm{J} \dot{\bm{\mathcal{X}}} = 1 \\]

Similarly, we can confine the exerted moving platform wrench \\(\bm{\mathcal{F}}^T \bm{\mathcal{F}} = 1\\) and compare the required actuator forces: \\(\bm{\tau}^T \bm{J} \bm{J}^T \bm{\tau} = 1\\).<br />

Consider the case of fully parallel manipulators.
Then \\(\bm{J}\bm{J}^T\\) and \\(\bm{J}^T\bm{J}\\) transformations are represented by \\(n \times n\\) matrices.
Geometrically, these transformations represent a **hyper-ellipsoid** in n-dimensional space, whose principal axes are the **eigenvectors** of \\(\bm{J}^T\bm{J}\\) and \\(\bm{J}\bm{J}^T\\) respectively.
Furthermore, the lengths of the principal axes are equal to the reciprocals of the square roots of the eigenvalues of \\(\bm{J}\bm{J}^T\\) and \\(\bm{J}^T\bm{J}\\), which are also equal to the reciprocals of the **singular values** of \\(\bm{J}\\).<br />

The shape of this hyper-ellipsoid in space indicates the characteristics of the transformation.
As this hyper-ellipsoid is closer to a hyper-sphere, the transformation becomes more uniform in different directions.
Since Jacobian matrix is **configuration dependent**, the shape of the hyper-ellipsoid is also configuration dependent, and as the moving platform moves from one pose to the other, the shape of the hyper-ellipsoid changes accordingly.<br />

A measure of the dexterity of the manipulator is the reciprocal of the Jacobian matrix condition number defined as:

\begin{equation}
  \frac{1}{\kappa} = \frac{\sigma\_{\text{min}}}{\sigma\_{\text{max}}}
\end{equation}

in which \\(\sigma\_{\text{min}}\\) and \\(\sigma\_{\text{max}}\\) are the smallest and the largest singular values of the Jacobian matrix.


#### Stiffness Analysis of the Stewart-Gough Platform {#stiffness-analysis-of-the-stewart-gough-platform}

In this section, we restrict our analysis to a 3-6 structure (Figure [fig:stewart36](#fig:stewart36)) in which there exist six distinct attachment points \\(A\_i\\) on the fixed base and three moving attachment point \\(B\_i\\).

<a id="org2921078"></a>

{{< figure src="/ox-hugo/taghirad13_stewart36.png" caption="Figure 9: Schematic of a 3-6 Stewart-Gough platform" >}}

Denote the vector of actuated joint forces by  \\(\bm{\tau} = [f\_1 \ f\_2 \ f\_3 \ f\_4 \ f\_5 \ f\_6 ]\\), and the corresponding vector of infinitesimal displacements of actuated limbs denoted by \\(\Delta \bm{L} = [\Delta l\_1 \ \Delta l\_2 \ \Delta l\_3 \ \Delta l\_4 \ \Delta l\_5 \ \Delta l\_6 ]\\).
The relation between \\(\Delta \bm{L}\\) and \\(\bm{\tau}\\) is described by a diagonal \\(6 \times 6\\) matrix \\(\mathcal{K}\\):
\\[ \bm{\tau} = \mathcal{K} \cdot \Delta \bm{L} \\]

Also, from the definition of the Jacobian, we have:
\\[ \Delta \bm{L} = \bm{J} \cdot \Delta \bm{\mathcal{X}} \\]
in which \\(\Delta \bm{\mathcal{X}} = [\Delta\_x \ \Delta\_y \ \Delta\_z \ \Delta \theta\_x \ \Delta \theta\_y \ \Delta \theta\_z ]\\) is the vector of infinitesimal linear and angular motions of the moving platform.

Also, the vector of the moving platform output wrench denoted by \\(\bm{F} = [f\_x \ f\_y \ f\_z \ n\_x \ n\_y \ n\_z ]\\) is related to the vector of actuated joint forces \\(\bm{\tau}\\) by:
\\[ \bm{F} = \bm{J}^T \cdot \bm{\tau} \\]

By substitution, we obtain:
\\[ \bm{\mathcal{F}} = \bm{K} \cdot \Delta \bm{\mathcal{X}} \\]
in which
\\[ \bm{K} = \bm{J}^T \mathcal{K} \bm{J} \\]
where \\(K\\) is called the **stiffness matrix** of the Stewart-Gough manipulator.<br />

For a given configuration of the moving platform, the eigenvalue of the stiffness matrix represents the stiffness of the manipulator in the corresponding eigenvector direction.
Furthermore, the reciprocal of the stiffness matrix condition number may be used to represent the dexterity of the manipulator.<br />

The maximum stiffness of the manipulator can be analyzed by the maximum singular value of the Jacobian matrix.
The largest axis of the stiffness transformation hyper-ellipsoid is given by this value at each configuration.


## Dynamics {#dynamics}

<a id="orgd3cd6ba"></a>


### Introduction {#introduction}

The dynamic analysis of parallel manipulators presents an inherent complexity due to their closed-loop structure.
Several approaches have been proposed.<br />

Traditional **Newton-Euler** formulation is used for dynamic analysis of general parallel manipulators.
In this formulation, the equation of motion of each limb and the moving platform must be derived, which inevitably leads to a large number of equations and less computational efficiency.
On the other hand, all the reaction forces can be computed, which is very useful in the design of a parallel manipulator.<br />

The **Lagrangian** formulation eliminates all the unwanted reaction forces at the outset, and therefore, is quite efficient.
However, because of the constraints imposed by the closed-loop structure, deriving explicit equations of motions in terms of a set of generalized coordinates becomes a prohibitive task.<br />

A third approach is to use the **principle of virtual work**, in which the computation of the constraint forces are bypassed.
In this method, inertial forces and moments are computed using linear and angular accelerations of the bodies.
Then, the whole manipulator is considered to be in static equilibrium by using the d'Alembert's principle, and the principle of virtual work is applied to derive the input forces and torques.<br />

Different objectives require different forms of formulations, there are three key issues pursued to derive dynamic formulation of parallel manipulators:

1.  **Calculation of internal forces** either active or passive for the design process of the manipulator
2.  **Study on dynamical properties** of the manipulator for controller design
3.  Utilization of dynamic specifications in an inverse dynamics controller or any **model-based control topology**

The first item is the main advantage of the Newton-Euler formulation, the second and third items are the benefits of using the Lagrange or virtual work approaches.<br />

The dynamic equations in an **explicit form** can be written as:

\begin{equation}
  \tcmbox{\bm{M}(\bm{\mathcal{X}}) \ddot{\bm{\mathcal{X}}} + \bm{C}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}}) \dot{\bm{\mathcal{X}}} + \bm{G}(\bm{\mathcal{X}}) = \bm{\mathcal{F}}}
\end{equation}

in which:

-   \\(\bm{\mathcal{X}}\\) is a vector of the generalized coordinates
-   \\(\bm{M}(\bm{\mathcal{X}})\\) denotes the system **mass matrix**
-   \\(\bm{C}(\bm{X},\dot{\bm{X}})\\) denotes the **Coriolis and centrifugal matrix**
-   \\(\bm{G}(\bm{X})\\) denotes the **gravity vector**
-   \\(\bm{\mathcal{F}}\\) denotes the generalized force

Deriving explicit dynamic equations for parallel manipulators is a very prohibitive task because of the closed-loop nature of the manipulator.


### Dynamics of the Rigid Bodies {#dynamics-of-the-rigid-bodies}


#### Acceleration of Rigid Bodies {#acceleration-of-rigid-bodies}

<div class="cbox">
  <div></div>

The acceleration analysis consists of studying the variations of linear velocity of a point and angular velocity of a rigid body with respect to time.

</div>

Direct differentiation of these vectors with respect to time in a **fixed** frame leads to linear velocity of a point and angular velocity of a rigid body, respectively.
Note that, to determine the absolute linear velocity of a point, the derivative must be calculated relative to a fixed frame.
In the study of robotic manipulators, usually **multiple moving frames** are defined to carefully determine the motion of the moving platform.
Therefore, it is necessary to define the required arithmetics to transform the relative accelerations into absolute ones.


##### Angular Acceleration of a Rigid Body {#angular-acceleration-of-a-rigid-body}

To define angular acceleration of a rigid body, consider a moving frame \\(\\{\bm{B}\\}\\) attached to the rigid body, and the motion analyzed with respect to a fixed frame.
Angular acceleration is an attribute of a rigid body and describes the variation of angular velocity of frame \\(\\{\bm{B}\\}\\) with respect to time.

<div class="cbox">
  <div></div>

**Angular acceleration vector**, denoted by the symbol \\(\dot{\bm{\Omega}}\\), describes the instantaneous change of the angular velocity of frame \\(\\{\bm{B}\\}\\), denoted by \\(\bm{\Omega}\\), with respect to the fixed frame \\(\\{\bm{A}\\}\\):

\begin{equation}
  \begin{aligned}
    \dot{\bm{\Omega}} = \frac{d \bm{\Omega}}{dt} &= \ddot{\theta} \hat{\bm{s}} + \dot{\theta} \dot{\hat{\bm{s}}} \\\\\\
                                       &= \ddot{\theta} \hat{\bm{s}} + \dot{\theta} (\bm{\Omega} \times \hat{\bm{s}}) \\\\\\
                                       &= \ddot{\theta} \hat{\bm{s}}
  \end{aligned}
\end{equation}

where \\(\\{\theta, \hat{\bm{s}}\\}\\) are the screw parameters representing the rotation of the rigid body.

</div>

As shown by \eqref{eq:angular_acceleration}, the angular acceleration of the rigid body is also along the screw axis \\(\hat{\bm{s}}\\) with a magnitude equal to \\(\ddot{\theta}\\).


##### Linear Acceleration of a Point {#linear-acceleration-of-a-point}

Linear acceleration of a point \\(P\\) can be easily determined by time derivative of the velocity vector \\(\bm{v}\_P\\) of that point with respect to a fixed frame:

\begin{equation}
  \bm{a}\_p = \dot{\bm{v}}\_p = \left( \frac{d\bm{v}\_p}{dt} \right)\_\text{fix}
\end{equation}

Note that this is correct only if the derivative is taken with respect to a **fixed** frame.<br />

Now consider the general motion of a rigid body, in which a moving frame \\(\\{\bm{B}\\}\\) is attached to the rigid body and the problem is to find the absolute acceleration of point \\(P\\) with respect to the fixed frame \\(\\{\bm{A}\\}\\).
The rigid body performs a general motion, which is a combination of a translation, denoted by the velocity vector \\({}^A\bm{v}\_{O\_B}\\), and an instantaneous angular rotation denoted by \\(\bm{\Omega}\\) (see Figure [fig:general_motion](#fig:general_motion)).
To determine acceleration of point \\(P\\), we start with the relation between absolute and relative velocities of point \\(P\\):

\begin{equation}
  {}^A\bm{v}\_P = {}^A\bm{v}\_{O\_B} + {}^A\bm{R}\_B{}^B\bm{v}\_P + {}^A\bm{\Omega}^\times {}^A\bm{R}\_B {}^B\bm{P}
\end{equation}

In order to derive acceleration of point \\(P\\), we differentiate both sides with respect to time and we obtain

\begin{equation}
  \begin{split}
    {}^A\bm{a}\_p\ &= {}^A\bm{a}\_{O\_B} \quad \text{(linear acc. of } \\{\bm{B}\\} \text{)} \\\\\\
             &+ {}^A\bm{R}\_B{}^B\bm{a}\_p \quad \text{(relative acc. of } P \text{ w.r.t. } \\{\bm{B}\\} \text{)} \\\\\\
             &+ {}^A\dot{\bm{\Omega}}^\times {}^A\bm{R}\_B {}^B\bm{P} \quad \text{(angular acceleration of } \\{\bm{B}\\} \text{)} \\\\\\
             &+ {}^A\bm{\Omega}^\times ({}^A\bm{\Omega}^\times {}^A\bm{R}\_B {}^B\bm{P}) \quad \text{(centrifugal)} \\\\\\
             &+ 2{}^A\bm{\Omega}^\times {}^A\bm{R}\_B {}^B\bm{v}\_P \quad \text{(Coriolis)}
  \end{split}
\end{equation}

For the case where \\(P\\) is a point embedded in the rigid body, \\({}^B\bm{v}\_P = 0\\) and \\({}^B\bm{a}\_P = 0\\) and we obtain:

\begin{equation}
  \begin{split}
  {}^A\bm{a}\_P = {}^A\bm{a}\_{O\_B} &+ {}^A\dot{\bm{\Omega}}^\times {}^A\bm{R}\_B {}^B\bm{P} \\\\\\
                                 &+ {}^A\bm{\Omega}^\times ({}^A\bm{\Omega}^\times {}^A\bm{R}\_B {}^B\bm{P})
  \end{split}
\end{equation}


#### Mass Properties {#mass-properties}

In this section, the properties of mass, namely **center of mass**, **moments of inertia** and its characteristics and the required transformations are described.

<a id="orgdfab15a"></a>

{{< figure src="/ox-hugo/taghirad13_mass_property_rigid_body.png" caption="Figure 10: Mass properties of a rigid body" >}}


##### Center of Mass {#center-of-mass}

Consider a reference frame \\(\\{\bm{A}\\}\\) in which the mass distribution of a material body is measured, and let \\(\bm{p}\\) denote the position vector of a differential mass \\(\rho dV\\) with respect to a reference frame.

The **center of mass** of a rigid body is defined as the point \\(C\\) which satisfied the following condition

\begin{equation}
  \bm{p}\_c = \frac{1}{m} \int\_V \bm{p} \rho dV
\end{equation}

in which the mass of the material body \\(\\{\bm{B}\\}\\) with density \\(\rho\\) and volume \\(V\\) is defined as

\begin{equation}
  m = \int\_V \rho dV
\end{equation}


##### Moments of Inertia {#moments-of-inertia}

As opposed to the mass, which introduces inertia to linear accelerations, moment of inertia is the property of mass which introduces **inertia to angular accelerations**.
Basically, for rotational motion, **the distribution of mass with respect to the axis of rotation introduces resistance to the angular acceleration**.<br />

Moments of inertia \\(\bm{I}\\) about \\(\bm{A}\\) is defined by the second moment of the mass with respect to a reference frame of rotation as:

\begin{equation}
  {}^A\bm{I} = \begin{bmatrix}
    I\_{XX} & I\_{XY} & I\_{XZ} \\\\\\
    I\_{YX} & I\_{YY} & I\_{YZ} \\\\\\
    I\_{ZX} & I\_{ZY} & I\_{ZZ}
  \end{bmatrix}
\end{equation}

in which

\begin{equation\*}
  \begin{aligned}
    I\_{XX} &= \int\_V (y^2 + z^2) \rho dV, \quad I\_{XY} = I\_{YX} = -\int\_V xy \rho dV \\\\\\
    I\_{YY} &= \int\_V (x^2 + z^2) \rho dV, \quad I\_{YZ} = I\_{ZY} = -\int\_V yz \rho dV \\\\\\
    I\_{ZZ} &= \int\_V (x^2 + y^2) \rho dV, \quad I\_{XZ} = I\_{ZX} = -\int\_V xz \rho dV
  \end{aligned}
\end{equation\*}


##### Principal Axes {#principal-axes}

As seen in equation \eqref{eq:moment_inertia}, the inertia matrix elements are a function of mass distribution of the rigid body with respect to the frame \\(\\{\bm{A}\\}\\).
Hence, it is possible to find **orientations of frame** \\(\\{\bm{A}\\}\\) in which the product of inertia terms vanish and inertia matrix becomes **diagonal**:

\begin{equation}
  {}^A\bm{I} = \begin{bmatrix}
    I\_{XX} & 0      & 0      \\\\\\
    0      & I\_{YY} & 0      \\\\\\
    0      & 0      & I\_{ZZ}
  \end{bmatrix}
\end{equation}

Such axes are called the **principal axes of inertia**, and diagonal terms are called the **principal moments of inertia**, which represent the maximum, minimum and intermediate values of the moments of inertia for a particular chosen origin \\(\\{\bm{A}\\}\\).

It can be shown that the principal moments of inertial and principal axes are invariant parameters and can be determined from an eigen value decomposition of the inertia matrix in any configuration of the reference frame \\(\\{\bm{A}\\}\\).


##### Inertia Matrix Transformations {#inertia-matrix-transformations}

The moment of inertia is usually given for **frames passing through the center of mass of the rigid body**.
**The inertia matrix changes under change of the reference frame**.<br />

Consider frame \\(\\{\bm{C}\\}\\) **parallel** to \\(\\{\bm{A}\\}\\) and attached to the center of mass of a rigid body and let \\(\bm{p}\_c = [x\_c, y\_c, z\_c]^T\\) denote the vector of the position of the center of mass with respect to frame \\(\\{\bm{A}\\}\\).
The relation between the inertia matrix about \\(A\\) and that about \\(C\\) is given by the following relation:

\begin{equation}
  \tcmbox{{}^A\bm{I} = {}^C\bm{I} + m(\bm{p}\_c^T \bm{p}\_c \bm{I}\_{3 \times 3} - \bm{p}\_c \bm{p}\_c^T)}
\end{equation}

in which \\(m\\) denotes the mass of the rigid body and \\(\bm{I}\_{3 \times 3}\\) denotes the identity matrix.<br />

On the other hand, if the reference frame \\(\\{B\\}\\) has **pure rotation** with respect to the frame attached to the center of mass \\(\\{A\\}\\):

\begin{equation}
  {}^A\bm{I} = {}^A\bm{R}\_C {}^C\bm{I} {}^A\bm{R}\_C^T
\end{equation}


#### Momentum and Kinetic Energy {#momentum-and-kinetic-energy}


##### Linear Momentum {#linear-momentum}

Linear momentum of a material body, shown in Figure [fig:angular_momentum_rigid_body](#fig:angular_momentum_rigid_body), with respect to a reference frame \\(\\{\bm{A}\\}\\) is defined as

\begin{equation}
  {}^A\bm{G} = \int\_V \frac{d\bm{p}}{dt} \rho dV
\end{equation}

For any mass element \\(\rho dV\\), the position vector \\(\bm{p}\\) can be written as
\\[ p = p\_c + r \\]

And because \\(\int\_V r \rho dV = 0\\), we have by substitution
\\[ {}^A\bm{G} = \frac{d\bm{p}\_c}{dt} \int\_V \rho dV \\]
and thus

\begin{equation}
  \tcmbox{{}^A\bm{G} = m \cdot {}^A\bm{v}\_C}
\end{equation}

in which \\({}^A\bm{v}\_C\\) denotes the velocity of the center of mass with respect to the frame \\(\\{\bm{A}\\}\\).<br />

This result implies that the **total linear momentum** of differential masses is equal to the linear momentum of a **point mass** \\(m\\) located at the **center of mass**.
This highlights the important of the center of mass in dynamic formulation of rigid bodies.

<a id="org0c8049e"></a>

{{< figure src="/ox-hugo/taghirad13_angular_momentum_rigid_body.png" caption="Figure 11: The components of the angular momentum of a rigid body about \\(A\\)" >}}


##### Angular Momentum {#angular-momentum}

Consider the solid body represented in Figure [fig:angular_momentum_rigid_body](#fig:angular_momentum_rigid_body).
Angular momentum of the differential masses \\(\rho dV\\) about a reference point \\(A\\), expressed in the reference frame \\(\\{\bm{A}\\}\\) is defined as
\\[ {}^A\bm{H} = \int\_V \left(\bm{p} \times \frac{d\bm{p}}{dt} \right) \rho dV \\]
in which \\(d\bm{p}/dt\\) denotes the velocity of differential mass with respect to the reference frame \\(\\{\bm{A}\\}\\).

By substituting \\(\bm{p} = \bm{p}\_c + \bm{r}\\) in the previous equations, be obtain:
\\[ {}^A\bm{H} = \bm{p}\_c \times m \bm{v}\_c + \int\_V \bm{r} \times (\bm{\Omega} \times \bm{r}) \rho dV \\]

Therefore, angular momentum of the rigid body about point \\(A\\) is reduced to

\begin{equation}
  \tcmbox{{}^A\bm{H} = \bm{p}\_c \times \bm{G}\_c + {}^C\bm{H}}
\end{equation}

in which
\\[ {}^C\bm{H} = \int\_V \bm{r} \times (\bm{\Omega} \times \bm{r}) \rho dV = {}^C\bm{I} \cdot \bm{\Omega} \\]

Equation \eqref{eq:angular_momentum} reveals that angular momentum of a rigid body about a point \\(A\\) can be written as \\(\bm{p}\_c \times \bm{G}\_c\\), which is the contribution of linear momentum of the rigid body about point \\(A\\), and \\({}^C\bm{H}\\) which is the angular momentum of the rigid body about the center of mass.

This also highlights the important of the center of mass in the dynamic analysis of rigid bodies.
If the center of mass is taken as the reference point, the relation describing angular momentum \eqref{eq:angular_momentum} is very analogous to that of linear momentum \eqref{eq:linear_momentum}.


##### Kinetic Energy {#kinetic-energy}

The Kinetic energy of a rigid body is defined as

\begin{equation}
  \tcmbox{\bm{K} = \frac{1}{2} \int\_V \bm{v} \cdot \bm{v} \rho dV}
\end{equation}

The velocity of a differential mass \\(\rho dV\\) can be represented by linear velocity of the center of mass and angular velocity of the rigid body as
\\[ \bm{v} = \bm{v}\_p + \bm{\Omega} \times \bm{r} \\]

By substitution, the kinetic energy of the rigid body may be obtained by:

\begin{equation}
  \tcmbox{\bm{K} = \frac{1}{2} \bm{v}\_c \times \bm{G}\_c + \frac{1}{2} \bm{\Omega} \cdot {}^C\bm{H}}
\end{equation}

in which \\(\bm{G}\_C\\) is the linear momentum of the rigid body and \\({}^C\bm{H}\\) is the angular momentum of the rigid body about the center of mass.<br />

This equation reveals that kinetic energy of a moving body can be represented as the **kinetic energy of a point mass located as the center of mass**, in addition to the **kinetic energy of a body rotating about the center of mass**.


#### Newton-Euler Laws {#newton-euler-laws}

The Newton and Euler laws can be written for three different cases where the angular motion:

1.  is about a fixed point in space
2.  is represented about the center of mass
3.  is represented about an arbitrary moving point in space

We only examine the case in which all rotations are represented about the center of mass.<br />

Consider a rigid body under general motion, that is, a combination of translation and rotation.

<div class="cbox">
  <div></div>

The Newton's law relates the change of linear momentum of the rigid body to the resulting external forces applied to it
\\[ \sum \bm{f}\_\text{ext} = \frac{d\bm{G}\_c}{dt} \\]

</div>

For the case of a constant mass rigid body, this law is reduced to
\\[ \sum \bm{f}\_\text{ext} = m \frac{d\bm{v}\_c}{dt} = m \bm{a}\_c \\]
in which \\(\bm{a}\_c\\) is the linear acceleration of the center of mass.<br />

<div class="cbox">
  <div></div>

The Euler's law relates the change of angular momentum of a rigid body about the center of mass, to the summation of all external moments applied to the rigid body about center of mass
\\[ \sum {}^c\bm{n}\_\text{ext} = \frac{d}{dt}({}^c\bm{H}) \\]

</div>

For the case of a constant mass rigid body, this law is reduced to
\\[ \sum {}^c\bm{n}\_\text{ext} = \frac{d}{dt}({}^c\bm{I} \bm{\Omega}) = {}^c\bm{I} \dot{\bm{\Omega}} + \bm{\Omega} \times ({}^c\bm{I} \bm{\Omega}) \\]
in which \\(\sum {}^c\bm{n}\_\text{ext}\\) is the summation of all external moments applied to the rigid body about the center of mass, \\({}^c\bm{I}\\) is the moment of inertia about the center of mass, and \\(\bm{\Omega}\\) is the angular velocity of the rigid body.


### Newton-Euler Formulation {#newton-euler-formulation}

The most popular approach used in robotics to derive the dynamic equation of motion of a parallel manipulator is the **Newton-Euler formulation**.

In the Newton-Euler formulation, the **free-body diagrams** of all the limbs and moving platform are considered and the **Newton Euler laws are applied to each isolated body**.
To apply the laws to each body, it is necessary to derive linear acceleration of links, center of mass, as well as angular acceleration of the links.
Hence, **acceleration analysis** would be performed on all the links of the manipulator and the moving platform.<br />

Furthermore, all the external forces and moments applied to the links and to the moving platform must be carefully determined.
Gravitational forces acting on the center of masses, frictional forces and moments acting on the joints, and any possible disturbance force or moment applied to the links and to the moving platform would be identified.
The most important external forces or moments applied on the manipulator are the one applied by the actuators, denoted by \\(\bm{\tau} = [\tau\_{1}, \tau\_{2}, \dots, \tau\_{m}]^{T}\\).
The forces and moments shall be derived from the set of Newton-Euler laws, which are written separately for each link and the moving platform.<br />

Finally, by elimination of these constraints forces and moments on the Newton-Euler equations written for the moving platform, the dynamic equations **relating the actuator forces and moments** \\(\bm{\tau}\\) to the **motion variables of the moving platform** \\(\bm{\mathcal{X}}\\), \\(\dot{\bm{\mathcal{X}}}\\) and \\(\ddot{\bm{\mathcal{X}}}\\) are derived.


#### Dynamic Formulation of the Stewart-Gough Platform {#dynamic-formulation-of-the-stewart-gough-platform}


##### Acceleration Analysis {#acceleration-analysis}

In acceleration analysis, it is intended to **derive expressions for linear and angular acceleration of the limbs**, namely \\(\ddot{l}\_{i}\\) and \\(\dot{\bm{\omega}}\_{i}\\) as a function of the moving platform acceleration \\(\ddot{\bm{\mathcal{X}}} = [\dot{\bm{v}}\_{p}, \dot{\bm{\omega}}]^{T}\\).
To obtain such a relation, let us rewrite the **velocity loop closure**:

\begin{equation}
  \bm{v}\_{p} + \bm{\omega} \times \bm{b}\_{i} = \dot{l}\_{i} \hat{\bm{s}}\_{i} + l\_{i}(\bm{\omega}\_{i} \times \hat{\bm{s}}\_{i})
\end{equation}

Since there is no actuation torque about \\(\hat{\bm{s}}\_{i}\\), the limb angular velocity and acceleration vectors (\\(\bm{\omega}\_i\\) and \\(\dot{\bm{\omega}}\_i\\)) are normal to \\(\hat{\bm{s}}\_{i}\\) provided that the following assumption are considered for the platform:

-   both end joints of the limb are spherical
-   the limbs are symmetric with respect to their axes
-   the effects of friction in spherical joints are neglected

Considering these assumptions, it can be concluded that the limbs cannot spin about their axes: \\(\bm{\omega}\_{i} \cdot \hat{\bm{s}}\_{i} = 0\\) and \\((\hat{\bm{s}}\_i \times (\bm{\omega}\_i \times \hat{\bm{s}}\_i)) = \bm{\omega}\_i\\).

To obtain the angular velocity of the limbs \\(\bm{\omega}\_{i}\\), we cross multiply \\(\hat{\bm{s}}\_{i}\\) to both sides of the previous equation:

\begin{equation}
  \bm{\omega}\_{i} = \frac{1}{l\_i} ( \hat{\bm{s}}\_i \times \bm{v}\_{b\_i} )
\end{equation}

With \\(\bm{v}\_{b\_{i}}\\) an **intermediate variable** corresponding to the velocity of point \\(\bm{b}\_{i}\\):

\begin{equation}
  \bm{v}\_{b\_{i}} = \bm{v}\_{p} + \bm{\omega} \times \bm{b}\_{i}
\end{equation}

As illustrated in Figure [fig:free_body_diagram_stewart](#fig:free_body_diagram_stewart), the piston-cylinder structure of the limbs is decomposed into two separate parts, the masses of which are denoted by \\(m\_{i\_1}\\) and \\(m\_{i\_2}\\).
The position vector of these two center of masses can be determined by the following equations:

\begin{align}
  \bm{p}\_{i\_1} &= \bm{a}\_{i} + c\_{i\_1} \hat{\bm{s}}\_{i} \\\\\\
  \bm{p}\_{i\_2} &= \bm{a}\_{i} + ( l\_i - c\_{i\_2}) \hat{\bm{s}}\_{i}
\end{align}

<a id="orgc1e3ded"></a>

{{< figure src="/ox-hugo/taghirad13_free_body_diagram_stewart.png" caption="Figure 12: Free-body diagram of the limbs and the moving platform of a general Stewart-Gough manipulator" >}}

By differentiating the previous equations and doing some manipulations, we obtain:

\begin{align}
  \ddot{l}\_i &= \bm{a}\_{b\_i} \times \hat{\bm{s}}\_i + l\_i (\bm{\omega\_i} \cdot \bm{\omega\_i}) \\\\\\
  \dot{\bm{\omega}}\_i &= \frac{1}{l\_i} (\hat{\bm{s}}\_i \times \bm{a}\_{b\_i} - 2 \dot{l}\_i \bm{\omega}\_i) \\\\\\
  \bm{a}\_{i\_1} &= c\_{i\_1} ( \dot{\bm{\omega}}\_i \times \hat{\bm{s}}\_{i} + \bm{\omega}\_i \times (\bm{\omega}\_i \times \hat{\bm{s}}\_i)) \\\\\\
  \bm{a}\_{i\_2} &= ( l\_i - c\_{i\_2}) (\dot{\bm{\omega}}\_i \times \hat{\bm{s}}\_{i} - (\bm{\omega}\_i \cdot \bm{\omega}\_i) \hat{\bm{s}}\_i) + 2 \dot{l}\_i (\bm{\omega}\_i \times \hat{\bm{s}}\_i) + \ddot{l}\_i \hat{\bm{s}}\_i
\end{align}

with

\begin{equation}
  \bm{a}\_{b\_i} = \bm{a}\_p + \dot{\bm{\omega}} \times \bm{b}\_i + \bm{\omega} \times (\bm{\omega} \times \bm{b}\_i)
\end{equation}


##### Dynamic Formulation of the Limbs {#dynamic-formulation-of-the-limbs}

To derive the dynamic formulation of the Stewart-Gough platform, the manipulator is decomposed into a moving platform and six identical limbs.
We assume that each limb consists of two parts, the cylinder and the piston, where the velocities and the accelerations of their centers of masses are determined.
We also assume that the centers of masses of the cylinder and the piston are located at a distance of \\(c\_{i\_1}\\) and \\(c\_{i\_2}\\) above their foot points, and their masses are denoted by \\(m\_{i\_1}\\) and \\(m\_{i\_2}\\).
Moreover, consider that the pistons are symmetric about their axes, and their centers of masses lie at their midlengths.

The free-body diagrams of the limbs and the moving platforms is given in Figure [fig:free_body_diagram_stewart](#fig:free_body_diagram_stewart).
The reaction forces at fixed points \\(A\_i\\) are denoted by \\(\bm{f}\_{a\_i}\\), the internal force at moving points \\(B\_i\\) are dentoed by \\(\bm{f}\_{b\_i}\\), and the internal forces and moments between cylinders and pistons are denoted by \\(\bm{f}\_{c\_i}\\) and \\(\bm{M\_{c\_i}}\\) respectively.

Assume that the only existing external disturbance wrench is applied on the moving platform and is denoted by \\(\bm{\mathcal{F}}\_d = [\bm{F}\_d, \bm{n}\_d]^T\\).

<div class="cbox">
  <div></div>

\begin{equation}
\begin{aligned}
  \bm{f}\_{b\_i} &= \frac{1}{l\_i}(I\_{xx\_i} +  l\_i^2 m\_{c\_e}) \hat{\bm{s}}\_i \times \dot{\bm{\omega}}\_i \\\\\\
               &\quad + \frac{2}{l\_i} m\_{i\_2} c\_{i\_2} \dot{l}\_i \hat{\bm{s}}\_i \times \bm{\omega}\_i \\\\\\
               &\quad - m\_{g\_e} \hat{\bm{s}}\_i \times (\hat{\bm{s}}\_i \times \bm{g})
\end{aligned}
\end{equation}

in which \\(m\_{c\_e}\\) is defined as

\begin{equation}
  m\_{c\_e} = \frac{1}{l\_i^2} \left( m\_{i\_1} c\_{i\_1}^2 + m\_{i\_2} c\_{i\_2}^2 \right)
\end{equation}

</div>


##### Dynamic Formulation of the Moving Platform {#dynamic-formulation-of-the-moving-platform}

Assume that the **moving platform center of mass is located at the center point** \\(P\\) and it has a mass \\(m\\) and moment of inertia \\({}^A\bm{I}\_{P}\\).
Furthermore, consider that gravitational force and external disturbance wrench are applied on the moving platform, \\(\bm{\mathcal{F}}\_d = [\bm{F}\_d, \bm{n}\_d]^T\\) as depicted in Figure [fig:free_body_diagram_stewart](#fig:free_body_diagram_stewart).

The Newton-Euler formulation of the moving platform is as follows:

\begin{align}
  \sum \bm{F}\_{\text{ext}} &= \sum\_{i=1}^6 \bm{f}\_{b\_i} + m \bm{g} + \bm{F}\_{d} = m \bm{a}\_p \\\\\\
  \sum {}^p\bm{n}\_{\text{ext}} &= \bm{n}\_d + \sum\_{i=1}^6 \bm{b}\_i \times \bm{f}\_{b\_i} \nonumber \\\\\\
                               & = {}^{A}\bm{I}\_{P} \dot{\bm{\omega}} + \bm{\omega} \times {}^{A}\bm{I}\_{P} \bm{\omega}
\end{align}

in which \\({}^A\bm{I}\_P\\) is considered in the fixed frame \\(\\{\bm{A}\\}\\) and can be calculated by:

\begin{equation}
  {}^A\bm{I}\_P = {}^A\bm{R}\_B {}^B\bm{I}\_P {}^A\bm{R}\_B^T
\end{equation}

These equations can be rewritten in an implicit form as

<div class="cbox">
  <div></div>

\begin{align}
  & m(\bm{a}\_p - \bm{g}) - \bm{F}\_d - \sum\_{i=1}^{6} \bm{f}\_{b\_i} = \bm{0} \label{eq:dyn\_form\_implicit\_trans}\\\\\\
  & {}^A\bm{I}\_P \dot{\bm{\omega}} + \bm{\omega} \times {}^A\bm{I}\_P\bm{\omega} - \bm{n}\_d - \sum\_{i=1}^{6} \bm{b}\_{i} \times \bm{f}\_{b\_i} = \bm{0} \label{eq:dyn\_form\_implicit\_rot}
\end{align}

</div>

These two equations are the governing dynamic formulation of the Stewart-Gough platform, in which \\(\bm{\mathcal{F}}\_d = [\bm{F}\_{d}, \bm{n}\_{d}]^T\\) denotes the disturbance wrench exerted on the moving plateform.

They can be viewed in an implicit vector form of

\begin{equation}
  \bm{f}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}}, \ddot{\bm{\mathcal{X}}}, \bm{\mathcal{F}}\_d, \bm{\tau}) = \bm{0}
\end{equation}

in which \\(\bm{\mathcal{X}} = [\bm{x}\_P, \bm{\theta}]^T\\) is the motion variable of the moving platform consisting of the linear position of point \\(P\\) and the moving platform orientation represented by screw coordinates.


#### Closed-Form Dynamics {#closed-form-dynamics}

While dynamic formulation in the form of Equation \eqref{eq:dynamic_formulation_implicit} can be used to simulate inverse dynamics of the Stewart-Gough platform, its implicit nature makes it unpleasant for the dynamic analysis and control.


##### Closed-Form Dynamics of the Limbs {#closed-form-dynamics-of-the-limbs}

To derive a closed-form dynamic formulation for the Stewart-Gough platform as

\begin{equation}
  \bm{M}(\bm{\mathcal{X}})\ddot{\bm{\mathcal{X}}} + \bm{C}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}})\dot{\bm{\mathcal{X}}} + \bm{G}(\bm{\mathcal{X}}) = \bm{\mathcal{F}}
\end{equation}

first consider an intermediate generalized coordinate \\(x\_i\\), which is in fact the position of point \\(b\_i\\).
This generalized coordinate is used to harmonize the limb and the moving platform dynamic formulation and to **derive an closed-form structure for the whole manipulator**.

Now, manipulate each limb dynamic equations to convert them into the closed form.
Let us first introduce some relations to substitute kinematic parameters like \\(\dot{\bm{\omega}}\_i\\), \\(\ddot{\bm{\omega}}\_i\\), \\(\ddot{l}\_i\\) with the intermediate generalized coordinate \\(x\_i\\) and its time derivatives.

After some manipulations, we obtain the following closed form equation:

\begin{equation}
  \bm{M}\_i \ddot{\bm{x}}\_i + \bm{C}\_i \dot{\bm{x}}\_i + \bm{G}\_i = \bm{F}\_i
\end{equation}

the corresponding mass matrix \\(\bm{M}\_i\\), the Coriolis matrix \\(\bm{C}\_i\\), and the gravity vector \\(\bm{G}\_i\\) can be simplified into the following form:

\begin{align}
  \bm{M}\_i &= m\_{i\_2}\hat{\bm{s}}\_i \hat{\bm{s}}\_i^T - \frac{1}{l\_i^2} I\_{xx\_i} \hat{\bm{s}}\_{i \times}^2 \\\\\\
  \bm{C}\_i &= -\frac{2}{l\_i} m\_{c\_o}\dot{l}\_i \hat{\bm{s}}\_{i\times}^2 - \frac{1}{l\_i^2} m\_{i\_2} c\_{i\_2} \hat{\bm{s}}\_i \dot{\bm{x}}\_i^T \hat{\bm{s}}\_{i \times}^2 \\\\\\
  \bm{G}\_i &= \big( m\_{g\_e} \hat{\bm{s}}\_{i\times}^2 - m\_{i\_2} \hat{\bm{s}}\_i \hat{\bm{s}}\_i^T \big) \bm{g} \\\\\\
  \bm{F}\_i &= -\bm{f}\_{b\_i} + \tau\_i \hat{\bm{s}}\_i
\end{align}

in which

\begin{align}
  m\_{c\_e} &= \frac{1}{l\_i^2} (m\_{i\_1} c\_{i\_1}^2 + m\_{i\_2} c\_{i\_2}^2) \\\\\\
  m\_{c\_o} &= \frac{1}{l\_i} m\_{i\_2} c\_{i\_2} - \frac{1}{l\_i^2} (I\_{xx\_i} + l\_i^2 m\_{c\_e}) \\\\\\
  m\_{g\_e} &= \frac{1}{l\_i} (m\_{i\_1} c\_{i\_1} + m\_{i\_2}(l\_i - c\_{i\_2}))
\end{align}


##### Closed-Form Dynamics of the Moving Platform {#closed-form-dynamics-of-the-moving-platform}

In this section, the dynamic equations of the moving platform are transformed in the following closed-form formulation

\begin{equation}
  \bm{M}\_{p}\ddot{\bm{\mathcal{X}}} + \bm{C}\_{p}\dot{\bm{\mathcal{X}}} + \bm{G}\_{p} = \bm{\mathcal{F}}\_{p}
\end{equation}

in which \\(\bm{\mathcal{X}}\\) consists of six coordinates: the first three \\(\bm{x}\_p\\) represent linear motion of the moving platform, and the last three \\(\bm{\theta}\\) its angular motion.
It is preferable to use the **screw coordinates** for representing the angular motion **as its derivative is also a vector representing angular velocity**:

\begin{equation}
  \tcmbox{\bm{\mathcal{X}} = \begin{bmatrix}\bm{x}\_p \\ \bm{\theta}\end{bmatrix}; \quad
  \dot{\bm{\mathcal{X}}} = \begin{bmatrix}\bm{v}\_p \\ \bm{\omega}\end{bmatrix}; \quad
  \ddot{\bm{\mathcal{X}}} = \begin{bmatrix}\bm{a}\_p \\ \dot{\bm{\omega}}\end{bmatrix}}
\end{equation}

Equations \eqref{eq:dyn_form_implicit_trans} and \eqref{eq:dyn_form_implicit_rot} can be simply converted into a closed form of Equation \eqref{eq:close_form_dynamics_platform} with the following terms:

\begin{equation}
  \begin{aligned}
    &\bm{M}\_p = \begin{bmatrix} m\bm{I}\_{3 \times 3} & \bm{O}\_{3\times 3} \\ \bm{O}\_{3\times 3} & {}^A \bm{I}\_p \end{bmatrix}\_{6\times 6}; \bm{C}\_p = \begin{bmatrix} \bm{O}\_{3\times 3} & \bm{O}\_{3\times 3} \\ \bm{O}\_{3\times 3} & \omega\_{\times} {}^A\bm{I}\_p \end{bmatrix}\_{6 \times 6} \\\\\\
    &\bm{G}\_p = \begin{bmatrix}-m\bm{g} \\ \bm{O}\_{3\times 1}\end{bmatrix}\_{6 \times 1}; \bm{\mathcal{F}}\_p = \begin{bmatrix} \bm{F}\_d + \sum \bm{f}\_{b\_i} \\ \bm{n}\_d + \sum \bm{b}\_{i \times} \bm{f}\_{b\_i} \end{bmatrix}\_{6\times 1}
  \end{aligned}
\end{equation}


##### Closed-Form Dynamics of the Stewart-Gough Manipulator {#closed-form-dynamics-of-the-stewart-gough-manipulator}

To derive the closed-form dynamic formulation for the whole manipulator, a transformation is required to map the intermediate generalized coordinates \\(x\_i\\) into the principal generalized coordinates \\(\bm{\mathcal{X}}\\).

Using such a transformation, and by adding the resulting equations of the limbs and the moving platform, the internal forces \\(\bm{f}\_{b\_i}\\) can be eliminated, and closed-form dynamic formulation for the whole manipulator can be derived.

To generate such a transformation define a Jacobian matrix \\(\bm{J}\_i\\) relating the intermediate coordinates to that of the principal generalized coordinate:

\begin{equation}
  \dot{\bm{x}}\_i = \bm{J}\_i \dot{\bm{\mathcal{X}}}
\end{equation}

in which

\begin{equation}
  \bm{J}\_i = \begin{bmatrix} \bm{I}\_{3 \times 3} & -\bm{b}\_{i \times} \end{bmatrix}
\end{equation}

\begin{equation}
  \bm{M}\_{li} \ddot{\bm{x}}\_i + \bm{C}\_{li} + \dot{\bm{x}}\_i + \bm{G}\_{li} = \bm{\mathcal{F}}\_{li}
\end{equation}

in which

\begin{equation}
  \begin{aligned}
    \bm{M}\_{li} = \bm{J}\_i^T \bm{M}\_i \bm{J}\_i; \quad & \bm{C}\_{li} = J\_i^T \bm{M}\_i \dot{\bm{J}}\_i + \bm{J}\_i^T \bm{C}\_i \bm{J}\_i \\\\\\
    \bm{G}\_{li} = \bm{J}\_i^T G\_i; \quad & \bm{\mathcal{F}}\_{li} = \bm{J}\_i^T \bm{F}\_i
  \end{aligned}
\end{equation}

<div class="cbox">
  <div></div>

\begin{equation}
  \begin{aligned}
    \bm{M}(\bm{\mathcal{X}}) &= \bm{M}\_p + \sum\_{i=1}^6 \bm{M}\_{li} \\\\\\
    \bm{C}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}}) &= \bm{C}\_p + \sum\_{i=1}^6 \bm{C}\_{li} \\\\\\
    \bm{G}(\bm{\mathcal{X}}) &= \bm{G}\_p + \sum\_{i=1}^6 \bm{G}\_{li} \\\\\\
    \bm{\mathcal{F}}(\bm{\mathcal{X}}) &= \bm{\mathcal{F}}\_d + \sum\_{i=1}^6 \bm{\mathcal{F}}\_{\tau\_i}
  \end{aligned}
\end{equation}

</div>


##### Forward Dynamics Simulations {#forward-dynamics-simulations}

As shown in Figure [fig:stewart_forward_dynamics](#fig:stewart_forward_dynamics), it is **assumed that actuator forces and external disturbance wrench applied to the manipulator are given and the resulting trajectory of the moving platform is to be determined**.

<a id="orgff76393"></a>

{{< figure src="/ox-hugo/taghirad13_stewart_forward_dynamics.png" caption="Figure 13: Flowchart of forward dynamics implementation sequence" >}}

The closed-form dynamic formulation of the Stewart-Gough platform corresponds to the set of equations given in \eqref{eq:closed_form_dynamic_stewart_wanted}, whose terms are given in \eqref{eq:close_form_dynamics_stewart_terms}.


##### Inverse Dynamics Simulation {#inverse-dynamics-simulation}

In inverse dynamics simulations, it is assumed that the **trajectory of the manipulator is given**, and the **actuator forces required to generate such trajectories are to be determined**.

As illustrated in Figure [fig:stewart_inverse_dynamics](#fig:stewart_inverse_dynamics), inverse dynamic formulation is implemented in the following sequence.
The first step is trajectory generation for the manipulator moving platform.
Many different algorithms are developed for a smooth trajectory generation.
For such a trajectory, \\(\bm{\mathcal{X}}\_{d}(t)\\) and the time derivatives \\(\dot{\bm{\mathcal{X}}}\_{d}(t)\\), \\(\ddot{\bm{\mathcal{X}}}\_{d}(t)\\) are known.

The next step is to solve the inverse kinematics of the manipulator and to find the limbs' linear and angular positions, velocity and acceleration as a function of the manipulator trajectory.
The manipulator Jacobian matrix \\(\bm{J}\\) is also calculated in this step.

Next, the dynamic matrices given in the closed-form formulations of the limbs and the moving platform are calculated using equations \eqref{eq:closed_form_intermediate_parameters} and \eqref{eq:close_form_dynamics_stewart_terms}, respectively.<br />

To combine the corresponding matrices, an to generate the whole manipulator dynamics, it is necessary to find intermediate Jacobian matrices \\(\bm{J}\_i\\), given in \eqref{eq:jacobian_intermediate}, and then compute compatible matrices for the limbs given in \eqref{eq:closed_form_stewart_manipulator}.
Now that all the terms required to **computed to actuator forces required to generate such a trajectory** is computed, let us define \\(\bm{\mathcal{F}}\\) as the resulting Cartesian wrench applied to the moving platform.
This wrench can be calculated from the summation of all inertial and external forces **excluding the actuator torques** \\(\bm{\tau}\\) in the closed-form dynamic formulation \eqref{eq:closed_form_dynamic_stewart_wanted}.

By this definition, \\(\bm{\mathcal{F}}\\) can be viewed as the projector of the actuator forces acting on the manipulator, mapped to the Cartesian space.
Since there is no redundancy in actuation in the Stewart-Gough manipulator, the Jacobian matrix \\(\bm{J}\\), squared and actuator forces can be uniquely determined from this wrench, by \\(\bm{\tau} = \bm{J}^{-T} \bm{\mathcal{F}}\\), provided \\(\bm{J}\\) is non-singular.
Therefore, actuator forces \\(\bm{\tau}\\) are computed in the simulation from

\begin{equation}
  \bm{\tau} = \bm{J}^{-T} \left( \bm{M}(\bm{\mathcal{X}})\ddot{\bm{\mathcal{X}}} + \bm{C}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}})\dot{\bm{\mathcal{X}}} + \bm{G}(\bm{\mathcal{X}}) - \bm{\mathcal{F}}\_d \right)
\end{equation}

<a id="org077ac35"></a>

{{< figure src="/ox-hugo/taghirad13_stewart_inverse_dynamics.png" caption="Figure 14: Flowchart of inverse dynamics implementation sequence" >}}


### Virtual Work Formulation {#virtual-work-formulation}


### Lagrange Formulation {#lagrange-formulation}

\begin{equation}
  K(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}}) = \frac{1}{2} \dot{\bm{\mathcal{X}}}^T \bm{M}(\bm{\mathcal{X}}) \dot{\bm{\mathcal{X}}}
\end{equation}

\begin{equation}
  \bm{G}(\bm{\mathcal{X}}) = \frac{\partial P(\bm{\mathcal{X}})}{\partial \bm{\mathcal{X}}}
\end{equation}

\begin{equation}
  \bm{C}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}}) = \frac{1}{2} (\dot{\bm{M}} + \bm{U}^T - \bm{U})
\end{equation}


## Motion Control {#motion-control}

<a id="orgdd65dc9"></a>


### Introduction {#introduction}

Parallel robots are designed for two different types of applications.<br />

In the first type, the moving platform of the robot accurately follows a **desired position and orientation** path in a specific time frame, while **no interacting forces** need to be applied to the environment.<br />

The second type of application include situations where the robot moving platform is in **contact with a stiff environment** (e.g. precision machining).
In such application, the contact force describe the state of interaction more effectively than the position and orientation of the moving platform.
The problem of **force control** can be described as to derive the actuator forces for such a manipulator required to generate a prescribed desired wrench (force and torque) at the manipulator moving platform, while the manipulator is performing its motion.<br />

Although a multiple degrees-of-freedom robotic manipulator can usually be represented by a MIMO and nonlinear model, many industrial controllers for such robots consist of a number of linear controller designed to **control individual joint motions**.
One of the reasons why such decentralization can perform well in practice is the use of large gear reductions in robot actuators, which significantly reduces the coupling and non linear behavior of robot dynamics.<br />

However, using advanced techniques in nonlinear and MIMO control permits to overcome limitations of the SISO approach.


### Controller Topology {#controller-topology}

<a id="org5d92117"></a>

<div class="cbox">
  <div></div>

In motion control of parallel manipulator, it is assumed that the controller computes the **required actuator forces** or torques to cause the robot motion to follow a desired position and orientation trajectory.

</div>

Let us use the motion variables as the generalized coordinate of the moving platform defined by \\(\bm{\mathcal{X}} = [\bm{x}\_P, \bm{\theta}]^T\\), in which the linear motion is represented by \\(\bm{x}\_p = [x\_p, y\_p, z\_p]^T\\), while the moving platform orientation is represented by **screw coordinates** \\(\bm{\theta} = \theta[s\_x, s\_y, s\_z]^T = [\theta\_x, \theta\_y, \theta\_z]^T\\).<br />

Consider the general closed-form dynamics formulation of a parallel robot

\begin{equation}
  \tcmbox{\bm{M}(\bm{\mathcal{X}})\ddot{\bm{\mathcal{X}}} + \bm{C}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}})\dot{\bm{\mathcal{X}}} + \bm{G}(\bm{\mathcal{X}}) = \bm{\mathcal{F}}}
\end{equation}

where

-   \\(\bm{M}(\bm{\mathcal{X}})\\) denotes the mass matrix
-   \\(\bm{C}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}})\\) denotes the Coriolis and centrifugal matrix
-   \\(\bm{G}(\bm{\mathcal{X}})\\) denotes the gravity vector
-   \\(\bm{\mathcal{F}}\\) denotes the generalized forces applied to the moving platform center of mass

The generalized forces can be decomposed as follow
\\[ \bm{\mathcal{F}} = \bm{J}^T \bm{\tau} + \bm{\mathcal{F}}\_d \\]
with

-   \\(\bm{J}\\) is the Jacobian
-   \\(\bm{\tau}\\) are the actuator forces
-   \\(\bm{\mathcal{F}}\_d\\) are any external wrenches

<div class="cbox">
  <div></div>

**Control topology** is referred to the **structure of the control system** used to compute the actuator forces/torques from the measurements, and the required pre and post processing.

</div>

For motion control of a manipulator, the controller has to compute the actuator force/torques required to cause the motion of the moving platform according to the desired trajectory.
In general, the desired motion of the moving platform may be represented by the desired generalized coordinate of the manipulator, denoted by \\(\bm{\mathcal{X}}\_d\\).<br />

To perform such motion in closed loop, it is necessary to **measure the output motion** \\(\bm{\mathcal{X}}\\) of the manipulator by an instrumentation system.
Such instrumentation usually consists of two subsystems: the first subsystem may use accurate accelerometers, or global positioning systems to calculate the position of a point on the moving platform; and a second subsystem may use inertial or laser gyros to determine orientation of the moving platform.<br />

Figure [fig:general_topology_motion_feedback](#fig:general_topology_motion_feedback) shows the general topology of a motion controller using direct measurement of the motion variable \\(\bm{\mathcal{X}}\\), as feedback in the closed-loop system.
In such a structure, the measured position and orientation of the manipulator is compared to its desired value to generate the **motion error vector** \\(\bm{e}\_\mathcal{X}\\).
The controller uses this error information to generate suitable commands for the actuators to minimize the tracking error.<br />

<a id="orga16758c"></a>

{{< figure src="/ox-hugo/taghirad13_general_topology_motion_feedback.png" caption="Figure 15: The general topology of motion feedback control: motion variable \\(\bm{\mathcal{X}}\\) is measured" >}}

However, it is usually much **easier to measure the active joint variable** rather than measuring the final position and orientation of the moving platform.
The relation between the **joint variable** \\(\bm{q}\\) and **motion variable** of the moving platform \\(\bm{\mathcal{X}}\\) is dealt with the **forward and inverse kinematics**.
The relation between the **differential motion variables** \\(\dot{\bm{q}}\\) and \\(\dot{\bm{\mathcal{X}}}\\) is studied through the **Jacobian analysis**.<br />

It is then possible to use the forward kinematic analysis to calculate \\(\bm{\mathcal{X}}\\) from the measured joint variables \\(\bm{q}\\), and one may use the control topology depicted in Figure [fig:general_topology_motion_feedback_bis](#fig:general_topology_motion_feedback_bis) to implement such a controller.

<a id="org72c22c0"></a>

{{< figure src="/ox-hugo/taghirad13_general_topology_motion_feedback_bis.png" caption="Figure 16: The general topology of motion feedback control: the active joint variable \\(\bm{q}\\) is measured" >}}

In this topology, the forward kinematic analysis of the manipulator has to be performed to implement the feedback loop.
As described earlier, this is a **complex task** for parallel manipulators.
It is even more complex when a solution has to be found in real time.<br />

However, as shown herein before, the inverse kinematic analysis of parallel manipulators is much easier to carry out.
To overcome the implementation problem of the control topology in Figure [fig:general_topology_motion_feedback_bis](#fig:general_topology_motion_feedback_bis), another control topology is usually implemented for parallel manipulators.

In this topology, depicted in Figure [fig:general_topology_motion_feedback_ter](#fig:general_topology_motion_feedback_ter), the desired motion trajectory of the robot \\(\bm{\mathcal{X}}\_d\\) is used in an **inverse kinematic analysis** to find the corresponding desired values for joint variable \\(\bm{q}\_d\\).
Hence, the controller is designed based on the **joint space error** \\(\bm{e}\_q\\).

<a id="org65fcdb0"></a>

{{< figure src="/ox-hugo/taghirad13_general_topology_motion_feedback_ter.png" caption="Figure 17: The general topology of motion feedback control: the active joint variable \\(\bm{q}\\) is measured, and the inverse kinematic analysis is used" >}}

Therefore, the **structure and characteristics** of the controller in this topology is totally **different** from that given in the first two topologies.

The **input and output** of the controller depicted in Figure [fig:general_topology_motion_feedback_ter](#fig:general_topology_motion_feedback_ter) are **both in the joint space**.
However, this is not the case in the previous topologies where the input to the controller is the motion error in task space, while its output is in the joint space.

For the topology in Figure [fig:general_topology_motion_feedback_ter](#fig:general_topology_motion_feedback_ter), **independent controllers** for each joint may be suitable.<br />

To generate a **direct input to output relation in the task space**, consider the topology depicted in Figure [fig:general_topology_motion_feedback_quater](#fig:general_topology_motion_feedback_quater).
A force distribution block is added which maps the generated wrench in the task space \\(\bm{\mathcal{F}}\\), to its corresponding actuator forces/torque \\(\bm{\tau}\\).

<a id="orgf53b839"></a>

{{< figure src="/ox-hugo/taghirad13_general_topology_motion_feedback_quater.png" caption="Figure 18: The general topology of motion feedback control in task space: the motion variable \\(\bm{\mathcal{X}}\\) is measured, and the controller output generates wrench in task space" >}}

For a fully parallel manipulator such as the Stewart-Gough platform, this mapping can be constructed from the **Jacobian** transpose of the manipulator:
\\[ \bm{\mathcal{F}} = \bm{J}^T \bm{\tau}; \quad \bm{\tau} = \bm{J}^{-T} \bm{\mathcal{F}} \\]


### Motion Control in Task Space {#motion-control-in-task-space}

<a id="org7befb5a"></a>


#### Decentralized PD Control {#decentralized-pd-control}

In the control structure in Figure [fig:decentralized_pd_control_task_space](#fig:decentralized_pd_control_task_space), a number of linear PD controllers are used in a feedback structure on each error component.
The decentralized controller consists of **six disjoint linear controllers** acting on each error component \\(\bm{e}\_x = [e\_x,\ e\_y,\ e\_z,\ e\_{\theta\_x},\ e\_{\theta\_y},\ e\_{\theta\_z}]\\).
The PD controller is denoted by \\(\bm{K}\_d s + \bm{K}\_p\\), in which \\(\bm{K}\_d\\) and \\(\bm{K}\_p\\) are \\(6 \times 6\\) **diagonal matrices** denoting the derivative and proportional controller gains for each error term.

<a id="org84d7f6d"></a>

{{< figure src="/ox-hugo/taghirad13_decentralized_pd_control_task_space.png" caption="Figure 19: Decentralized PD controller implemented in task space" >}}

Hence, by this structure, each tracking error component is **treated separately**.
The output of the controller is denoted by \\(\bm{\mathcal{F}} = [F\_x\ F\_y\ F\_z\ \tau\_x\ \tau\_y\ \tau\_z]\\).<br />

In practice, the calculated output wrench is transformed into actuator forces through the force distribution block.
This mapping is implemented through inverse of the manipulator **Jacobian** transpose by \\(\bm{\tau} = \bm{J}^{-T} \bm{\mathcal{F}}\\).<br />

Different alternatives of linear controllers can be used instead of the PD controller used in this structure, however PD controller is the simplest form which can preserve the manipulator stability while providing suitable tracking performance.<br />

The proposed decentralized PD controller is very simple in structure and therefore easily implementable.
The design of such a controller needs no detailed information on the manipulator dynamics.
The controller gains are generally tuned experimentally based on physical realization of the controller by trial and error.


#### Feed Forward Control {#feed-forward-control}

A feedforward wrench denoted by \\(\bm{\mathcal{F}}\_{ff}\\) may be added to the decentralized PD controller structure as depicted in Figure [fig:feedforward_control_task_space](#fig:feedforward_control_task_space).
This term is generated from the dynamic model of the manipulator in the task space, represented in a closed form by the following equation:
\\[ \bm{\mathcal{F}}\_{ff} = \bm{\hat{M}}(\bm{\mathcal{X}}\_d)\ddot{\bm{\mathcal{X}}}\_d + \bm{\hat{C}}(\bm{\mathcal{X}}\_d, \dot{\bm{\mathcal{X}}}\_d)\dot{\bm{\mathcal{X}}}\_d + \bm{\hat{G}}(\bm{\mathcal{X}}\_d) \\]

<a id="org11304dd"></a>

{{< figure src="/ox-hugo/taghirad13_feedforward_control_task_space.png" caption="Figure 20: Feed forward wrench added to the decentralized PD controller in task space" >}}

The desired trajectory in task space \\(\bm{\mathcal{X}}\_d\\), and its derivatives \\(\dot{\bm{\mathcal{X}}}\_d\\), \\(\ddot{\bm{\mathcal{X}}}\_d\\) are the required inputs for the feedforward block.
This term is called feedforward since no online information of the output motion trajectory \\(\bm{\mathcal{X}}\\) is needed for its computation.<br />

In order to generate this term, dynamic formulation of the robots and its kinematic and dynamic parameters are needed.
In practice, exact knowledge of dynamic matrices are not available, and therefore, **estimate** of these matrices are used in practice, denoted by \\(\hat{\bm{M}}\\), \\(\hat{\bm{C}}\\) and \\(\hat{\bm{G}}\\).<br />

The information required to generate the feedforward wrench \\(\bm{\mathcal{F}}\_{ff}\\) is usually available beforehand and can be derived offline.
The closed-loop dynamic formulation for the manipulator becomes:

\begin{equation}
  \begin{aligned}
    \bm{M}(\bm{\mathcal{X}})\ddot{\bm{\mathcal{X}}} &+ \bm{C}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}})\dot{\bm{\mathcal{X}}} + \bm{G}(\bm{\mathcal{X}}) \\\\\\
      &= \bm{\mathcal{F}} + \bm{\mathcal{F}}\_d \\\\\\
      &= \bm{\mathcal{F}}\_{pd} + \bm{\mathcal{F}}\_{ff} + \bm{\mathcal{F}}\_d \\\\\\
      &= \bm{K}\_d \dot{\bm{e}}\_x + \bm{K}\_p \bm{e}\_x + \bm{\mathcal{F}}\_{ff} + \bm{\hat{M}}\ddot{\bm{\mathcal{X}}}\_d + \bm{\hat{C}}\dot{\bm{\mathcal{X}}}\_d + \bm{\hat{G}}
  \end{aligned}
\end{equation}

If the knowledge of the dynamic matrices is complete, we may assume that \\(\hat{\bm{M}} = \bm{M}\\), \\(\hat{\bm{C}} = \bm{C}\\) and \\(\hat{\bm{G}} = \bm{G}\\).
Furthermore, if we consider that the controller performs well such that \\(\bm{\mathcal{X}}(t) \simeq \bm{\mathcal{X}}\_d(t)\\) and \\(\dot{\bm{\mathcal{X}}}(t) \simeq \dot{\bm{\mathcal{X}}}\_d(t)\\), the simplified closed-loop dynamics become:

\begin{equation}
  \begin{aligned}
    \bm{M} (\ddot{\bm{\mathcal{X}}}\_d - \ddot{\bm{\mathcal{X}}}) + \bm{K}\_d \dot{\bm{e}}\_x + \bm{K}\_p \bm{e}\_x + \bm{\mathcal{F}}\_d &= 0 \\\\\\
    \bm{M} \ddot{\bm{e}}\_x + \bm{K}\_d \dot{\bm{e}}\_x + \bm{K}\_p \bm{e}\_x + \bm{\mathcal{F}}\_d &= 0
  \end{aligned}
\end{equation}

This equation implies that, if the mentioned assumptions hold, the **error dynamics** satisfies a set of **second-order system** in the presence of disturbance.
By choosing appropriate gains for PD controller, the transient and steady-state performance of tracking error can be designed so as to satisfy the application requirements.<br />

Note that except the mass matrix, the error dynamic terms are all **configuration independent**, and therefore, it is much **easier to tune the PD controller gains** to work well within the whole workspace of the robot.<br />

However, this method faces a number of **limitations** in practice.
The most important limitation of this control technique is the **stringent assumption of a complete knowledge requirement of the dynamic matrices**.
In practice, derivation of these matrices is a prohibitive task.<br />

Finally, because of the dependency of the mass matrix to the configuration of the robot, the error dynamics are not completely decoupled.
This means that correction in one error component may be considered as a disturbance effect to the other components.
To overcome these limitations, inverse dynamic approach is given in the following section.


#### Inverse Dynamics Control {#inverse-dynamics-control}

<div class="cbox">
  <div></div>

In inverse dynamics control (IDC), nonlinear dynamics of the model is used to add a **corrective term** to the decentralized PD controller.
By this means, **nonlinear and coupling behavior of the robotic manipulator is significantly attenuated**, and therefore, the performance of linear controller is greatly improved.

</div>

General structure of IDC applied to a parallel manipulator is depicted in Figure [fig:inverse_dynamics_control_task_space](#fig:inverse_dynamics_control_task_space).
A corrective wrench \\(\bm{\mathcal{F}}\_{fl}\\) is added in a **feedback structure** to the closed-loop system, which is calculated from the Coriolis and centrifugal matrix and gravity vector of the manipulator dynamic formulation.

Furthermore, mass matrix is added in the forward path in addition to the desired trajectory acceleration \\(\ddot{\bm{\mathcal{X}}}\_d\\).

As for the feedforward control, the **dynamics and kinematic parameters of the robot are needed**, and in practice estimates of these matrices are used.<br />

<a id="org7bcc842"></a>

{{< figure src="/ox-hugo/taghirad13_inverse_dynamics_control_task_space.png" caption="Figure 21: General configuration of inverse dynamics control implemented in task space" >}}

The controller output wrench applied to the manipulator may be derived as follows:

\begin{subequations}
\begin{align}
  \bm{\mathcal{F}} &= \hat{\bm{M}}(\bm{\mathcal{X}}) \bm{a} + \bm{\mathcal{F}}\_{fl} \\\\\\
                   &= \hat{\bm{M}}(\bm{\mathcal{X}}) \bm{a} + \hat{\bm{C}}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}}) \dot{\bm{\mathcal{X}}} + \hat{\bm{G}}(\bm{\mathcal{X}}) \\\\\\
  \bm{a} &= \ddot{\bm{\mathcal{X}}}\_d + \bm{K}\_d \dot{\bm{e}}\_x + \bm{K}\_p \bm{e}\_x
\end{align}
\end{subequations}

The closed-loop dynamic formulation for the manipulator becomes:

\begin{equation}
  \begin{aligned}
    \bm{M}(\bm{\mathcal{X}})\ddot{\bm{\mathcal{X}}} &+ \bm{C}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}})\dot{\bm{\mathcal{X}}} + \bm{G}(\bm{\mathcal{X}}) \\\\\\
      &= \bm{\mathcal{F}} + \bm{\mathcal{F}}\_d \\\\\\
      &= \hat{\bm{M}}(\bm{\mathcal{X}}) \left(\ddot{\bm{\mathcal{X}}}\_d + \bm{K}\_d \dot{\bm{e}}\_x + \bm{K}\_p \bm{e}\_x \right) \\\\\\
       &\quad + \hat{\bm{C}}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}})\dot{\bm{\mathcal{X}}} + \hat{\bm{G}}(\bm{\mathcal{X}}) + \bm{\mathcal{F}}\_d \\\\\\
  \end{aligned}
\end{equation}

If the knowledge of the dynamic matrices is complete, the closed-loop dynamic formulation simplifies to:

\begin{equation}
  \hat{\bm{M}}(\bm{\mathcal{X}}) \left(\ddot{\bm{e}}\_d + \bm{K}\_d \dot{\bm{e}}\_x + \bm{K}\_p \bm{e}\_x \right) + \bm{\mathcal{F}}\_d = 0
\end{equation}

This control technique is very popular in practice because of the fact that this technique can significantly **linearize and decouple dynamic formulation of the closed-loop error dynamics**.
Furthermore, the error dynamic terms are all **configuration independent**, and therefore, it is much easier to tune the PD controller gains for suitable performance in the whole workspace of the robot.<br />

However, note that for a good performance, and **accurate model of the system is required**, and the overall procedure is **not robust to modeling uncertainty**.
Furthermore, this technique is computationally intensive in terms of the online computations needed to carry out the closed-loop control structure.


#### Partial Linearization IDC {#partial-linearization-idc}

Inverse dynamics control has several features making it very attractive in practice.
However, to apply this method, complete knowledge of the dynamic formulation matrices is required.
This requirement has the main drawbacks that the dynamic formulation of the parallel manipulator is a complicated step to be carried out.<br />

To implement all the terms in IDC structure, not only the structure and components of such matrices must be carefully determined, but also the kinematics and inertial parameters of the robot are needed to be identified and calibrated.
This step requires the use of high-precision calibration equipment which are not usually accessible.
Finally, if all the terms and parameters are well known, implementation of full inverse dynamic linearization is computationally intensive.

These are the reasons why, in practice, IDC control is extended to different forms where the above-mentioned stringent requirements are reduced.<br />

To develop the simplest possible implementable IDC, let us recall dynamic formulation complexities:

-   the manipulator mass matrix \\(\bm{M}(\bm{\mathcal{X}})\\) is derived from kinetic energy of the manipulator (Eq. \eqref{eq:kinetic_energy})
-   the gravity vector \\(\bm{G}(\bm{\mathcal{X}})\\) is derived from potential energy (Eq. \eqref{eq:gravity_vectory})
-   the Coriolis and centrifugal matrix \\(\bm{C}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}})\\) is derived from Eq. \eqref{eq:gravity_vectory}

The computation of the Coriolis and centrifugal matrix is more intensive than that of the mass matrix.
Gravity vector is more easily computable.

However, it is shown that certain properties hold for mass matrix, gravity vector and Coriolis and centrifugal matrix, which might be directly used in the control techniques developed for parallel manipulators.
One of the most important properties of dynamic matrices is the skew-symmetric property of the matrix \\(\dot{\bm{M}} - 2 \bm{C}\\) .<br />

Consider dynamic formulation of parallel robot given in Eq. \eqref{eq:closed_form_dynamic_formulation}, in which the skew-symmetric property of dynamic matrices is satisfied.
The simplest form of IDC control effort \\(\bm{\mathcal{F}}\\) consists of:
\\[ \bm{\mathcal{F}} = \bm{\mathcal{F}}\_{pd} + \bm{\mathcal{F}}\_{fl} \\]
in which the first term \\(\bm{\mathcal{F}}\_{pd}\\) is generated by the simplified PD form on the motion error:
\\[ \bm{\mathcal{F}}\_{pd} = \bm{K}\_d \dot{\bm{e}}\_x + \bm{K}\_p + \bm{e}\_x \\]

The second term \\(\bm{\mathcal{F}}\_{fl}\\) is considered to be only the gravity vector of the manipulator \\(\bm{G}(\bm{\mathcal{X}})\\), at any configuration, and the computationally intensive Coriolis and centrifugal term is not used:
\\[ \bm{\mathcal{F}}\_{fl} = \bm{G}(\bm{\mathcal{X}}) \\]

Note that for an appreciable tracking performance with no static error at steady state, it is required to have complete knowledge of **only** the gravity term.
By this means, computations required in this control technique are significantly less than that of the general IDC.<br />

Despite the simple structure of such a controller, the resulting control technique is very well performed, especially at steady state.
We can show that this control topology achieves asymptotic tracking for a constant desired trajectory motion, that is, \\(\dot{\bm{\mathcal{X}}}\_d = 0\\).<br />

This reveals the fact that even if the mass matrix and Coriolis and centrifugal matrix are not used in the feedback, and the closed-loop dynamics is not completely linearized, the PD control structure with gravity compensation can still lead to asymptotic tracking.
However, to suitable transient performance, more information of the system dynamics must be used in the linearization technique given in IDC.


### Robust and Adaptative Control {#robust-and-adaptative-control}

Inverse dynamics control faces the stringent requirement that for a good performance, an **accurate model** of the system is required, and the overall procedure is **not robust** to modeling uncertainty.
Furthermore, this technique is computationally intensive in terms of online computation needed to carry out the closed-loop control structure.
The proposed modified inverse dynamics control, while being beneficial in terms of computational cost, is not suitable in terms of a closed-loop transient performance.<br />

Another approach to modify IDC is to consider a complete linearization, but **assume that complete knowledge of dynamic formulation matrices is not available**.
To **compensate for the lack of knowledge**, two advanced control methods, namely **robust** and **adaptive control** are proposed:

-   In the **robust approach**, a **fixed controller** is designed to satisfy the control objectives for the **worst possible case of modeling uncertainty** and disturbance wrenches.
-   In the **adaptive approach**, the estimates of dynamic formulation matrices are **updated** such that the difference between the true values of these matrices to their estimates converges to zero.

A global understanding of the trade-offs involved in each method is needed to employ either of them in practice.


#### Robust Inverse Dynamics Control {#robust-inverse-dynamics-control}

Various sources of uncertainties such as unmodelled dynamics, unknown parameters, calibration error, unknown disturbance wrenches, and varying payloads may exist, and are not seen in dynamic model of the manipulator.

To consider these modeling uncertainty in the closed-loop performance of the manipulator, recall the general closed-form dynamic formulation of the manipulator given in Eq. \eqref{eq:closed_form_dynamic_formulation}, and modify the inverse dynamics control input \\(\bm{\mathcal{F}}\\) as

\begin{align\*}
  \bm{\mathcal{F}} &= \hat{\bm{M}}(\bm{\mathcal{X}}) \bm{a}\_r + \hat{\bm{C}}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}}) \dot{\bm{\mathcal{X}}} + \hat{\bm{G}}(\bm{\mathcal{X}})\\\\\\
  \bm{a}\_r &= \ddot{\bm{\mathcal{X}}}\_d + \bm{K}\_d \dot{\bm{e}}\_x + \bm{K}\_p \bm{e}\_x + \bm{\delta}\_a
\end{align\*}

in which \\(\bm{a}\_r\\) is the robustified control input.

Comparing this equation to the usual IDC, a robustifying term \\(\bm{\delta}\_a\\) is added to compensate for modeling uncertainties.

Note that, as defined earlier, the notation \\(\hat{(.)}\\) represents the estimated value of \\((.)\\) and \\(\tilde{(.)}\\) is defined as the error mismatch between the estimated value and the true value as \\(\tilde{(.)} = \hat{(.)}- (.)\\).

In a similar manner \\(\tilde{(.)}\\) notation may be applied to the motion variables as
\\[ \tilde{\bm{\mathcal{X}}} = \bm{\mathcal{X}} - \bm{\mathcal{X}}\_d = - \bm{e}\_x \\]

The closed-loop dynamic formulation of the manipulator can be written as:
\\[ \ddot{\bm{\mathcal{X}}} = \bm{a}\_r + \bm{\eta}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}}, \bm{a}\_r) \\]
in which
\\[ \bm{\eta} = \bm{M}^{-1} \left( \tilde{\bm{M}} \bm{a}\_r + \tilde{\bm{C}} \dot{\bm{\mathcal{X}}} + \tilde{\bm{G}} \right) \\]
is a measure of modeling uncertainty.

<a id="org3cb985a"></a>

{{< figure src="/ox-hugo/taghirad13_robust_inverse_dynamics_task_space.png" caption="Figure 22: General configuration of robust inverse dynamics control implemented in the task space" >}}


#### Adaptive Inverse Dynamics Control {#adaptive-inverse-dynamics-control}

<a id="org2d5c44d"></a>

{{< figure src="/ox-hugo/taghirad13_adaptative_inverse_control_task_space.png" caption="Figure 23: General configuration of adaptative inverse dynamics control implemented in task space" >}}


### Motion Control in Joint Space {#motion-control-in-joint-space}

Although the motion control schemes developed in section [sec:control_task_space](#sec:control_task_space) are very effective for tracking performance, they suffer from an implementation constraint that the motion variable \\(\bm{\mathcal{X}}\\) must be measured in practice.<br />

If this measurement is available without any doubt, such topologies are among the best routines to be implemented in practice.
However, as explained in Section [sec:control_topology](#sec:control_topology), in many practical situations measurement of the motion variable \\(\bm{\mathcal{X}}\\) is difficult or expensive, and usually just the active joint variables \\(\bm{q}\\) are measured.
In such cases, the controllers developed in the joint space may be recommended for practical implementation.<br />

To generate a direct input to output relation in the joint space, consider the topology depicted in Figure [fig:general_topology_motion_feedback_bis](#fig:general_topology_motion_feedback_bis).
In this topology, the controller input is the joint variable error vector \\(\bm{e}\_q = \bm{q}\_d - \bm{q}\\), and the controller output is directly the actuator force vector \\(\bm{\tau}\\), and hence there exists a **one-to-one correspondence between the controller input to its output**.<br />

The general form of dynamic formulation of parallel robot is usually given in the task space.
For motion control in joint space, we need to transform the dynamic formulation in the joint space, by which the actuator forces \\(\bm{\tau}\\) are directly related to the active joint variables \\(\bm{q}\\).


#### Dynamic Formation in the Joint Space {#dynamic-formation-in-the-joint-space}

The relation between the task space variables to their counterparts in the joint space can be derived by forward and inverse kinematics relations.
Although both analyses involve solution to a set of non-linear equations, for parallel manipulators, inverse kinematic solution proves to be much easier to obtain than that of forward kinematic solution.<br />

This relation in **differential kinematics** is much simpler and can be completely determined by the Jacobian matrix:
\\[ \tcmbox{\dot{\bm{q}} = \bm{J} \dot{\bm{\mathcal{X}}} \Longrightarrow \dot{\bm{\mathcal{X}}} = \bm{J}^{-1} \dot{\bm{q}}} \\]

The acceleration variables are then:
\\[ \ddot{\bm{q}} = \dot{\bm{J}} \dot{\bm{\mathcal{X}}} + \bm{J} \ddot{\mathcal{X}} \Longrightarrow \ddot{X} = \bm{J}^{-1} \ddot{\bm{q}} - \bm{J}^{-1} \dot{\bm{J}} \dot{\bm{\mathcal{X}}} \\]

Furthermore, the relation between the actuator force vector \\(\bm{\tau}\\) to the corresponding task space wrench is given by:
\\[ \tcmbox{\bm{\mathcal{F}} = \bm{J}^T \bm{\tau} \Longrightarrow \bm{\tau} = \bm{J}^{-T} \bm{\mathcal{F}}} \\]

Substituting \\(\dot{\bm{\mathcal{X}}}\\) and \\(\ddot{\bm{\mathcal{X}}}\\) from the above equations into the dynamic formulation of the parallel robot gives:

\begin{equation\*}
  \begin{aligned}
    & \left( \bm{J}^{-T} \bm{M} \bm{J}^{-1} \right) \ddot{\bm{q}} \\\\\\
    & \quad + \bm{J}^{-T} \left( \bm{C} - \bm{M} \bm{J}^{-1} \dot{\bm{J}} \right) \bm{J}^{-1} \dot{\bm{q}} \\\\\\
    & \quad + \bm{J}^{-T} \bm{G} + \bm{J}^{-T} \bm{\mathcal{F}}\_d = \bm{\tau}
  \end{aligned}
\end{equation\*}

<div class="cbox">
  <div></div>

\begin{equation}
  \bm{M}\_q \ddot{\bm{q}} + \bm{C}\_q \dot{\bm{q}} + \bm{G}\_q + \bm{\tau}\_d = \bm{\tau} \label{eq:dynamics\_joint\_space}
\end{equation}

with:

\begin{subequations}
\begin{align}
  \bm{M}\_q =& \bm{J}^{-T} \bm{M} \bm{J}^{-1} \\\\\\
  \bm{D}\_q =& \bm{J}^{-T} \left( \bm{C} - \bm{M} \bm{J}^{-1} \dot{\bm{J}} \right) \bm{J}^{-1} \\\\\\
  \bm{G}\_q =& \bm{J}^{-T} \bm{G} \\\\\\
  \bm{\tau}\_q =& \bm{J}^{-T} \bm{\mathcal{F}}\_d
\end{align}
\end{subequations}

</div>

Equation [eq:dynamics_joint_space](#eq:dynamics_joint_space) represents the closed form dynamic formulation of a general parallel robot in the joint space.<br />

Note that the dynamic matrices are **not** explicitly represented in terms of the joint variable vector \\(\bm{q}\\).
In fact, to fully derive these matrices, the Jacobian matrices must be computed and are generally derived as a function of the motion variables \\(\bm{\mathcal{X}}\\).
Furthermore, the main dynamic matrices are all functions of the motion variable \\(\bm{\mathcal{X}}\\).
Hence, in practice, to find the dynamic matrices represented in the joint space, **forward kinematics** should be solved to find the motion variable \\(\bm{\mathcal{X}}\\) for any given joint motion vector \\(\bm{q}\\).<br />

Since in parallel robots the forward kinematic analysis is computationally intensive, there exist inherent difficulties in finding the dynamic matrices in the joint space as an explicit function of \\(\bm{q}\\).
In this case it is possible to solve forward kinematics in an online manner, it is recommended to use the control topology depicted in [fig:general_topology_motion_feedback_bis](#fig:general_topology_motion_feedback_bis), and implement control law design in the task space.<br />

However, one implementable alternative to calculate the dynamic matrices represented in the joint space is to use the **desired motion trajectory** \\(\bm{\mathcal{X}}\_d\\) instead of the true value of motion vector \\(\bm{\mathcal{X}}\\) in the calculations.
This approximation significantly reduces the computational cost, with the penalty of having mismatch between the estimated values of these matrices to their true values.


#### Decentralized PD Control {#decentralized-pd-control}

The first control strategy introduced in the joint space consists of the simplest form of feedback control in such manipulators.
In this control structure, depicted in Figure [fig:decentralized_pd_control_joint_space](#fig:decentralized_pd_control_joint_space), a number of PD controllers are used in a feedback structure on each error component.

The PD controller is denoted by \\(\bm{K}\_d s + \bm{K}\_p\\), where \\(\bm{K}\_d\\) and \\(\bm{K}\_p\\) are \\(n \times n\\) **diagonal** matrices denoting the derivative and proportional controller gains, respectively.<br />

<a id="orgef87ac5"></a>

{{< figure src="/ox-hugo/taghirad13_decentralized_pd_control_joint_space.png" caption="Figure 24: Decentralized PD controller implemented in joint space" >}}

By this structure, each tracking error component is **treated separately** by its disjoint PD controller.
The proposed decentralized PD controller is very simple in structure, and therefore very easy to be implemented on the manipulator.
The design of such a controller **needs no detailed information on the manipulator dynamic formulation and parameters**.
However, the tracking performance of such a controller is relatively poor, and **static tracking errors** might be unavoidable.
Also, the performance of the closed-loop system is configuration dependent.<br />

In practice, the gains are tuned experimentally and obtained as a trade-off between transient behavior and steady-state errors at different configurations.
As the dynamics of the system in the joint space is configuration dependent, finding suitable controller gains to result in required performance in all configurations is a difficult task.<br />

The performance of the controller to attenuate measurement noise and external disturbance wrenches are also poor in practice.
To remedy these shortcomings, some modifications have been proposed to this structure and further described.


#### Feedforward Control {#feedforward-control}

The tracking performance of the simple PD controller implemented in the joint space is usually not sufficient at different configurations.
To improve the tracking performance, a feedforward actuator force denoted by \\(\bm{\tau}\_{ff}\\) may be added to the structure of the controller as depicted in Figure [fig:feedforward_pd_control_joint_space](#fig:feedforward_pd_control_joint_space).

<a id="org7dd1247"></a>

{{< figure src="/ox-hugo/taghirad13_feedforward_pd_control_joint_space.png" caption="Figure 25: Feed forward actuator force added to the decentralized PD controller in joint space" >}}

The feedforward term is generated from the dynamic formulation of the manipulator.
The desired trajectory in the task space \\(\bm{\mathcal{X}}\_d\\) and its derivatives \\(\dot{\bm{\mathcal{X}}}\_d\\), \\(\ddot{\bm{\mathcal{X}}}\_d\\) are thus required.<br />

In practice, exact knowledge of dynamic matrices are not available, and therefore, estimates of these matrices are used in this derivation denoted by \\(\hat{\bm{M}}\\), \\(\hat{\bm{C}}\\) and \\(\hat{\bm{G}}\\).<br />

The information required to generate the feedforward actuator force \\(\bm{\tau}\_{ff}\\) is usually available beforehand, and in such a case, the feedforward term corresponding to a given trajectory can be **determined off-line**, while the computation of the decentralized feedback term would be executed online.<br />

If complete information of the dynamic matrices is available, and if we assume that the system is performing well, meaning that \\(\bm{\mathcal{X}}(t) \simeq \bm{\mathcal{X}}\_d(t)\\) and \\(\dot{\bm{\mathcal{X}}}(t) \simeq \dot{\bm{\mathcal{X}}}\_d(t)\\), we can write the closed loop dynamics as follow:
\\[ \bm{M}\_q \ddot{\bm{e}}\_q + \bm{K}\_d \dot{\bm{e}}\_q + \bm{K}\_p \bm{e}\_q = \bm{\tau}\_d \\]

The error dynamics satisfy a set of second-order differential equations in the presence of disturbance.
Therefore, by choosing appropriate gains of the PD controller, the transient and steady-state performance of the tracking error can be suitably designed.

Note that except for the mass matrix, the error dynamics terms are all **configuration independent**, and therefore, it is much easier to tune the PD controller gains to work well in the whole workspace of the robot in such a structure.<br />

However, this method suffers from a number of **limitations** in practice.
The most important limitation is the **stringent assumption of the complete information requirement of dynamics matrices**.
Furthermore, even is all the assumption hold, because of the configuration dependence of the mass matrix, the error dynamics is still not completely decoupled.
This means that correction in one component may be considered as a disturbance effect to the other components.
To overcome these limitations, the inverse dynamic approach has been developed and is given in the following section.


#### Inverse Dynamics Control {#inverse-dynamics-control}

As seen in the previous section, the tracking performance of a decentralized PD controller implemented in the joint space is not uniform at different configurations.
To compensate for such effects, a feedforward torque is added to the structure of the controller, by which the shortcomings of the decentralized controller is partially remedied.
However, the closed-loop performance still faces a number of limitations, which cannot be completely remedied because of the inherent conditions on feedforward structure of that proposed controller.
To overcome these limitations, in this section, a control technique based on **inverse dynamic feedback** of the manipulator in the joint space is presented.<br />

<div class="cbox">
  <div></div>

In the inverse dynamics control (IDC) strategy, the **nonlinear dynamics of the model is used to add a corrective term to the decentralized PD controller**.
By this means, the **nonlinear and coupling characteristics** of robotic manipulator is significantly **attenuated**, and therefore, the performance of linear controller is significantly improved.

</div>

The general structure of inverse dynamics control applied to a parallel manipulator in the joint space is depicted in Figure [fig:inverse_dynamics_control_joint_space](#fig:inverse_dynamics_control_joint_space).

A corrective torque \\(\bm{\tau}\_{fl}\\) is added in a **feedback** structure to the closed-loop system, which is calculated from the Coriolis and Centrifugal matrix, and the gravity vector of the manipulator dynamic formulation in the joint space.
Furthermore, the mass matrix is acting in the **forward path**, in addition to the desired trajectory acceleration \\(\ddot{\bm{q}}\_q\\).
Note that to generate this term, the **dynamic formulation** of the robot, and its **kinematic and dynamic parameters are needed**.
In practice, exact knowledge of dynamic matrices are not available, and there estimates are used.<br />

<a id="orgd592b03"></a>

{{< figure src="/ox-hugo/taghirad13_inverse_dynamics_control_joint_space.png" caption="Figure 26: General configuration of inverse dynamics control implemented in joint space" >}}

The controller output torque applied to the manipulator may be calculated by:

\begin{subequations}
\begin{align}
  \bm{\tau} &= \hat{\bm{M}}\_q \bm{a}\_q + \bm{\tau}\_{fl} \\\\\\
  \bm{\tau}\_{fl} &= \hat{\bm{C}}\_q \dot{\bm{q}} + \hat{\bm{G}}\_q \\\\\\
  \bm{a}\_q &= \ddot{\bm{q}}\_d + \bm{K}\_d \dot{\bm{e}}\_q + \bm{K}\_p \bm{e}\_q
\end{align}
\end{subequations}

If the knowledge of dynamic matrices is complete, the closed-loop dynamic formulation is simplified to:
\\[ \hat{\bm{M}}\_q \left( \ddot{\bm{e}}\_q + \bm{K}\_d \dot{\bm{e}}\_q + \bm{K}\_p \bm{e}\_q \right) + \bm{\tau}\_d = 0 \\]

This equation implies that if there exist complete knowledge of the dynamic matrices, the tracking error dynamic equation satisfies a set of second-order systems in the presence of disturbance.
Consider the case where no disturbance wrench is applied to the manipulator, as the mass matrix \\(\bm{M}\_q\\) is positive definite at all non-singular configurations, it can be inverted, and the error dynamics simplifies to:
\\[ \ddot{\bm{e}}\_q + \bm{K}\_d \dot{\bm{e}}\_q + \bm{K}\_p \bm{e}\_q = 0 \\]

This control technique is very popular in practice because of the fact that it can significantly **linearize** and **decouple** the dynamic formulation of the closed-loop system for error dynamics components.
Furthermore, the error dynamic terms are all **configuration independent**, and therefore, it is **much easier to tune the PD controller gains** to perform well in the whole workspace of the robot.<br />

However, note that for a good performance, an **accurate model of the system is required**, and the overall procedure is **not robust** to model uncertainties.


### Summary of Motion Control Techniques {#summary-of-motion-control-techniques}

In this section, a number of control techniques have been developed for parallel robots.
Based on the dynamic formulation given in Section [sec:dynamics](#sec:dynamics), many **model-based** control techniques have been developed for implementation in the task space as well as in the joint space.
These control techniques are presented from the simplest form of decentralized PD control to more advanced robust and adaptive inverse dynamics control.

A summary of these techniques is given below.


##### Dynamic Formulations {#dynamic-formulations}

The dynamic formulation of a parallel robot may be directly represented as a function of motion variable \\(\bm{\mathcal{X}}\\) in the task space as follows:

\begin{equation\*}
  \bm{M}(\bm{\mathcal{X}})\ddot{\bm{\mathcal{X}}} + \bm{C}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}})\dot{\bm{\mathcal{X}}} + \bm{G}(\bm{\mathcal{X}}) = \bm{\mathcal{F}} + \bm{\mathcal{F}}\_d
\end{equation\*}

The dynamic formulation may be represented as a function of actuator motion variable \\(\bm{q}\\) as

\begin{equation\*}
  \bm{M}\_q \ddot{\bm{q}} + \bm{C}\_q \dot{\bm{q}} + \bm{G}\_q = \bm{\tau} + \bm{\tau}\_d
\end{equation\*}

in which these two formulations are closely related to each other by the following relations:

\begin{equation\*}
  \begin{aligned}
    \bm{M}\_q &= \bm{J}^{-T} \bm{M} \bm{J}^{-1} \\\\\\
    \bm{C}\_q &= \bm{J}^{-T} \left( \bm{C} - \bm{M}\bm{J}^{-1}\dot{\bm{J}} \right) \bm{J}^{-1} \\\\\\
    \bm{D}\_q &= \bm{J}^{-T} \bm{G} \\\\\\
    \bm{\tau}\_q &= \bm{J}^{-T} \bm{\mathcal{F}}
  \end{aligned}
\end{equation\*}


##### Decentralized PD Control {#decentralized-pd-control}

The simplest controller for a parallel robot can be considered as a decentralized PD controller being implemented individually on each error component.
If such a structure is implemented in the task space, the control effort is calculated by

\begin{equation\*}
  \bm{\mathcal{F}} = \bm{K}\_d \dot{\bm{e}}\_x + \bm{K}\_p \bm{e}\_x
\end{equation\*}

and the actuator effort can be generally determined through a force distribution scheme.

For a completely parallel manipulator, the actuator forces can be generated by \\(\bm{\tau} = \bm{J}^{-T} \bm{\mathcal{F}}\\) at non-singular configurations.

Decentralized PD control can be directly implemented in the joint space by the following equation:

\begin{equation\*}
  \bm{\tau} = \bm{K}\_d \dot{\bm{e}}\_q + \bm{K}\_p \bm{e}\_q
\end{equation\*}


##### Feed Forward Control {#feed-forward-control}

The reduce the performance limitations of simple PD control, the control effort may be enforced with a feed forward wrench given by

\begin{equation\*}
  \bm{\mathcal{F}} = \bm{\mathcal{F}}\_{pd} + \bm{\mathcal{F}}\_{ff}
\end{equation\*}

in which

\begin{equation\*}
  \begin{aligned}
    \bm{\mathcal{F}}\_{ff} &= \bm{K}\_d \dot{\bm{e}}\_x + \bm{K}\_p \bm{e}\_x \\\\\\
                          &+ \hat{\bm{M}}(\bm{\mathcal{X}}\_d)\ddot{\bm{\mathcal{X}}}\_d + \hat{\bm{C}}(\bm{\mathcal{X}}\_d, \dot{\bm{\mathcal{X}}}\_d)\dot{\bm{\mathcal{X}}}\_d + \hat{\bm{G}}(\bm{\mathcal{X}}\_d)
  \end{aligned}
\end{equation\*}

where \\(\hat{\bm{M}}\\), \\(\hat{\bm{C}}\\) and \\(\hat{\bm{G}}\\) are estimation of the dynamic matrices.

This controller can be implemented in joint space as follows

\begin{equation\*}
  \begin{aligned}
    \bm{\tau} &= \bm{\tau}\_{pd} + \bm{\tau}\_{ff} \\\\\\
              &= \bm{K}\_d \dot{\bm{e}}\_q + \bm{K}\_p \bm{e}\_q + \bm{J}^{-T} \bm{\mathcal{F}}\_{ff}
  \end{aligned}
\end{equation\*}


##### Inverse Dynamics Control {#inverse-dynamics-control}

In the inverse dynamics control, the nonlinear dynamics of the model is used to add a corrective term to the decentralized PD controller.
If such a structure is implemented in the task space, the control effort is calculated by

\begin{equation\*}
  \begin{aligned}
    \bm{\mathcal{F}} &= \hat{\bm{M}}(\bm{\mathcal{X}})\bm{a} + \hat{\bm{C}}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}})\dot{\bm{\mathcal{X}}} + \hat{\bm{G}}(\bm{\mathcal{X}}) \\\\\\
    \bm{a} &= \ddot{\bm{\mathcal{X}}}\_d + \bm{K}\_d \dot{\bm{e}}\_x + \bm{K}\_p \bm{e}\_x
  \end{aligned}
\end{equation\*}

In general, the tracking error dynamics can be represented by

\begin{equation\*}
  \ddot{\bm{e}}\_x + \bm{K}\_d \dot{\bm{e}}\_x + \bm{K}\_p \bm{e}\_x + \hat{\bm{M}}^{-1} \left[ \tilde{\bm{M}} \ddot{\bm{\mathcal{X}}} + \tilde{\bm{C}} \dot{\bm{\mathcal{X}}} + \tilde{\bm{G}} + \bm{\mathcal{F}}\_d \right] = 0
\end{equation\*}

This controller can be implemented in the joint space as follows:

\begin{equation\*}
  \begin{aligned}
    \bm{\tau} &= \hat{\bm{M}}\_q \bm{a}\_q + \hat{\bm{C}}\_q \dot{\bm{q}} + \hat{\bm{G}}\_q \\\\\\
    \bm{a}\_q  &= \ddot{\bm{q}}\_d + \bm{K}\_d \dot{\bm{e}}\_q + \bm{K}\_p \bm{e}\_q
  \end{aligned}
\end{equation\*}

by which the tracking error dynamics is summarized as

\begin{equation\*}
  \ddot{\bm{e}}\_q + \bm{K}\_d \dot{\bm{e}}\_q + \bm{K}\_p \bm{e}\_q + \hat{\bm{M}}\_{q}^{-1} \left[ \tilde{\bm{M}}\_q \ddot{\bm{q}} + \tilde{\bm{C}}\_q \dot{\bm{q}} + \tilde{\bm{G}}\_q + \bm{\mathcal{\tau}}\_d \right] = 0
\end{equation\*}


##### Partial Linearization IDC {#partial-linearization-idc}

To reduce the computational cost of the inverse dynamic control, it is possible to use partial linearization of dynamic formulation, just by gravity compensation, while keeping asymptotic tracking stability of the closed-loop system.
In which a case, the control input wrench in the task space is simplified to

\begin{equation\*}
    \bm{\mathcal{F}} = \bm{K}\_d \dot{\bm{e}}\_x + \bm{K}\_p \bm{e}\_x + \hat{\bm{G}}(\bm{\mathcal{X}})
\end{equation\*}

The following Lyapunov function may be used to analyze the stability of tracking dynamics of the closed-loop system:

\begin{equation\*}
  \dot{V} = \dot{\bm{\mathcal{X}}}^T \bm{M} \ddot{\bm{\mathcal{X}}} + \frac{1}{2} \dot{\bm{\mathcal{X}}}^T \dot{\bm{M}} \dot{\bm{\mathcal{X}}} + \bm{e}\_x^T \bm{K}\_p \bm{e}\_x
\end{equation\*}

Stability analysis of the closed-loop system in this case reveals the fact that this simplified version of inverse dynamics control can lead to asymptotic tracking for constant desired trajectories.


##### Robust Inverse Dynamics Control {#robust-inverse-dynamics-control}

To accommodate modeling uncertainties in inverse dynamic control, the following robust control scheme in the task space is developed:

\begin{equation\*}
  \begin{aligned}
    \bm{\mathcal{F}} &= \hat{\bm{M}}(\bm{\mathcal{X}}) \bm{a}\_r + \hat{\bm{C}}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}})\dot{\bm{\mathcal{X}}} + \hat{\bm{G}}(\bm{\mathcal{X}}) \\\\\\
    \bm{a}\_r &= \ddot{\bm{\mathcal{X}}}\_d + \bm{K}\_d \dot{\bm{e}}\_x + \bm{K}\_p \bm{e}\_x + \bm{\delta}\_a
  \end{aligned}
\end{equation\*}

in which the robustifying corrective term \\(\bm{\delta}\_a\\) is found through a Lyapunov stability analysis of tracking error dynamics.
The tracking error dynamics can be represented by the following linear and nonlinear components:

\begin{equation\*}
  \bm{A} = \begin{bmatrix}
    \bm{0} & \bm{I} \\\\\\
    -\bm{K}\_p & -\bm{K}\_d
  \end{bmatrix}, \quad \bm{B} = \begin{bmatrix}
    \bm{0} \\ \bm{I}
  \end{bmatrix}
\end{equation\*}

The corrective term \\(\bm{\delta}\_a\\) can be found as

\begin{equation\*}
  \bm{\delta}\_a = \left\\{ \begin{matrix}
    - \rho \frac{v}{\\|v\\|} & \text{if} \\|v\\| > \epsilon \\\\\\
    - \rho \frac{v}{\epsilon} & \text{if} \\|v\\| \le \epsilon
  \end{matrix} \right.
\end{equation\*}

in which \\(v\\) is defined by \\(v = \bm{B}^T \bm{P} \bm{\epsilon}\\), where \\(\bm{P}\\) is the solution to the matrix Lyapunov equation and \\(\epsilon\\) is a smoothing threshold.
It is shown that by adding this corrective term to the regular inverse dynamics control, the closed-loop system achieves uniform ultimate bounded tracking errors.


##### Adaptive Inverse Dynamics Control {#adaptive-inverse-dynamics-control}

In the adaptive version of the inverse dynamics control, full feedback linearization is considered through adaptive update of dynamic formulation matrices.
The error dynamics in this case is

\begin{equation\*}
  \dot{\bm{\epsilon}} = \bm{A} \bm{\epsilon} + \bm{B} \bm{\Phi} \tilde{\bm{\theta}}
\end{equation\*}

in which

\begin{equation\*}
  \begin{aligned}
    \bm{A} &= \begin{bmatrix}
      \bm{0} & \bm{I} \\ -bm{K}\_p & -\bm{K}\_d
    \end{bmatrix}, \quad \bm{B} = \begin{bmatrix}
      \bm{0} \\ \bm{I}
    \end{bmatrix} \\\\\\
    \bm{\Phi} &= \hat{\bm{M}}^{-1} \bm{\Upsilon}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}}, \ddot{\bm{\mathcal{X}}})
  \end{aligned}
\end{equation\*}

Based on the Lyapunov stability analysis, by using the following Lyapunov function

\begin{equation\*}
  V = \bm{\epsilon}^T \bm{P} \bm{\epsilon} + \tilde{\bm{\theta}}^T \bm{\Gamma} \bm{\theta}
\end{equation\*}

the following parameter adaptation law is derived for updates

\begin{equation\*}
  \dot{\hat{\bm{\theta}}} = - \bm{\Gamma}^{-1} \bm{\Phi}^T \bm{B}^T \bm{P} \bm{\epsilon}
\end{equation\*}

By this means, the closed-loop system achieves asymptotic tracking performance, while the parameter estimation errors remain bounded.


### Motion Control of the Stewart-Gough Platform {#motion-control-of-the-stewart-gough-platform}


#### Control in the Task space {#control-in-the-task-space}

For the Stewart-Gough platform, the motion variable in the task space is a six-dimensional vector
\\[ \bm{\mathcal{X}} = \begin{bmatrix} \bm{x}\_p \\ \bm{\theta} \end{bmatrix} \\]
with:

-   \\(\bm{x}\_p = [x\_p\ y\_p\ z\_p]^T\\) is the position vector of the motion platform center of mass
-   \\(\bm{\theta} = \theta [s\_x\ s\_y\ s\_z]^T = [\theta\_x\ \theta\_y\ \theta\_z]^T\\) is the moving platform orientation expressed by screw coordinates

Therefore, the tracking error is defined as \\(\bm{e} = [e\_x\ e\_y\ e\_z\ e\_{\theta\_x}\ e\_{\theta\_y}\ e\_{\theta\_z}]^T\\).

The decentralized controller consists of six disjoint proportional derivative controllers acting on each error component and is denoted by \\(\bm{K}\_d s + \bm{K}\_p\\).<br />

The output of the controller is denoted by \\(\bm{\mathcal{F}} = [F\_x\ F\_y\ F\_z\ \tau\_x\ \tau\_y\ \tau\_z]^T\\).
Note that since the output of the controller is defined in the task space, each wrench component directly manipulates the corresponding tracking error component, and therefore, the overall tracking performance of the manipulator is suitable is high controller gains are used.<br />

In practice, the calculated output wrench is transformed into actuator forces through the force distribution block corresponding to the inverse of Jacobian transpose.<br />


#### Control in the Joint space {#control-in-the-joint-space}

The joint variable \\(\bm{q}(t)\\) is a six-dimensional vector consisting of the limb lengths denoted by \\(\bm{q} = [l\_1\ l\_2\ l\_3\ l\_4\ l\_5\ l\_6]^T\\).
Therefore, the tracking error is defined as \\(\bm{e}\_q = \bm{q}\_d - \bm{q}\\), in which is the desired motion variable in the joint space \\(\bm{q}\_d\\) is determined by the solution of inverse kinematics, and \\(\bm{q}\\) is given by direct measurement of the limb lengths.<br />

The decentralized controller, therefore, consists of six disjoint PD controllers acting on each error component.
The output of the controller directly generates the actuator torques denoted by \\(\tau\\).<br />

In simulation, it is observe that the orientation error significantly increase in the joint space control scheme.
The main reason is that the controller gains directly penalize the position error of the limb lengths, and not the orientation errors, and therefore, there is no direct controller action to be suitably tuned to reduce the orientation error.<br />

Comparing the closed-loop performance of the PD controllers designed in the joint space to those designed in the task space, it can be concluded that **tuning of the PD gains for a suitable performance is much easier in task space designs**.
Furthermore, a very small error signature in the joint space may be accumulated to produce relatively larger tracking errors in the task space.
Hence, it is recommended to design and implement controllers in the task space, if the required motion variables can be directly measured or the forward kinematic solution can be calculated in an online routine.


## Force Control {#force-control}

<a id="org7035f2e"></a>


### Introduction {#introduction}

In many applications, it may occur that the robot moving platform is in contact with a **stiff** environment and **specific interacting wrench is required**.
In such applications, the contact wrench describes the state of interaction more effectively than the position and orientation of the moving platform.

<div class="cbox">
  <div></div>

The problem of force control can be described as to derive the actuator forces required to generate a prescribed desired wrench at the manipulator moving platform, when the manipulator is carrying out its desired motion.

</div>

This problem and its extents are treated in the **force control** algorithms described in this chapter.
A force control strategy is one that modifies position trajectories based on the sensed wrench, or force-motion relations.<br />

If a pure **motion control** scheme is used for manipulator, in case it contacts an environment, the robot does not sense the presence of the environment, and its driving forces become harshly high to reduce the tracking errors.
In such a case, the robot may break the object it is in contact or will break its internal structure.
Additional sensors should be included in the manipulator in order for it to be able to feel the interaction and to control the interacting forces.
Different wrench sensors are developed for such applications, and it is possible to use joint torque or link force measurement units to determine the projection of the interacting forces in the joint space.<br />

The use of wrench sensors either in the task space or in the joint space open horizons to use different force control topologies for the manipulators.
Using such sensors does not imply that regular motion sensors used in motion control schemes are not necessary.
The use of motion sensors and the usual corresponding control topologies are usually necessary, since the motion of the manipulator is one of the outputs to be controlled.
Depending on the type and configuration of the wrench sensors, different force control topologies are developed.<br />


### Controller Topology {#controller-topology}

For a force control scheme, the desired interacting wrench of the moving platform and the environment may be of interest.
This quantity may be denoted by \\(\bm{\mathcal{F}}\_d\\), which has the same dimension and structure of the manipulator wrench \\(\bm{\mathcal{F}}\\).
To carry out such a control task in a closed-loop structure, it is necessary to **measure the output wrench** of the manipulator through an instrumentation system.<br />

Although there are many commercial six-degrees-of-freedom wrench sensors available in the market, they are usually more expensive than single joint force measurement units.
Another alternative for force measurement is **direct measurement of the actuator forces**.
Many commercial linear actuators are available in the market in which **embedded force measurement** is considered in their design.
Therefore, it might be preferable in some applications to use direct actuator force measurements to carry out the feedback control.


#### Cascade Control {#cascade-control}

In a general force control scheme, the **prime objective** is tracking of the interacting wrench between the moving platform and the environment.
However, note that the motion control of the robot when the robot is in interaction with the environment is also another **less-important objective** and when the contact of the robot moving platform is released, motion control becomes the prime objective.

<div class="cbox">
  <div></div>

To follow **two objectives** with different properties in one control system, usually a **hierarchy** of two feedback loops is used in practice.
This kind of control topology is called **cascade control**, which is used when there are **several measurements and one prime control variable**.
Cascade control is implemented by **nesting** the control loops, as shown in Figure [fig:cascade_control](#fig:cascade_control).
The output control loop is called the **primary loop**, while the inner loop is called the secondary loop and is used to fulfill a secondary objective in the closed-loop system.

</div>

<a id="orgc775ba2"></a>

{{< figure src="/ox-hugo/taghirad13_cascade_control.png" caption="Figure 27: Block diagram of a closed-loop system with cascade control" >}}

The measured variables are here the motion and interacting wrench that may be measured in the task space or in the joint space, and therefore, different control topologies may be advised for each set of measurement variables.<br />

To improve the performance of the control system for a particular objective, it is important to **choose the right variables for internal and external feedback loops**, and to **design suitable controllers for each feedback system**.
Although these differ in different topologies described in the following sections, some **general rules** are applied to design a well performing cascade control system.

A general idea in cascade control design is the **ideal case**, in which the inner loop is designed so tight that the secondary (inner) loop behaves as a **perfect servo**, and responds very quickly to the internal control command.
This idea is effectively used in many applications, wherein a nearly-perfect actuator to respond to the requested commands is designed by using an inner control feedback.

<div class="cbox">
  <div></div>

The design criteria for the inner loop is to have a high control gain such that the time response of the secondary variable is at least 5 times more than that of the primary variable, and such that it can overcome the effect of **disturbances** and **unmodelled dynamics** in the **internal feedback structure**.

</div>

It is also necessary to have a well-defined relation between the primary and secondary variables, to have harmony in the objectives followed in the primary and secondary loops.


#### Force Feedback in Outer Loop {#force-feedback-in-outer-loop}

Consider the force control schemes, in which **force tracking is the prime objective**.
In such a case, it is advised that the outer loop of cascade control structure is constructed by wrench feedback, while the inner loop is based on position feedback.
Since different types of measurement units may be used in parallel robots, different control topologies may be constructed to implement such a cascade structure.<br />

Consider first the cascade control topology shown in Figure [fig:taghira13_cascade_force_outer_loop](#fig:taghira13_cascade_force_outer_loop) in which the measured variables are both in the **task space**.
The inner loop is constructed by position feedback while the outer loop is based on force feedback.
As seen in Figure [fig:taghira13_cascade_force_outer_loop](#fig:taghira13_cascade_force_outer_loop), the force controller block is fed to the motion controller, and this might be seen as the **generated desired motion trajectory for the inner loop**.

The output of motion controller is also designed in the task space, and to convert it to implementable actuator force \\(\bm{\tau}\\), the force distribution block is considered in this topology.<br />

<a id="orgc0138a8"></a>

{{< figure src="/ox-hugo/taghira13_cascade_force_outer_loop.png" caption="Figure 28: Cascade topology of force feedback control: position in inner loop and force in outer loop. Moving platform wrench \\(\bm{\mathcal{F}}\\) and motion variable \\(\bm{\mathcal{X}}\\) are measured in the task space" >}}

Other alternatives for force control topology may be suggested based on the variations of position and force measurements.
If the force is measured in the joint space, the topology suggested in Figure [fig:taghira13_cascade_force_outer_loop_tau](#fig:taghira13_cascade_force_outer_loop_tau) can be used.
In this topology, the measured actuator force vector \\(\bm{\tau}\\) is mapped into its corresponding wrench in the task space by the Jacobian transpose mapping \\(\bm{\mathcal{F}} = \bm{J}^T \bm{\tau}\\).<br />

<a id="orgf060936"></a>

{{< figure src="/ox-hugo/taghira13_cascade_force_outer_loop_tau.png" caption="Figure 29: Cascade topology of force feedback control: position in inner loop and force in outer loop. Actuator forces \\(\bm{\tau}\\) and motion variable \\(\bm{\mathcal{X}}\\) are measured" >}}

Consider the case where the force and motion variables are both measured in the **joint space**.
Figure [fig:taghira13_cascade_force_outer_loop_tau_q](#fig:taghira13_cascade_force_outer_loop_tau_q) suggests the force control topology in the joint space, in which the inner loop is based on measured motion variable in the joint space, and the outer loop uses the measured actuator force vector.
In this topology, it is advised that the force controller is designed in the **task** space, and the Jacobian transpose mapping is used to project the measured actuator force vector into its corresponding wrench in the task space.
However, as the inner loop is constructed in the joint space, the desired motion variable \\(\bm{\mathcal{X}}\_d\\) is mapped into joint space using **inverse kinematic** solution.

Therefore, the structure and characteristics of the position controller in this topology is totally different from that given in the first two topologies.<br />

<a id="orgfe2ecb4"></a>

{{< figure src="/ox-hugo/taghira13_cascade_force_outer_loop_tau_q.png" caption="Figure 30: Cascade topology of force feedback control: position in inner loop and force in outer loop. Actuator forces \\(\bm{\tau}\\) and joint motion variable \\(\bm{q}\\) are measured in the joint space" >}}


#### Force Feedback in Inner Loop {#force-feedback-in-inner-loop}

Consider the force control scheme in which the **motion-force relation is the prime objective**.
In such a case, force tracking is not the primary objective, and it is advised that the outer loop of cascade control structure consists of a motion control feedback.

Since different type of measurement units may be used in parallel robots, different control topologies may be constructed to implement such cascade controllers.<br />

Figure [fig:taghira13_cascade_force_inner_loop_F](#fig:taghira13_cascade_force_inner_loop_F) illustrates the cascade control topology for the system in which the measured variables are both in the task space (\\(\bm{\mathcal{F}}\\) and \\(\bm{\mathcal{X}}\\)).
The inner loop is loop is constructed by force feedback while the outer loop is based on position feedback.
By this means, when the manipulator is not in contact with a stiff environment, position tracking is guaranteed through the primary controller.
However, when there is interacting wrench \\(\bm{\mathcal{F}}\_e\\) applied to the moving platform, this structure controls the force-motion relation.
This configuration may be seen as if the **outer loop generates a desired force trajectory for the inner loop**.<br />

<a id="org1509ffc"></a>

{{< figure src="/ox-hugo/taghira13_cascade_force_inner_loop_F.png" caption="Figure 31: Cascade topology of force feedback control: force in inner loop and position in outer loop. Moving platform wrench \\(\bm{\mathcal{F}}\\) and motion variable \\(\bm{\mathcal{X}}\\) are measured in the task space" >}}

Other alternatives for control topology may be suggested based on the variations of position and force measurements.
If the force is measured in the joint space, control topology shown in Figure [fig:taghira13_cascade_force_inner_loop_tau](#fig:taghira13_cascade_force_inner_loop_tau) can be used.
In such case, the Jacobian transpose is used to map the actuator force to its corresponding wrench in the task space.<br />

<a id="org4e33f80"></a>

{{< figure src="/ox-hugo/taghira13_cascade_force_inner_loop_tau.png" caption="Figure 32: Cascade topology of force feedback control: force in inner loop and position in outer loop. Actuator forces \\(\bm{\tau}\\) and motion variable \\(\bm{\mathcal{X}}\\) are measured" >}}

If the force and motion variables are both measured in the **joint** space, the control topology shown in Figure [fig:taghira13_cascade_force_inner_loop_tau_q](#fig:taghira13_cascade_force_inner_loop_tau_q) is suggested.
The inner loop is based on the measured actuator force vector in the joint space \\(\bm{\tau}\\), and the outer loop is based on the measured actuated joint position vector \\(\bm{q}\\).
In this topology, the desired motion in the task space is mapped into the joint space using **inverse kinematic** solution, and **both the position and force feedback controllers are designed in the joint space**.
Thus, independent controllers for each joint may be suitable for this topology.

<a id="org159af61"></a>

{{< figure src="/ox-hugo/taghira13_cascade_force_inner_loop_tau_q.png" caption="Figure 33: Cascade topology of force feedback control: force in inner loop and position in outer loop. Actuator forces \\(\bm{\tau}\\) and joint motion variable \\(\bm{q}\\) are measured in the joint space" >}}


### Stiffness Control {#stiffness-control}


#### Single-Degree-of-Freedom Stiffness Control {#single-degree-of-freedom-stiffness-control}


#### General Stiffness Control {#general-stiffness-control}


#### Stiffness Contorl of the Stewart-Gough Platform {#stiffness-contorl-of-the-stewart-gough-platform}


### Direct Force Control {#direct-force-control}

<a id="orgb779c33"></a>

{{< figure src="/ox-hugo/taghira13_direct_force_control.png" caption="Figure 34: Direct force control scheme, force feedback in the outer loop and motion feedback in the inner loop" >}}


### Impedance Control {#impedance-control}

For the stiffness control and direct force control schemes, it is observed that when the manipulator-moving platform is in contact with a stiff environment, the motion variable \\(\bm{\mathcal{X}}\\) and the interacting force variable \\(\bm{\mathcal{F}}\\) are two dynamically **dependent** quantities.<br />

In stiffness control, it is aimed to adjust the **static** relation between these two quantities.
In this scheme, no force measurement is required, however, careful design on the desired motion trajectory and PD controller gains is needed to tune the stiffness property of the interaction at steady stage.<br />

In force control schemes, on the other hand, the force tracking is the prime objective, and force measurement is a stringent requirement to implement such schemes.

The main reason that the motion and force variables are not being gable to be controlled independently is that for an n-degrees-of-freedom manipulator, only n-independent control inputs are available, and therefore, only n-independent variables can be controlled, while the force and motion quantities count to \\(2n\\) independent variables.<br />

<div class="cbox">
  <div></div>

The key idea behind **impedance control** schemes, is to **tune the dynamic relation between the force and the motion variables**, and not a hierarchy of tracking objectives in force and in position variables.
In this scheme, contrary to stiffness control schemes, **both force and position variables are measured** and used in the control structure.

</div>

The definition of mechanical **impedance** is given in an analogy of the well-known electrical impedance definition as the **relationship between the effort and flow variables**.
Since this relation can be well determined in the frequency domain, the dynamical relation of force and motion variable may be represented by mechanical impedance.
Impedance control schemes provide control topology to tune the mechanical impedance of a system to a desired value.
By this means, the force and the motion variables are not controlled independently, or in a hierarchy, but their dynamic relation represented by mechanical impedance is suitably controlled.


#### Impedance {#impedance}

Impedance was first defined in electrical networks as the measure of the opposition that an electrical circuit presents to the passage of a current when a voltage is applied.
To **generalize** the impedance definition to other disciplines, voltage is generalized to the **effort** and current is generalized to the **flow**.

Impedance is a **complex** function defined as the ratio of the Laplace transform of the effort to the Laplace transform of the flow.<br />

Impedance is usually denoted by \\(\bm{Z}(s)\\) and it may be represented by writing its magnitude and phase in the form of \\(\abs{\bm{Z}(s)}\\) and \\(\angle{\bm{Z}(s)}\\).
The magnitude of the complex impedance \\(\abs{\bm{Z}}\\) is the ratio of the effort amplitude to that of the flow, while the phase \\(\angle{\bm{Z}}\\) is the phase shift by which the flow is ahead of the effort.

<div class="cbox">
  <div></div>

Mechanical Impedance is defined as the ratio of the Laplace transform of the mechanical effort to the Laplace transform of the mechanical flow:

\begin{equation}
  \bm{Z}(s) = \frac{\bm{F}(s)}{\bm{v}(s)}
\end{equation}

in which effort in mechanical systems is represented by force \\(\bm{F}\\) and flow is represented by velocity \\(\bm{v}\\).

</div>

Note that this definition is given for a single-degree-of-freedom motion system.
The motion can be generalized to angular motion, in which the effort is represented by torque, while the flow is represented by angular velocity.
Furthermore, the impedance may be generalized to multiple-degrees-of-freedom system, in which for a general spatial motion effort is represented by a wrench \\(\bm{\mathcal{F}}\\), while flow is represented by motion twist \\(\dot{\bm{\mathcal{X}}}\\).<br />

Nevertheless, note that Laplace transform is only applicable for **linear time invariant** systems, and for a parallel manipulator the dynamic formulation of which is nonlinear, the concept of mechanical impedance may be extended to the differential equation relating the mechanical wrench \\(\bm{\mathcal{F}}\\) to motion twist \\(\dot{\bm{\mathcal{X}}}\\).

<div class="cbox">
  <div></div>

Consider an RLC circuit depicted in Figure [fig:taghirad13_impedance_control_rlc](#fig:taghirad13_impedance_control_rlc).
The differential equation relating voltage \\(v\\) to the current \\(i\\) is given by
\\[ v = L\frac{di}{dt} + Ri + \int\_0^t \frac{1}{C} i(\tau)d\tau \\]
in which \\(L\\) denote the inductance, \\(R\\) the resistance and \\(C\\) the capacitance.

The impedance of the system may be found from the Laplace transform of the above equation:

\begin{equation}
  Z(s) = \frac{v(s)}{i(s)} = Ls + R + \frac{1}{Cs} \label{eq:rlc\_impedance}
\end{equation}

</div>

<div class="cbox">
  <div></div>

Consider the mass-spring-damper system depicted in Figure [fig:taghirad13_impedance_control_rlc](#fig:taghirad13_impedance_control_rlc).
The governing dynamic formulation for this system is given by
\\[ m \ddot{x} + c \dot{x} + k x = f \\]
in which \\(m\\) denote the body mass, \\(c\\) the damper viscous coefficient and \\(k\\) the spring stiffness.

The impedance of the system may be found from the Laplace transform of the above equation:

\begin{equation}
  Z(s) = \frac{f(s)}{v(s)} = ms + c + \frac{k}{s} \label{eq:mass\_spring\_damper\_impedance}
\end{equation}

</div>

<a id="orge490c94"></a>

{{< figure src="/ox-hugo/taghirad13_impedance_control_rlc.png" caption="Figure 35: Analogy of electrical impedance in (a) an electrical RLC circuit to (b) a mechanical mass-spring-damper system" >}}

As inferred from the above two examples, although the physical nature of the system may differ from each other, they may be represented by similar impedances.
From this analogy, a terminology for impedance is introduced.

<div class="cbox">
  <div></div>

An impedance \\(\bm{Z}(s)\\) is called

-   **Inductive** if \\(\abs{\bm{Z}(0)} = 0\\)
-   **Resistive** if \\(\abs{\bm{Z}(0)} = R\\)
-   **Capacitive** if \\(\lim\_{s\to 0} \abs{\bm{K}(s)} = \infty\\)

</div>

Hence, for the mechanical system represented in Figure [fig:taghirad13_impedance_control_rlc](#fig:taghirad13_impedance_control_rlc):

-   mass represents inductive impedance
-   viscous friction represents resistive impedance
-   spring stiffness represents capacitive impedance

The environments that a robot interacts with may be represented by these classified impedance components.
A very stiff environment may be represented by high-capacitive impedance models, whereas an environment with high structural damping may be represented by high-resitive impedance.


#### Impedance Control Concept {#impedance-control-concept}

The key idea behind impedance control schemes is to tune the dynamic relation between force and motion variables.
Impedance control schemes provide control topology to tune the mechanical impedance of a system toward a desired impedance.

A desired impedance could be adjusted by desired inductive, resistive and capacitive impedances, which forms a desired linear impedance for the closed-loop system as follows:
\\[ \bm{Z}\_d(s) = \bm{M}\_d s + \bm{C}\_d + \frac{1}{s} \bm{K}\_d \\]
where \\(\bm{Z}\_d\\) denotes the desired impedance of the closed-loop system, which is composed of the desired inductive impedance \\(\bm{M}\_d\\), desired resistive impedance \\(\bm{C}\_d\\) and desired capacitive impedance \\(\bm{K}\_d\\).
Impedance control structures may be used to tune the closed-loop impedance of the system suitably to follow such a desired impedance.


#### Impedance Control Structure {#impedance-control-structure}

Consider a parallel robot with multiple-degrees-of-freedom interacting with a stiff environment.
In such a case, the motion of the robot end effector is represented by the motion vector \\(\bm{\mathcal{X}}\\), and the interacting wrench applied to the robot end effector is denoted by \\(\bm{\mathcal{F}}\_e\\).
It is considered that the interacting wrench is measured in the task space and is used in the inner force feedback loop.
Furthermore, it is considered that the motion variable \\(\bm{\mathcal{X}}\\) is measured and is used in the outer feedback loop.<br />

In the impedance control scheme, **regulation of the motion-force dynamic relation is the prime objective**, and since force tracking is not the primary objective, it is advised to used a cascade control structure with motion control feedback in the outer loop and force feedback in the inner loop.

Therefore, when the manipulator is not in contact with a stiff environment, position tracking is guaranteed by a primary controller.
However, when there is an interacting wrench \\(\bm{\mathcal{F}}\_e\\) applied to the moving platform, this structure may be designed to control the force-motion dynamic relation.<br />

As a possible impedance control scheme, consider the closed-loop system depicted in Figure [fig:taghira13_impedance_control](#fig:taghira13_impedance_control), in which the position feedback is considered in the outer loop, while force feedback is used in the inner loop.
This structure is advised when a desired impedance relation between the force and motion variables is required that consists of desired inductive, resistive, and capacitive impedances.
As shown in Figure [fig:taghira13_impedance_control](#fig:taghira13_impedance_control), the motion-tracking error is directly determined from motion measurement by \\(\bm{e}\_x = \bm{\mathcal{X}}\_d - \bm{\mathcal{X}}\\) in the outer loop and the motion controller is designed to satisfy the required impedance.

Moreover, direct force-tracking objective is not assigned in this control scheme, and therefore the desired force trajectory \\(\bm{\mathcal{F}}\_d\\) is absent in this scheme.
However, an auxiliary force trajectory \\(\bm{\mathcal{F}}\_a\\) is generated from the motion control law and is used as the reference for the force tracking.
By this means, no prescribed force trajectory is tracked, while the **motion control scheme would advise a force trajectory for the robot to ensure the desired impedance regulation**.<br />

<a id="org9902c61"></a>

{{< figure src="/ox-hugo/taghira13_impedance_control.png" caption="Figure 36: Impedance control scheme; motion feedback in the outer loop and force feedback in the inner loop" >}}

The required wrench \\(\bm{\mathcal{F}}\\) in the impedance control scheme, is based on inverse dynamics control and consists of three main parts.
In the inner loop, the force control scheme is based on a feedback linearization part in addition to a mass matrix adjustment, while in the outer loop usually a linear motion controller is considered based on the desired impedance requirements.

Although many different impedance structures may be considered as the basis of the control law, in Figure [fig:taghira13_impedance_control](#fig:taghira13_impedance_control), a linear impedance relation between the force and motion variables is generated that consists of desired inductive \\(\bm{M}\_d\\), resistive \\(\bm{C}\_d\\) and capacitive impedances \\(\bm{K}\_d\\).<br />

According to Figure [fig:taghira13_impedance_control](#fig:taghira13_impedance_control), the controller output wrench \\(\bm{\mathcal{F}}\\), applied to the manipulator may be formulated as
\\[ \bm{\mathcal{F}} = \hat{\bm{M}} \bm{M}\_d^{-1} \bm{e}\_F + \bm{\mathcal{F}}\_{fl} \\]
with:

\begin{align\*}
  \bm{e}\_F &= \bm{\mathcal{F}}\_a - \bm{\mathcal{F}}\_m \\\\\\
  \bm{\mathcal{F}}\_a &= \bm{M}\_d \ddot{\bm{\mathcal{X}}}\_{d} + \bm{C}\_{d} \dot{\bm{e}}\_{x} + \bm{K}\_{d} \bm{e}\_{x}
\end{align\*}

\\(\bm{M}\_d\\) denotes the desired inductive impedance, \\(\bm{C}\_d\\) the desired resistive impedance and \\(\bm{K}\_d\\) is desired capacitive impedance matrices.

The feedback linearizing term is given by:
\\[ \bm{\mathcal{F}}\_{fl} = \hat{\bm{C}}(\bm{\mathcal{X}}, \dot{\bm{\mathcal{X}}}) \dot{\bm{\mathcal{X}}} + \hat{\bm{G}}(\bm{\mathcal{X}}) + \bm{\mathcal{F}}\_m \\]

If the information on the dynamic matrices is complete, and if the force measurements are noise free (\\(\bm{\mathcal{F}}\_m = \bm{\mathcal{F}}\_e\\)), the closed-loop dynamic formulation simplifies to:
\\[ \bm{M}\_d \ddot{\bm{e}}\_x + \bm{C}\_d \dot{\bm{e}}\_x + \bm{K}\_d \bm{e}\_x = \bm{\mathcal{F}}\_e \\]

And thus the closed-loop error dynamic equation satisfies a set of second-order systems with the desired impedance coefficients in relation to the interacting force.
In other words, **the control structure guarantees that the force and motion relation follows a desired impedance**.
Therefore, by choosing appropriate impedance matrices, the transient performance of the force-motion relation can be shaped so as to have a fast but stable interaction.
By this means, what is controlled is the dynamic relation between the force and motion variables, and not directly the position.
However, if the robot is moving freely in space and has no interaction with the environment, \\(\bm{\mathcal{F}}\_e = 0\\), the closed-loop system will provide a suitable motion tracking thanks to the motion controller in the outer loop.<br />

The impedance control scheme is very popular in practice, wherein tuning the force and motion relation in a robot manipulator interacting with a stiff environment is the prime objective.
However, note that for a good performance, an accurate model of the system is required, and the obtained force and motion dynamics are not robust to modeling uncertainty.

# Bibliography
<a id="taghirad13_paral"></a>Taghirad, H., *Parallel robots : mechanics and control* (2013), Boca Raton, FL: CRC Press. [↩](#1a9683d2a60e031fa9b8ed16f4944c9f)
