Forward and Inverse Kinematics of A Delta Robot With CGA5

Download as pdf or txt
Download as pdf or txt
You are on page 1of 13

See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/345607697

The Forward and Inverse Kinematics of a Delta Robot

Chapter · October 2020


DOI: 10.1007/978-3-030-61864-3_38

CITATIONS READS
12 7,419

3 authors, including:

Hugo Hadfield Joan Lasenby


University of Cambridge University of Cambridge
15 PUBLICATIONS   69 CITATIONS    192 PUBLICATIONS   2,989 CITATIONS   

SEE PROFILE SEE PROFILE

Some of the authors of this publication are also working on these related projects:

Analogies between relativity and aeroacoustics using Geometric Algebra View project

Human pose estimation for human-robot interaction View project

All content following this page was uploaded by Hugo Hadfield on 09 November 2020.

The user has requested enhancement of the downloaded file.


The Forward and Inverse Kinematics of a
Delta Robot

Hugo Hadfield1[0000−0003−4318−050X] , Lai Wei1 ,


Joan Lasenby1[0000−0002−0571−0218]

University of Cambridge, Department of Engineering, Cambridge, UK,


[email protected], [email protected], [email protected]

Abstract. The Delta robot is one of the most popular parallel robots
in industrial use today. In this paper we analyse the forward and inverse
kinematics of the robot from a geometric perspective using Conformal
Geometric Algebra. We calculate explicit formulae for all joints in both
the forward and inverse kinematic problems as well as explicit forward
and inverse Jacobians to allow for velocity and force control. Finally
we verify the kinematics in Python and simulate a physical model in
the Unity3D game engine to act as a test-bed for future development of
control algorithms.

1 Introduction
The Delta robot [1, 2] was invented in 1985 by Raymond Clavel at EPFL after
being inspired by a visit to a chocolate packing factory [3]. It has since become a
particularly popular robot in industrial settings due to its good precision coupled
with high speed and acceleration.
The Delta robot is a specific type of robot known as a parallel manipulator.
Parallel manipulators, also known as parallel robots, are a class of robots that
feature end-effectors driven by multiple underactuated parallel kinematic chains
[4, 5]. Typically a parallel robot is designed such that all actuators remain fixed
to the support structure of the robot thereby minimising the mass of the moving
parts of the robot and enabling very fast accelerations. Indeed this goal of high
speed/fast acceleration has been the primary driving force in the development of
parallel robots for industry and today architectures such as the Delta robot are
widespread in many high precision, high throughput manufacturing applications.
Parallel robots, while practically very useful, are often significantly more difficult
to analyse than their serial cousins due to the end-point position being a function
of the configuration of multiple kinematic chains.
Conformal Geometric Algebra (CGA) is a specific 5D representation of 3D
space that embeds geometric primitives and conformal transformations as ele-
ments of the same algebra [6, 7]. Our mathematics in this paper will be phrased
within CGA. We will use the standard extension of the 3D geometric algebra,
where our 5D CGA space is made up of the standard Euclidean basis vectors {ej }
j = 1, 2, 3, where e2j = 1, plus two additional basis vectors, e and ē with signa-
tures, e2 = 1, ē2 = −1. Two null vectors can therefore be defined as: n∞ = e + ē
2 Hugo Hadfield , Lai Wei , Joan Lasenby

and n0 = ē−e 2 . The mapping of a 3D vector x to its conformal representation X


is given by X = F (x) = 21 (x2 n∞ + 2x + 2n0 ).
In this paper we draw inspiration from the established literature of robotic
analysis in GA [12–20] as well as the previous literature on Delta robot kinemat-
ics [5, 8, 11] and combine them, leveraging the representational power of CGA
to illustrate the geometry of the constraints inherent in the mechanism of the
Delta robot.

2 Geometry of a Delta Robot

Fig. 1. Left: the 3D geometry of the delta robot. Right: The geometry of a single arm
in plane.

