# Codingame Solution: Inverse Kinematics of a Stewart Platform

In industry and science often hydraulic Stewart Platforms are deployed for their robustness, fast acceleration and retention force, but only few publications focus on a cheap design using electric motors. In recent years many hobbyists built Stewart Platforms, but either they are not documented well or they are built to have a proof of concept without much theory behind. With this article I want to close this gap, derive the inverse kinematics of the Stewart Platform using cheap servo motors and provide a software library to visualize and build own platforms.

The library is used here for the simulation of a platform that is capable to follow paths like skewed squares (press q), circles (press w), eight-figures (press e), rotate (press r), tilt (press t), draw lissajous-figures (press y), simulate breath (press b), get attached to the mouse cursor (press m), do a helical sweep (press h), get controlled with a Playstation or Xbox controller (plug in your controller and press g) you have laying around, get controlled via a Leap Motion sensor and to plot SVG paths (click on the icons) in a CNC fashion.

## Previous Work

The Stewart Platform has been studied a lot since its first analysis by Stewart in 1965 [Stewart1965], who wanted to create a simple platform with six dimensions of freedom that is applicable to a variety of fields.

Originally with the focus on flight condition simulation, simulation of space conditions and tire testing, the Stewart Platform became a versatile tool in the simulation of earth quakes or waves, as the end effector of wheel loaders [Großmann2012], in animatronics, as the basis for telescopes to follow stars or to level DSLR cameras, but the largest application of Stewart Platforms has been in medicine, robotics and precise tool movement.

In medicine, people got really creative and so the Stewart Platform is used for surgical training by simulating breathing [Patel2018] and heartbeats, tremor-compensation for surgery [Yang2015], fixation devices in deformity correction [Paley2011] or ankle rehabilitation apparatuses that simulates running exercises [Girone2001].

In summary, it can be said that the platform exists in many different ways and can always be used when extremely high flexibility is required and even if that is not enough, there is an extension using two concentric rails and double spherical joints to increase the workspace [Coulombe2013].

## Inverse Kinematics of a Stewart Platform

The inverse kinematics of a Stewart Platform is the calculation of the leg length given the position of the platform. The forward kinematic is not distinct and can only be determined with additional constraints or sensor input and is not covered here.

The Stewart Platform consists of two frames, the base frame and the platform frame that are connected with six variable length legs. With this setup the platform can be moved in three translational dimensions and three rotational dimensions.

The translation \(^p\mathbf{T}_b\) from the base origin \(\;^b\mathcal{O}\) to the platform origin \(\;^p\mathcal{O}\) can be described with a single vector \(\mathbf{T}=(t_x, t_y, t_z)^T\in\mathbb{R}^3\). The rotation of the platform \(^p\mathbf{R}_b\) is described by the unit quaternion \(\mathbf{R}\in\mathbb{H}\). This means that the following relationship for the frame of reference can be stated:

\[\begin{array}{rl}^p\mathbf{T}_b =& \mathbf{T}\\^p\mathbf{R}_b =& \mathbf{R}\\^b\mathbf{T}_p =& -(^p\mathbf{T}_b)=-\mathbf{T}\\^b\mathbf{R}_p =& (^p\mathbf{R}_b)^{-1}=\overline{\mathbf{R}}\end{array}\]

When we now focus on a single leg \(k\) with an anchor point \(\mathbf{B}_k\) on the base plate and an anchor point \(\mathbf{P}_k\) on the platform in the platform frame, described by the vectors \(\mathbf{b}_k\) and \(\mathbf{p}_k\) in their own frame, the vector \(\mathbf{q}_k\) from \(\;^b\mathcal{O}\) to \(\mathbf{P}_k\) is

\[\mathbf{q}_k=\;^p\mathbf{T}_b + \;^p\mathbf{R}_b\cdot\mathbf{p}_k = \mathbf{T} + \underbrace{\mathbf{R}\times\mathbf{p}_k\times\overline{\mathbf{R}}}_{\text{quaternion rotation}}\]

The leg with length \(|\mathbf{l}_k|\) can therefore be found with

\[\mathbf{l}_k = \mathbf{P}_k - \mathbf{B}_k = \mathbf{T} + \mathbf{R}\times\mathbf{p}\times\overline{\mathbf{R}} - \mathbf{b}_k\]

