FORWARD KINEMATICS SOLUTION OF A STEWART PLATFORM

ACTUATED BY ROTARY MOTORS (DRAFT)

NICOLAS JEANMONOD, OUILOGIQUE.COM, MAY 2019

Part 1. Abstract
This paper presents an algorithm for the calculation of the forward kinematics solution of a Stewart platform
actuated by rotary motors. It is based on the following paper by an unknown author from the Wokingham U3A
Math Group: https://bit.ly/2FU3rUJ. This algorithm has been implemented on an ESP32 microcontroller:
https://github.com/NicHub/stewart-platform-esp32.

Part 2. Positions of the platform joints Pi relative to the servo pivots Bi

Let’s consider an orthogonal system of axis xyz with origin in the middle of the moving platform and the z
axis pointing downwards. The coordinates of the platform joints are called Pi where i = motor index = 0..5.
Note that at home position P zi = 0.
Pi = {P xi , P yi , 0, 1}
The platform has six degrees of freedom, three rotations and three translations with the following transformation
matrices:
   
1 0 0 0 cos B 0 − sin B 0
 0 cos A sin A 0   0 1 0 0 
Rx = 
 0 − sin A cos A
 Ry =  
0   sin B 0 cos B 0 
0 0 0 1 0 0 0 1
   
cos C sin C 0 0 1 0 0 0
 − sin C cos C 0 0 0 1 0 0
Rz = 
 0
 T xyz =  
0 1 0 0 0 1 0
0 0 0 1 Tx Ty Tz 1
The servo pivots have the following coordinates.

Bi = {Bxi , Byi , Zhome, 1}

Translation of servo pivots Bi relative to the origin:
 
1 0 0 0
 0 1 0 0
TB =   0

0 1 0
−Bx −By −Zhome 1
The positions of the platform joints Pi relative to the servo pivots Bi after movement are called BPi and can
be found by calculating the dot product below. Note that the result is the solution of the forward kinematics
of platforms actuated by linear motors.

BPi = P i · Rx · Ry · Rz · T xyz · T B =

 T  T
BP xi P xi cos B cos C + P yi (sin A sin B cos C − cos A sin C) + T x − Bxi
 BP yi   P xi cos B sin C + P yi (sin A sin B sin C + cos A cos C) + T y − Byi 
(1)  BP zi  =  −P xi sin B
   
+ P yi sin A cos B + Tz − Zhome
1 1

1
 FORWARD KINEMATICS SOLUTION OF A STEWART PLATFORM ACTUATED BY ROTARY MOTORS (DRAFT) 2

Part 3. Rotation of the vector BP in the plane of the servo arm

The arm plane is rotated by a negative angle Θs and it defines a new system of coordinates that is also
rotated. Let’s find the coordinates of BP in this new reference system:
 T    T  T
BP xi cos Θs sin Θs 0 0 cos Θs BP xi − sin Θs BP yi a
 BP yi   − sin Θs cos Θs 0 0 sin Θs BP xi + cos Θs BP yi  b
 BP zi  · 
   =  = 
0 0 1 0  BP zi  c
1 0 0 0 1 1 1
In the original algorithm, Θs is positive, so we can rewrite:

 T  T
a cos Θs BP xi + sin Θs BP yi
  = − sin Θs BP xi + cos Θs BP yi 
b  
(2) c  BP zi 
1 1

Part 4. Intersection between servo arm circle and rod sphere in the plane of the servo arm circle
The end of the rod is somewhere on a sphere. This sphere will intersect the circle of the servo arm 0, 1 or 2
times. This circle lies in a plane which has two axis x and z, so y = 0. This plane is rotated by an angle Θs
along the z axis. We applied the rotation to the BP vector in the previous section to find the values of a, b
and c. The rotation implies that BP is in the negative region of y and that explains the sign of the term +b2
in the equation of the sphere below.
Now we will find the intersection of the rod sphere and the arm circle. The circle is expressed in parametric
coordinates, but is also possible to solve this problem by expressing it in Cartesian coordinates.
 2 2 2 2
(x − a) + b + (z − c) = rod

arm cos ϕ =x