Since its inception, there have been many variants of the Delta robot [8].
In this paper we will assume the simple robot described in this section and
illustrated in Figure 1. The static part of the robot is a base plate to which
three motors are rigidly attached, we will assume a space in which the origin
is at the centre of this plate. Each motor shaft is rigidly attached to an ‘upper
arm’ of length l; we will number each upper arm i ∈ [1, 2, 3]. The connection
point of the motor and upper arm will be labelled Bi . The arm can only rotate
in plane about the motor axis as the motor shaft and upper arm are rigidly
connected. We will refer to the other end of this upper arm as the ‘elbow point’
and will label it Wi . At the elbow point each arm is rigidly attached to a central
point of a horizontal rod we will refer to as the ‘elbow rod’. At each end of the
elbow rod a ball joint connects to a ‘forearm’ piece. The two forearm pieces for
each arm are the same length and, at the other end from the elbow rod, are
connected to a rigid plate that we will refer to as the end-effector plate. The
point half-way between where the two forearm rods connect to the end-effector
The Forward and Inverse Kinematics of a Delta Robot 3

plate is labelled Xi . We will label the point at the centre of the end-effector plate
Y . Assuming the robot is infinitely stiff, the end plate is constrained, due to this
specific arrangement of the forearms, to always remain parallel to the base plate
and to have its in-plane orientation fixed as well. The Delta robot is therefore a
purely translational mechanism.
The labels have assigned here will also serve as the notation for points in
conformal space; for example, the centre of the end-effector plate has 3D position,
y with Y = F (y).

3 Inverse Kinematics
The inverse kinematic problem for the Delta robot is summarised as follows: To
what angle relative to the base should we move the upper arms given we want
the centre of the end-effector plate to be in a specific position in 3D space?

Fig. 2. The geometry of the inverse kinematic problem. There are three spheres, one
for each arm of the robot, centred at the conformal points Xi . Each sphere intersects
with a circle centred at Bi allowing the extraction of the conformal elbow point Wi .

To solve this problem we need to work backwards from the 3D end-effector


plate position y to the motor angles θi considering the geometry of the robot
as we go. Starting at the end-effector plate the 3D points xi are translationally
offset in plane in the direction si giving xi = y + re si where re is the radius of
the end-effector plate. Due to the geometry of the robot the elbow point Wi is
constrained to lie on a sphere with radius equal to the length of the forearms
4 Hugo Hadfield , Lai Wei , Joan Lasenby

ρ centred at this point xi . We can represent this sphere as a dual sphere in


conformal geometric algebra as follows:
1
Σi∗ = Xi − ρ2 n∞
2
The elbow point is also simultaneously constrained to lie on a circle of radius l
centred at the motor shaft to upper-arm joint, Bi . We can represent this circle
as the dual circle Ci∗ in CGA, where Ci∗ is the intersection of a dual sphere of
radius l centred at the position Bi , Bi − 21 l2 n∞ , and the dual plane through


the origin, Bi and e3 which is given by I3 (si ∧ e3 ). In CGA we calculate the


intersection of objects via the ‘meet’ operator, as both operands are in their
dual form however, here we simply need an outer product:
 
∗ 1 2
Ci = Bi − l n∞ ∧ (I3 (si ∧ e3 ))
2
where e3 is the vertical unit vector. So long as y is within the reachable volume
of the robot there are two possible solutions for this pair of constraints. These
two solutions lie at the intersection points of the sphere and circle and the ‘meet’
operation of CGA provides us with a direct means to calculate these intersection
points. As with the circle, our sphere and circle are in the dual form (ie. 1 and
2-vectors respectively), and so the point-pair bivector resulting from their meet
is calculated as simply their outer product followed by multiplication with the
5D pseudo-scalar, I5 :
Ti = (Ci∗ ∧ Σi∗ )I5
The individual solutions can be extracted from this point-pair object by projec-
tion operators [6]:
!
1 Ti
Pi = 1+ p 2
2 Ti
Wi = −P̃i (Ti · n∞ )Pi
We can then convert from the CGA to the 3D vector point:
Pj=3
j=1 (Wi · ej )ej
wi = −
Wi · n ∞
and so, with a little trigonometry we can extract the motor angles:
θi = atan2(zi · e3 , zi · si ), zi = wi − rb si
Figure 2 illustrates the geometry of the inverse kinematic problem graphically.

4 Forward Kinematics
The forward kinematic problem is, in some sense, the opposite of the inverse
kinematic one. Our goal here is to calculate the 3D vector position of the end-
effector plate y given the motor angles θi , i ∈ [1, 2, 3].
The Forward and Inverse Kinematics of a Delta Robot 5