For a hydraulic Stewart Platform this is all that is needed. The length \(|\mathbf{l}_k|\) defined as the Euclidean \(L_2\) norm can be used as the input for a controller. It is more complicated when we use a rotational servo motor with a servo horn.

## Inverse Kinematics using Rotational Servo Motors

Instead of having a leg with a variable length \(|\mathbf{l}_k|\), we now use a fixed rod of length \(|\mathbf{d}|\) between the servo horn anchor \(\mathbf{H}_k\) and the platform anchor point \(\mathbf{P}_k\). The anchor \(\mathbf{H}_k\) has a distance of \(|\mathbf{h}|\) from the original base anchor and servo shaft \(\mathbf{B}_k\). The vector \(\mathbf{h}\) is perpendicular to the servo shaft \(\mathbf{s}_k\) and is rotated by angle \(\alpha_k\) when lifted from the horizontal line.

When we now look from top onto the base, each servo can be rotated by the angle \(\beta_k\) in addition to its position \(\mathbf{B}_k\). The servo shaft \(\mathbf{s}_k\) lives in the x-y plane and is orthogonal to the vector \(\mathbf{h}\).

The anchor \(\mathbf{H}_k\) can now be calculated as we rotate around the z-axis with angle \(\beta_k\) and as shown in the sketch, we need to rotate by \(-\alpha_k\) around the y-axis to lift the servo horn which lies in the local x-axis.

\[\begin{array}{rl}\mathbf{H}_k &= \mathbf{B}_k + \mathbf{R}_z(\beta_k)\mathbf{R}_y(-\alpha_k)\left(\begin{array}{c}|\mathbf{h}|\\0\\0\end{array}\right) \\&= \mathbf{B}_k + \underbrace{|\mathbf{h}|\left(\begin{array}{c}\cos(\alpha_k)cos(\beta_k)\\\cos(\alpha_k)sin(\beta_k)\\\sin(\alpha_k)\end{array}\right)}_{=\mathbf{h}}\end{array}\]

When we do the same with the servo arm when it is on the opposite and we get

\[\begin{array}{rl}\mathbf{H}_k &= \mathbf{B}_k + \mathbf{R}_z(\beta_k)\mathbf{R}_y(\pi-\alpha_k)\left(\begin{array}{c}-|\mathbf{h}|\\0\\0\end{array}\right) \\& = \mathbf{B}_k + |\mathbf{h}|\left(\begin{array}{c}\cos(\alpha_k)cos(\beta_k)\\\cos(\alpha_k)sin(\beta_k)\\\sin(\alpha_k)\end{array}\right)\end{array}\]

It follows for the position of the anchor that it does not matter what value \(\beta_k\) has and in which direction the servo is aligned - it will always rotate upwards! When we now square the lengths of \(|\mathbf{h}|, |\mathbf{d}|, |\mathbf{l}_k|\) we get the relationship

\[\begin{array}{rl}|\mathbf{h}|^2 &= (\mathbf{H}_k-\mathbf{B}_k)^T(\mathbf{H}_k-\mathbf{B}_k)\\|\mathbf{d}|^2 &= (\mathbf{P}_k-\mathbf{H}_k)^T(\mathbf{P}_k-\mathbf{H}_k)\\|\mathbf{l}_k|^2 &= (\mathbf{P}_k-\mathbf{B}_k)^T(\mathbf{P}_k-\mathbf{B}_k)\\\end{array}\]

By subtracting the equations from another, factoring again and substituting \(\mathbf{H}_k\) we derived previously, we get

\[\begin{array}{rl}|\mathbf{l}_k|^2 - (|\mathbf{d}|^2 - |\mathbf{h}|^2) &= 2 \mathbf{B}_k^T\mathbf{B}_k - 2 \mathbf{B}_k^T \mathbf{H}_k - 2 \mathbf{B}_k^T \mathbf{P}_k + 2 \mathbf{H}_k^T \mathbf{P}_k \\&= 2(\mathbf{H}_k-\mathbf{B}_k)^T(\mathbf{P}_k-\mathbf{B}_k)\\&= 2|\mathbf{h}| \left(\begin{array}{c}\cos(\alpha_k)cos(\beta_k)\\\cos(\alpha_k)\sin(\beta_k)\\\sin(\alpha_k)\end{array}\right)^T\mathbf{l}_k\\&= 2|\mathbf{h}|\sin(\alpha_k)\mathbf{l}_k^{(z)} + 2|\mathbf{h}|\cos(\alpha_k) (\cos(\beta_k)\mathbf{l}_k^{(x)} + \sin(\beta_k)\mathbf{l}_k^{(y)})\end{array}\]