arm sin ϕ =z


2 2
(arm cos ϕ − a) + b2 + (arm sin ϕ − c) = rod2
arm2 cos2 ϕ − 2 arm cos ϕ a + a2 +

arm2 sin2 ϕ − 2 arm sin ϕ c + c2 = rod2 − b2

a2
 
a
cos2 ϕ − 2 cos ϕ + +
arm arm2
c2 rod2 − b2
 
c
sin2 ϕ − 2 sin ϕ + 2
=
arm arm arm2
a
d=
arm
c
e=
arm
cos2 ϕ − 2d cos ϕ + d2 +

rod2 + b2
sin2 ϕ − 2e sin ϕ + e2 =
arm2
2 2
cos ϕ + sin ϕ = 1
rod2 − b2
 
1 2 2
d cos ϕ + e sin ϕ = 1+d +e −
2 arm2
2 2
c2 rod2 − b2
 
1 arm a
d cos ϕ + e sin ϕ = + + −
2 arm2 arm2 arm2 arm2
a2 + b2 + c2 + arm2 − rod2
d cos ϕ + e sin ϕ = =f
2 arm2
 FORWARD KINEMATICS SOLUTION OF A STEWART PLATFORM ACTUATED BY ROTARY MOTORS (DRAFT) 3

Identity
m sin α + n cos α = o sin (α + p)
p
o = m2 + n2
n
tan p =
m
p  n
n cos α + m sin α = ± m2 + n2 sin α + arctan
m
π n π
if − < arctan < then
2 m 2
p  n
n cos α + m sin α = m2 + n2 sin α + arctan =f
m
Thus
 
p
2 2
d
f = d + e sin ϕ + arctan
e
f d
ϕ = arcsin √ − arctan
d2 + e2 e
a2 +b2 +c2 +arm2 −rod2
2 arm2 a
ϕ = arcsin q − arctan
a2
+ c2 c
arm2 arm2

a + b + c + arm2 − rod2
2 2 2
a
ϕ = arcsin √ − arctan
2
2 arm a + c 2 c
a2 + b2 + c2 + arm2 − rod2 a
ϕ = arcsin q
2 − arctan c
2 · arm · c · 1 + ac
The length of the vector BP is the same in both systems of coordinates (arm and platform). So we calculate
it in the platform system because this allows us to drop the calculation of b:

(3) a2 + b2 + c2 = BPx2 + BPy2 + BPz2 = BP 2

BP 2 + arm2 − rod2 a
(4) ϕ = arcsin q − arctan
2 c
2 · arm · c · 1 + ac

The angle ϕ equals 0 when the arm is horizontal, but we want it to be 0 when the arm is at half of its full
angular range:

servo f ull angular range

(5) ϕf inal = ϕ +
2

Part 5. Conversion from radian to PWM microseconds and mirroring

The conversion of the angle in radian to an angle expressed in PWM microseconds is done with a simple
linear relation that is different for odd and even servomotors:
(
gain · ϕf inal + servo min pwm µs + of f set
(6) ϕP W M µs =
−gain · ϕf inal + servo max pwm µs + of f set
where

servo max pwm µs − servo min pwm µs

(7) gain =
servo f ull angular range rad
A typical value for the gain is (2000 − 1000) /π ≈ 318 µs/rad ≈ 5.6 µs/◦ .
 FORWARD KINEMATICS SOLUTION OF A STEWART PLATFORM ACTUATED BY ROTARY MOTORS (DRAFT) 4

The of f set will be typically 0, but can be adjusted to take into account that the arms cannot be mounted
exactly at the correct angle. The sign of gain takes into account the fact that the odd and even arms are a
reflection of each other. Thus, odd and even servomotors must have opposite gain values.

Part 6. Calculation of Zhome

Zhome must be calculated for each platform geometry so that when the platform is at home position, i.e.
with no translation and no rotation, the arms are horizontal or tilted at any value that makes sense.
TO DO: calculation of Zhome.
FORWARD KINEMATICS SOLUTION OF A STEWART PLATFORM ACTUATED BY ROTARY MOTORS (DRAFT) 5