Fig. 3. The geometry of the forward kinematic problem. Each motor connects to an
upper arm at position Bi . The upper arms end in the elbow point Wi . Each elbow
point has an associated pseudo-elbow point Ai and forearm constraint sphere. All
three constraint spheres meet at the centre of the end-point plate, Y .

To solve the forward kinematic problem we will consider the robot one arm
at a time. For a given arm motor angle θi the 3D position of the elbow point wi
can be calculated as:

wi = (rb + l cos(θi ))si + l sin(θi )e3

For each arm we will now define a pseudo-elbow point, ai which is offset hori-
zontally from the true elbow point by the radius of the end effector plate and in
the direction of the origin.

ai = (rb − re + l cos(θi ))si + l sin(θi )e3

The equivalent CGA point is then:


1 2
Ai = a n∞ + ai + n0
2 i
Given the geometry of the robot, these pseudo-elbow points all lie a distance
equal to the length of the robot’s forearms, ρ, from the centre of the end-point
plate Y . Geometrically these fixed distance constraints manifest themselves as
6 Hugo Hadfield , Lai Wei , Joan Lasenby

spheres, which we will label Σi , on which the centre of the end-point plate can
lie:
1
Σi∗ = Ai − ρ2 n∞
2
Each arm contributes one constraint sphere and the intersection of the three
spheres produces a point-pair, T , that represents the two possible configurations
of the end-plate:
i=3
^
T = I5 Σi∗
i=1
V
where the notation implies an outer product of all elements following it.
Practically only one of these possible solutions is feasible, the solution which
places Y at a greater position along the e3 axis. We can once again get the 3D
position of this point, y by extracting the point with projectors.
 
1 T
P = 1+ √
2 T2
Pj=3
− j=1 (Y · ej )ej
y= , Y = −P̃ (T · n∞ )P,
Y · n∞
Figure 3 illustrates the geometry of the forward kinematic problem.

5 The Inverse Jacobian


Knowing static kinematic solutions is useful but to do more advanced analysis
of the Delta robot mechanism we need to look at derivatives. We will start
with inverse kinematics and ask ourselves the question, if the end-point plate
moves with a specific velocity what speeds must the motors be moving at to be
compatible?
First we will write the 3D end-point plate position as a linear combination
of basis vectors with coefficients denoted αj , j ∈ 1, 2, 3:

y = α1 e1 + α2 e2 + α3 e3

Our goal now becomes to calculate the partial derivative of each motor angle
with respect to one each of these α coefficients. Taking partial derivatives of y
with respect to one of the αj coefficients trivially gives:
∂y
= ej
∂αj
For now we do not need to worry about which α parameter we are taking
derivatives with respect to, so we will leave the derivative of the end-point written
∂y
as ∂α . Using this notation, our ultimate goal in this section is to find an equation
for the partial derivative of a given motor angle θi with respect to α, ie. ∂θ ∂α . To
i

∂θi
find ∂α we select a specific robot arm i and work back through its joints from
the end-point.
The Forward and Inverse Kinematics of a Delta Robot 7

The first joint position of interest is xi , we saw in section 3 that:

xi = y + re si

Taking partial derivatives gives:


∂xi ∂y
=
∂α ∂α
The 3D point xi can then be represented as the CGA point Xi :
1 2
Xi = x n∞ + xi + n0
2 i
The derivative of this CGA point is then easily found:
 
∂Xi ∂xi ∂xi
= · xi n∞ +
∂α ∂α ∂α
We then form the dual constraint sphere:
1
Σi∗ = Xi − ρ2 n∞
2
which, as the radius is fixed, has partial derivative:
∂Σi∗ ∂Xi
=
∂α ∂α
As we saw in the previous section, the intersection of this dual constraint sphere
Σi∗ and the dual circle Ci∗ centred on the motor shaft produces a point-pair Ti
that represents the two possible elbow positions for that arm:
 
∗ 1 2
Ci = Bi − l n∞ ∧ (I3 (si ∧ e3 ))
2
Ti = (Σi∗ ∧ Ci∗ )∗