We now use the trigonometric identity \(e\cdot\sin\varphi+f\cdot\cos\varphi=\sqrt{e^2+f^2}\sin(\varphi+\text{atan2}(f, e))\), from which is obvious that

\[\begin{array}{rl}e_k &= 2|\mathbf{h}|\mathbf{l}_k^{(z)}\\f_k &=2|\mathbf{h}| \left(\cos(\beta_k)\mathbf{l}_k^{(x)} + \sin(\beta_k)\mathbf{l}_k^{(y)}\right)\\g_k &= |\mathbf{l}_k|^2 - (|\mathbf{d}|^2 - |\mathbf{h}|^2)\end{array}\]

And therefore the inverse kinematics that is reduced to calculating each servo angle is

\[\begin{array}{rrl}& g_k &= e_k\cdot\sin\alpha_k+f_k\cdot\cos\alpha_k\\& &= \sqrt{e_k^2+f_k^2}\sin(\alpha_k+\text{atan2}(f_k, e_k))\\\Leftrightarrow &\frac{g_k}{\sqrt{e_k^2+f_k^2}} &= \sin(\alpha_k+\text{atan2}(f_k, e_k))\\\Leftrightarrow &\alpha_k &= \sin^{-1}\left(\frac{g_k}{\sqrt{e_k^2+f_k^2}}\right) - \text{atan2}(f_k, e_k)\\\end{array}\]

## Arrangement of servo motors

The theory allows to position the servos and anchors on the platform on arbitrary places in the plane. However, to be more practically, we define the Stewart platform to be circular and hexagonal.

### Circular Platform arrangement

Looking from top, the simplest arrangement of the anchors on a Stewart Platform is circular where pairs of platform anchors are placed every \(120^\circ\) beginning with \(60^\circ\) on a platform radius of \(\;^pr\) and each pair of base anchors are placed every \(120^\circ\) beginning with \(0^\circ\) on a base radius \(\;^br\). Each pair of platform anchors has a radial distance of \(\;^p\Delta\) and each pair of base anchors has a radial distance of \(\;^b\Delta\).

The angle \(\;^p\varphi_k\) to a platform anchor and \(\;^b\varphi_k\) to a base anchor is thus

\[\begin{array}{rl}\;^p\varphi_k =& \frac{2\pi}{3}\left\lfloor\frac{k}{2}\right\rfloor - (-1)^k \cdot \frac{\;^p\Delta}{2} + \frac{\pi}{3}\\\;^b\varphi_k =& \frac{2\pi}{3}\left\lfloor\frac{k+1}{2}\right\rfloor +(-1)^k \cdot \frac{\;^b\Delta}{2}\end{array}\]

The last missing piece is the rotation \(\beta_k\), which we define to be tangential to the base circle on each base anchor \(\mathbf{B}_k\) looking away from the group, which is

\[\beta_k = \;^b\varphi_k + \frac{\pi}{2} (-1)^k\]

### Hexagonal Platform arrangement

A better design for a Stewart Platform is using the typical hexagonal plates. However, instead of having one possible setup, we can imagine 16 where the top plate, here indicated by the three red anchor points can be smaller or larger than the base plate, the motors can be rotated inwards or outwards and the same for the servo horns. Additionally we can attach the motors on the motor sides or the sides in between.

We first focus on how to construct a hexagonal plate. To be able to parametrize the plate freely, we describe the hexagon by two equilateral triangles defined by their inscribed circles.

In general, the radius of the circumscribed circle of an equilateral triangle is \(R=2r\), given the radius \(r\) of its inscribed circle. Additionally, the height of an equilateral triangle is \(h = \frac{\sqrt{3}}{2}a\), given the side length \(a\). When we now look at the smaller triangle whose inscribed radius \(r_i\) is \(\leq\) the circumscribed radius \(r_o\) of the larger circle, the height \(h_\triangle\) of its little pyramidion is \(h_\triangle = R_i - r_o\), which is the radius of the circumscribed circle of the inner triangle minus the radius of the inscribed circle of the outer triangle. It follows that half of the side length of the small pyramidion is \(\frac{a_\triangle}{2} = \frac{2r_i-r_o}{\sqrt{3}}\). When we now pretend to draw the smaller triangle again with a radius reduced to the radius of the inscribed circle of the larger triangle and draw orthogonal from there at distance \(\pm\frac{a_\triangle}{2}\), we can easily construct the polygon to form the hexagonal plate with its edges \(\mathbf{e}_k\) - two at a time. Or expressed individually and more formally with \(\gamma_k=\frac{2\pi}{3}\left\lfloor\frac{k}{2}\right\rfloor\):

