Thesis report - Date: April 5th 2018

Forward and Inverse Kinematics of 3 revolution-joint robotic arm

I. Introduction
Forward and Inverse kinematics are the fundamentals in the art of dictation of robotic
arms motion. The problem of kinematics is to describe the motion of a robotic arm without
consideration of the forces and torques causing the motion.

Forward kinematic: determine the position and orientation of the end-effector given
the value of the joint variables of the robot.

Inverse kinematic: determine the values of the joint variables given the end-effector
position and orientation.

II. Method

1. Forward kinematics

1.1. Algebraic approach

The problem of forward kinematic is solved using the Denavit-Harternberg
convention, or DH convention. In this convention, each homogeneous transformation Ai is

X3

Y0

Y3 X2

θ3 Figure 1. A free body diagram of a 3 rotating-joint

L3 robotic arm

Y2

X1
Y1
L2
θ2

L1

θ1
X0
represented as a product of four basic transformations

Ai  Rot z ,i Transz ,di Transx ,ai Rot x ,i

ci  si 0 0  1 0 0 0  1 0 0 ai  1 0 0 0
   0 
ci 0 0  0 0  0 1 0 0  0 ci  si
  i
s 1 0
0 0 1 0  0 0 1 di  0 0 1 0  0 si ci 0
    
 0 0 0 1  0 0 0 1  0 0 0 1  0 0 0 1 
ci  si ci si ci ai ci 
 
 s ci ci ci si ai si 
 i
0 si ci di 
 
 0 0 0 1 

where the for quantities i , ai , di , i are parameters associated with link i and joint i. The four
parameters i , ai , di , i are given the names joint angle, link length, link offset, link twist.

For this particular problem, we can establish the DH-table as table 1

Link ai αi di 𝜃i
1 a1 0 0 𝜃1*
2 a2 0 0 𝜃2*
3 a3 0 0 𝜃3*
From the DH-parameters above, we can devise the transformation matrices.

c1  s1 0 a1c1  c2  s2 0 a2c2  c3  s3 0 a3c3 

s c1 
0 a1s1  s c2 
0 a2 s2  s c3 0 a3 s3 
A1   1 A2   2 A3   3
0 0 1 0  0 0 1 0  0 0 1 0 
     
0 0 0 1  0 0 0 1  0 0 0 1 

T10  A1 T2 0  A1 A2
(1)
 c123  s123 0 a1c1  a2c12  a3c123 
s c3 0 a1s1  a2 s12  a3 s123 
T30  A1 A2 A3   3
 0 0 1 0 
 
 0 0 0 1  (2)

The first two entries of the last column of T30 are the x and y coordinate of the end-effector in
base frame (2)

x  a1c1  a2c12  a3c123 (3)

y  a1s1  a2 s12  a3s123

1.2. Geometric approach

Another method for determining the end-effector coordinate of this manipulator is using the
geometric approach

Y0

Y3
P3(xp3, yp3)
X3
X2

L3
θ3
L3*sin(θ 1+θ 2+θ 3)

P2(xp2, yp2)
Y2
L2*sin(θ 1+θ 2)

Y1 L2 X1
θ2
P1(xp1, yp1)

L1
L1*sin(θ 1)

θ1
X0
L1*cos(θ 1) L2*cos(θ 1+θ 2) L3*cos(θ 1+θ 2+θ 3)

Figure 2. Geometric
For joint P1:

xP1  l1 cos(1 ) yP1  l1 sin(1 ) zP1  l1 sin(1 )

For joint P2:

xP 2  xP1  l2 cos(1  2 ) yP 2  yP1  l2 sin(1  2 ) zP 2  zP1  l2 sin(1  2 )

For joint P3:

xP3  xP 2  l3 cos(1  2  3 ) yP3  yP 2  l3 sin(1  2  3 ) zP3  zP 2  l3 sin(1  2  3 )

[Link] Kinematics
For the problems of inverse kinematics, we also have 2 approaches: the algebraic approach
and the geometric one.

2.1. Geometric approach

In this approach, we calculate the position and the angles of each joints by determining the
travelling trajectory of those joints.
We can determine that the joint J1 is moving on an arc (C1) that has the formula:

( X 2  Px 2 )  (Y 2  Py 2 )  R12

with: Px is the horizontal coordinate of the end-effector

Py is the vertical coordinate of the end-effector

R1 is equal to L3

(X, Y) is the possible coordinate of the joint

if we intersect the arc (C1) with the circle (C2), which is the travelling trajectory of the joint
J1 if the 2 links L1 and L2 fully extended, we will obtain 2 intersections. These intersections
are the corresponding highest and lowest obtainable coordinate of the joint J1 on the arc (C1).
Their formulae are respectively:

PHIGH (X1, Y1)