The outer product and taking the dual are both linear, which means that taking
derivatives is particularly easy here:
∗
∂Σi∗

∂Ti
= ∧ Ci∗
∂α ∂α
Of course the elbow can only actually be in one position which we can extract
via projection operators:
! !
∂Ti
1 Ti ∂Pi 1
q
∂Ti · T i
Pi = 1+ p 2 , = Ti2 − Ti ∂α
2Ti2
p
2 Ti ∂α ∂α Ti2
Wi = −P̃i (Ti · n∞ )Pi
 
∂Wi ∂ P̃i ∂Ti ∂Pi
=− (Ti · n∞ )Pi − P̃i · n∞ Pi − P̃i (Ti · n∞ )
∂α ∂α ∂α ∂α
8 Hugo Hadfield , Lai Wei , Joan Lasenby

We can then convert from the CGA to the 3D vector point:


Pj=3
j=1 (Wi · ej )ej
wi = −
Wi · n ∞
Pj=3 ∂Wi  Pj=3 ∂Wi

∂wi − j=1 ∂α · ej ej (Wi · n∞ ) + j=1 (Wi · ej ) ej ∂α · n∞
=
∂α (Wi · n∞ )2
and use this to form the derivative of the motor angles with regard to α:
∂zi ∂wi
zi = wi − rb si , =
∂α ∂α
θi = atan2(zi · e3 , zi · si )
zi · si (zi · si ) ∂z ∂zi
 
∂α · e3 − (zi · e3 ) · si
i
∂θi ∂α
=
∂α |zi · si | zi2
This finally gives us an expression for the derivative of the motor angle with
respect to the α of the endpoint. Typically in engineering scenarios we would
construct a matrix of the partial derivatives with respect to αj , j ∈ 1, 2, 3,
known as the Jacobian matrix:
 
∂θ1 ∂θ1 ∂θ1
∗  ∂α
∂θ2
1 ∂α2
∂θ2
∂α3
∂θ2 
J =  ∂α 1 ∂α2 ∂α3 
∂θ3 ∂θ3 ∂θ3
∂α1 ∂α2 ∂α3

This matrix can then be used to convert an end-point velocity vector to a set of
motor velocities:  ∂θ1   ∂α1 
∂t ∂t
 ∂θ2  = J ∗  ∂α2 
∂t ∂t
∂θ3 ∂α3
∂t ∂t
As it is the Jacobian matrix for the inverse kinematic problem, this matrix is
specifically labelled the inverse Jacobian matrix.

6 The Forward Jacobian


Many problems in robotics require us to take derivatives of the forward kinematic
equations. Specifically, we need to know the end-point plate velocity as a function
of the motor speeds.
Our forward kinematic solution begins with calculating the position of the
elbow point for a given arm i:
∂wi
wi = (rb + l cos(θi ))si + l sin(θi )e3 , = −l sin(θi )si + l cos(θi )e3
∂θi
With the elbow point we can then calculate the pseudo-elbow point:
∂ai ∂wi
ai = wi − re si , =
∂θi ∂θi
The Forward and Inverse Kinematics of a Delta Robot 9

We then convert the pseudo-elbow to a CGA point:


 
1 ∂Ai ∂ai ∂ai
Ai = a2i n∞ + ai + n0 , = · ai n∞ +
2 ∂θi ∂θi ∂θi
The forearm length dual constraint sphere can then be constructed about the
pseudo-elbow point
1 ∂Σi∗ ∂Ai
Σi∗ = Ai − ρ2 n∞ , =
2 ∂θi ∂θi
The intersection of all three constraint spheres, one from each arm, produces the
point pair on which the solution lies.
T = (Σ1 ∨ Σ2 ∨ Σ3 ) ≡ I5 (Σ1∗ ∧ Σ2∗ ∧ Σ3∗ )
We can take derivatives of this point-pair with respect to each of the motor
angles:
∂Σ1∗ ∂Σ2∗
   
∂T ∗ ∗ ∂T ∗ ∗
= I5 ∧ Σ2 ∧ Σ3 , = I5 Σ1 ∧ ∧ Σ3
∂θ1 ∂θ1 ∂θ2 ∂θ2
∂Σ3∗
 