\[\begin{array}{rl}\mathbf{e}_k &= r_o\left(\begin{array}{c}\cos(\gamma_k)\\\sin(\gamma_k)\end{array}\right) + \frac{a_\triangle}{2} \left(\begin{array}{c}\cos(\gamma_k - (-1)^{k}\frac{\pi}{2})\\\sin(\gamma_k - (-1)^{k}\frac{\pi}{2})\end{array}\right)\\&= r_o\left(\begin{array}{c}\cos(\gamma_k)\\\sin(\gamma_k)\end{array}\right) + \frac{(-1)^ka_\triangle}{2}\left(\begin{array}{c}\sin(\gamma_k)\\-\cos(\gamma_k)\end{array}\right)\\\end{array}\]

Now always two \(\mathbf{b}_k\) are arranged between every second pair of \(\mathbf{e}_k\). If you want to 3D print the plate, I created an OpenScad sketch for it.

## Relative height measure

The height of the platform from the base can be variable and as such be defined with an absolute \(\mathbf{T}\). Defining the absolute translation \(\mathbf{T}\) with an offset \(\mathbf{T}_0\) and only input relative \(\mathbf{t}\) such that \(\mathbf{T}=\mathbf{T}_0+\mathbf{t}\) makes it easier to optimize the platform. In this way we can state \(\mathbf{T}_0\) to be optimal in terms of torque that can be applied to the rod, which is the case when rod and servo horn are orthogonal.

We previously said that \(\mathbf{q}_k=\;^p\mathbf{T}_b + \;^p\mathbf{R}_b\cdot\mathbf{p}_k\) but since we don't apply any rotation or translation to find the best offset, the vector \(\mathbf{q}_0 = \mathbf{T}_0 + \mathbf{p}_k\). Since we want the rod and horn orthogonal, it follows that

\[\begin{array}{rl}|\mathbf{d}|^2+|\mathbf{h}|^2 &= |\mathbf{l}_k|^2\\& = (\mathbf{q}_k^{(x)} - \mathbf{b}_k^{(x)})^2 + (\mathbf{q}_k^{(y)} - \mathbf{b}_k^{(y)})^2 + (\mathbf{q}_k^{(z)} - \mathbf{b}_k^{(z)})^2 \\& = (\mathbf{p}_k^{(x)} - \mathbf{p}_k^{(x)})^2 + (\mathbf{p}_k^{(y)} - \mathbf{b}_k^{(y)})^2 + z^2 \\\end{array}\]

This states that the initial height \(z\) of the platform for \(\mathbf{T}_0 =[0, 0, z]^T\) is

\[z = \sqrt{|\mathbf{d}|^2+|\mathbf{h}|^2 - (\mathbf{p}_k^{(x)} - \mathbf{p}_k^{(x)})^2 - (\mathbf{p}_k^{(y)} - \mathbf{b}_k^{(y)})^2}\]

## Pseudocode

To make a long story short. To implement a Stewart Platform with cheap servo motors, all that is needed is

- Define the Stewart Platform configuration with the position of base anchor \(\mathbf{b}_k\) position of the platform anchor \(\mathbf{p}_k\) (both with z-value=0), the length of the rod \(|\mathbf{d}|\) the length of the servo horn \(|\mathbf{h}|\) and the rotation of each servo horn \(\beta_k\) at build time.
- Input relative translation \(\mathbf{t}\) or absolute translation \(\mathbf{T}\) and rotation \(\mathbf{R}\)
- Calculate the effective leg lengths \(\mathbf{l}_k=\mathbf{T} + \mathbf{R}\times\mathbf{p}_k\times\overline{\mathbf{R}} - \mathbf{b}_k\)
- Calculate servo rotation \(\alpha_k=\sin^{-1}\left(\frac{g_k}{\sqrt{e_k^2+f_k^2}}\right) - \text{atan2}(f_k, e_k)\)
- Apply calculated horn angles to servo motors
- Go to step 2 to repeat the process