X 1  (P x 2  Py 2  R12  R 22  (P y *(P x 2 * P y  Py * R12  Py * R 2 2  Py 3

 Px *(( Px 2  Py 2  R12  2* R1* R 2  R 2 2 ) *(P x 2  Py 2  R12  2* R1* R 2  R 22 ))(1/ 2) ))
/(P x 2  Py 2 )) / (2* P x)

Y1  (P x 2 *P y  Py *R12  Py *R 22  Py3  Px *(( Px 2  Py 2  R12  2*R1*R 2  R22 )

*(P x 2  Py 2  R12  2*R1*R 2  R22 ))(1/2) ) / (2*(P x 2  Py 2 ))

PLOW(X2, Y2)

X 2  (P x 2  Py 2  R12  R 22  (P y *(P x 2 * P y  Py * R12  Py * R 22  Py 3  Px *(( Px 2  Py 2  R12  2* R1*

*(P x 2  Py 2  R12  2* R1* R 2  R 22 ))(1/2) ))
/(P x 2  Py 2 )) / (2* P x)

Y 2  (P x 2 *P y  Py *R12  Py *R 22  Py 3  Px *(( Px 2  Py 2  R12  2*R1*R 2  R22 )

*(P x 2  Py 2  R12  2*R1*R 2  R22 ))(1/2) ) / (2*(P x 2  Py 2 ))

Then, we assign the horizontal coordinate of the random point (Pn) on the trajectory as a
value between X1 and X2. And we proceed to calculate the vertical coordinate of that point
base on the arc formula:

Y 1random  Py  (P x  R 2  X rand )(1/2) *(R 2  Px  X rand )(1/2)

Y 2random  Py  (P x  R 2  X rand )(1/2) *(R 2  Px  X rand )(1/2)

But one of these solutions will be out of bounds of the joint travelling arc, so it must be
discarded.

Yrand  Y1random OR Yrand  Y 2random

So, the coordinate of the joint J1 will be (Xrand, Yrand)

After we have determined the J1's coordinate, we can proceed to determine the one of joint
J2.

Following the same principle, we establish 2 travelling trajectories of joint J2 with one
around the joint J1 and the other around the base (J3).

Y0

(Px, Py)
2 2 2 2 2
(C3): (X - Pnx ) + (Y – Pny ) = L2
L3

θ3
J1

L2
P3
θ2
J2
L1 P4

θ1
O
X0
J3

(C4): (X2) + (Y2) = L12

Intersecting the two will give us the highest and lowest possible travelling points of the joint
J2 on the arc (C3) which we name P3 and P4. Their formulae are:

P3(X3, Y3)

X 3  ( L12  L 22  Xrand 2  Yrand 2  (Yrand *( L12 * Yrand  L 22 * Yrand  Xrand 2 * Yrand  Yrand 3
 Xrand *(( L 22  2* L 2* L1  L12  Xrand 2  Yrand 2 ) *( L 22  2* L 2* L1  L12  Xrand 2  Yrand 2 )) (1/2) ))
/( Xrand 2  Yrand 2 )) / (2* Xrand )

Y3  ( L12 * Yrand  L22 *Yrand  Xrand 2 *Yrand  Yrand 3  Xrand *((L 22  2* L 2* L1  L12
 Xrand 2  Yrand 2 )*( L22  2* L2* L1  L12  Xrand 2  Yrand 2 ))(1/2) ) / (2*( Xrand 2  Yrand 2 ))

P4(X4, Y4)

X 4  ( L12  L 22  Xrand 2  Yrand 2  (Yrand *( L12 * Yrand  L 22 * Yrand  Xrand 2 * Yrand  Yrand 3
 Xrand *(( L 22  2* L 2* L1  L12  Xrand 2  Yrand 2 ) *( L 22  2* L 2* L1  L12  Xrand 2  Yrand 2 )) (1/2) ))
/( Xrand 2  Yrand 2 )) / (2* Xrand )

Y4  ( L12 * Yrand  L22 *Yrand  Xrand 2 *Yrand  Yrand 3  Xrand *((L 22  2* L 2* L1  L12
 Xrand 2  Yrand 2 )*( L22  2* L2* L1  L12  Xrand 2  Yrand 2 ))(1/2) ) / (2*( Xrand 2  Yrand 2 ))

These are the only 2 possible solutions for every random point Pn, which corresponds to the
elbow up and elbow down configurations.

From the set of equation we devise in the geometric approach of forward kinematics, we can
solve the angles of this manipulator based on the coordinate of its joints

1_1  a tan 2(Y3 , X 3 )

1_ 2  a tan 2(Y4 , X 4 )

 2 _1  a tan 2(Yrand  Y3 , X rand  X 3 )  1_1

 2 _ 2  a tan 2(Yrand  Y4 , X rand  X 4 )  1_ 2

3_1  a tan 2(Py Yrand , Px  X rand )  1_1   2 _1

3_ 2  a tan 2(Py Yrand , Px  X rand )  1_ 2   2 _ 2

With this method, we can have an infinite number of solution for every end-effector
coordinate.