∂T
= I5 Σ1∗ ∧ Σ2∗ ∧
∂θ3 ∂θ3
We can re-write these derivatives as follows:
∂Σi∗
 
∂T ^
i−1
= (−1) I5 ∧ C , where C ∗ =

Σj∗ (1)
∂θi ∂θi
j∈1,2,3 j6=i

Practically, when we take partial derivatives with respect to one θ at a time


we are effectively freezing two of the motors in position and moving the third.
Geometrically, this process forces the end-point plate to move along the surface
of the circle formed by the intersection of the two constraint spheres centred at
the pseudo-elbow points of the frozen motors.
Figure 4 shows the geometric significance of Equation 1. To get the end-point
plate position we again extract one end of the point-pair T :
∂T
!
1

T

∂P 1 √ ∂T ∂θ ·T
P = 1+ √ , = T2 −T √ i

2 T2 ∂θi 2T 2 ∂θi T2
 
∂Y ∂ P̃ ∂T ∂P
Y = −P̃ (T · n∞ )P, =− (T · n∞ )P − P̃ · n∞ P − P̃ (T · n∞ )
∂θi ∂θi ∂θi ∂θi
Finally we convert our end-point back to a 3D point:
Pj=3
− j=1 (Y · ej )ej
y=
Y · n∞
Pj=3  ∂Y  Pj=3 ∂Y
∂y − j=1 ∂θi · ej ej (Y · n∞ ) + j=1 (Y · ej ) ej ( ∂θ i
· n∞ )
=
∂θi (Y · n∞ )2
10 Hugo Hadfield , Lai Wei , Joan Lasenby

Fig. 4. With two limbs frozen the end-point plate is constrained to move such that its
centre always lies on the circle (shown in red) formed from the intersection of the other
two limbs’ constraint spheres.

We can write the end-point plate position as:

∂y ∂α1 ∂α2 ∂α3


y = α1 e1 + α2 e2 + α3 e3 , = e1 + e2 + e3
∂θi ∂θi ∂θi ∂θi
∂y
With ∂θi we are therefore in a position to build the forward Jacobian matrix:
 ∂y ∂y ∂y

· e1 · e1 · e1
 ∂θ
∂y
1 ∂θ2
∂y
∂θ3
∂y
J= · e2 · e2 · e2 

 ∂θ1 ∂θ2 ∂θ3
∂y ∂y ∂y
∂θ1 · e3 ∂θ2 · e3 ∂θ3 · e3

The inverse Jacobian matrix and the forward Jacobian matrix are, as the names
suggest, inverse to each other.
JJ ∗ = I

7 Simulation and Verification in Python and Unity3D


To verify our derivatives we implemented the above mathematics in the Clifford
Python package [10] and tested it against a central differences approximation.
With the mathematics verified we constructed a CGA Unity3D library based
on the C# output of the ganja.js [9] code generator. Figure 5 shows examples of
this library being used to intersect circles, lines, spheres and planes in Unity3D.
Alongside the CGA library we also constructed a physical delta robot model
using Unity3D’s built-in joint system and physics system, the right hand side of
The Forward and Inverse Kinematics of a Delta Robot 11

Fig. 5. Intersections of geometric primitives calculated and visualised with the Unity3D
CGA library

Figure 6 shows the model in the Unity3D scene view. The simulated physical
model of the robot allows us to design and tune a controller safely and for low
cost without requiring us to build or buy a real Delta robot.
The CGA Unity3D library and simulated physical robot together form a test-
bed for the design and tuning of control systems. On the left hand side of Figure
6 is the representation of the robot to a controller constructed using the CGA
library and the mathematics described in the previous sections.

Fig. 6. Left: The internal robot geometry model of the controller. Right: The physically
simulated Delta robot in Unity3D.

8 Conclusion
In this paper we have derived, step-by-step, the forward and inverse kinematic
solutions for a Delta robot in CGA as well as the forward and inverse Jaco-
bian matrices. We then implemented the mathematics in Python and C# before
simulating a physical robot model in Unity3D and using our mathematics to
design a basic position and velocity control system to position the end-plate of
the robot.

References
1. Clavel, R. Conception d’un robot parallèle rapide à 4 degrés de liberté. Infoscience
https://infoscience.epfl.ch/record/31403 (1991) doi:10.5075/epfl-thesis-925.
12 Hugo Hadfield , Lai Wei , Joan Lasenby

2. Clavel, R., Device for the Movement and Positioning of an Element in Space, United
States Patent No. 4976582, (1990)
3. Pessina, L., Reymond Clavel, creator of the Delta Robot, re-
flects on his career, EPFL website, https://sti.epfl.ch/
reymond-clavel-creator-of-the-delta-robot-reflects-on-his-career/
4. Gallardo-Alvarado, J. Kinematic Analysis of Parallel Manipulators by Algebraic
Screw Theory. (Springer International Publishing, 2016).
5. Merlet, J.-P. Parallel robots. (Kluwer Academic Publishers, 2006).
6. Lasenby, A., Lasenby, J. and Wareham, R.: A Covariant Approach to Geometry Us-
ing Geometric Algebra., CUED Technical Report CUED/F-INFENG/TR-483. Cam-
bridge. University Engineering Department (2004).
7. Dorst, L., Fontijne, D. and Mann, S.: Geometric algebra for computer science: an
object-oriented approach to geometry. 1st edn. Elsevier; Morgan Kaufmann, Ams-
terdam (2007).
8. Pierrot, F., Fournier, A. and Dauchex, P. Towards a fully-parallel 6 DOF robot
for high-speed applications. in 1991 IEEE International Conference on Robotics and
Automation Proceedings 1288–1293 vol.2 (1991).
9. De Keninck, S.: ganja.js https://github.com/enkimute/ganja.js (2017).
10. Arsenovic, A., Hadfield, H., Wieser, E., Kern, R., and The Pygae Team. Clifford
(Version v1.3.0). (2020, May 29). https://github.com/pygae/clifford. Zenodo.
http://doi.org/10.5281/zenodo.3865446
11. Zsombor-Murray, P. J. Descriptive Geometric Kinematic Analysis of Clavel’s
“Delta” Robot. Centre for Intelligent Machines, McGill University. (2004).
12. Zamora, J. and Bayro-Corrochano, E. Inverse kinematics, fixation and grasping
using conformal geometric algebra. in 2004 IEEE/RSJ International Conference on
Intelligent Robots and Systems (IROS) (IEEE Cat. No.04CH37566) vol. 4 3841–3846
vol.4 (2004).
13. Hildenbrand, D., Zamora, J. and Bayro-Corrochano, E. Inverse Kinematics Com-
putation in Computer Graphics and Robotics Using Conformal Geometric Algebra.
Advances in Applied Clifford Algebras 18, 699–713 (2008).
14. Aristidou, A. Tracking and modelling motion for biomechanical analysis (Doctoral
thesis). https://doi.org/10.17863/CAM.13996. (2010).
15. Kleppe, A. L. and Egeland, O. Inverse Kinematics for Industrial Robots using
Conformal Geometric Algebra. Modeling, Identification and Control: A Norwegian
Research Bulletin 37, 63–75 (2016).
16. Kim, J. S., Jeong, J. H. and Park, J. H. Inverse kinematics and geometric singu-
larity analysis of a 3-SPS/S redundant motion mechanism using conformal geometric
algebra. Mechanism and Machine Theory 90, 23–36 (2015).
17. Fu, Z., Yang, W. and Yang, Z. Solution of Inverse Kinematics for 6R Robot Ma-
nipulators With Offset Wrist Based on Geometric Algebra. Journal of Mechanisms
and Robotics 5, (2013).
18. Tichý, R. Inverse Kinematics for the Industrial Robot IRB4400 Based on Confor-
mal Geometric Algebra. in Modelling and Simulation for Autonomous Systems (eds.
Mazal, J., Fagiolini, A. and Vasik, P.) 148–161 (Springer International Publishing,
2020). https://doi.org/10.1007/978-3-030-43890-6_12.
19. Hildenbrand, D., Hrdina, J., Návrat, A. and Vašı́k, P. Local Controllability of
Snake Robots Based on CRA, Theory and Practice. Adv. Appl. Clifford Algebras 30,
2 (2019).
20. Selig, J. M. Geometric Fundamentals of Robotics. (Springer-Verlag, 2005). https:
//doi.org/10.1007/b138859.

View publication stats

You might also like