Université de Montpellier - LIRMM

Rapport de projet HAE920E

Model, Motion planning and

Control of UR10

Supervisor : ABDELAZIZ Salih

Authors : BOUZIT Zakaria - BENALI Abderahmane
1 Introduction
Kinematic modeling and control is the foundation and focus of robotics research.
In this project, taking a 6 degrees of freedom (dof) UR10 robot as the re-
search object. Based on the homogenous transformation, the kinematic mod-
eling of the robot was performed with the modified Denavit-Hartenberg (D-H)
parameterization method. In addition, Implementation of a motion planning in
operation space is carried out.
Finally, a kinematic control of the end effector pose is developped based on
robot model and the motion planning solution.

2 Robot Modeling
2.1 Structual Analysis :
The rehabilitation system in this project is based on Universal Robot 10, which
consists of seven revolute joints: base, shoulder, elbow, wrist-1, wrist-2 and
tool mounting bracket. Each joint is driven by one motor separately to realize
the relative motion of adjacent links, and the whole system is a typical series
structure device. Moreover, all joints of UR10 are rotation joints, which means
that only thtea is a variable parameter when describing each joint. So for this
rehabilitation system only six joint variables (rotation variables, except joint7
which is on the top of link6) are needed to figure the changes of the whole system
out, and other parameters are used to represent the fixed part of kinematics.

Figure 1: UR10 Structure and Joints

1
2.2 The modified D-H parametrization method :
D-H parameterization method is a basic method in kinematic modeling. The
principle of this basic method is to establish a coordinate system at each joint
of the robot, then determine the kinematics parameters between two adjacent
coordinates, and finally get the homogeneous transformation matrix between
two adjacent coordinate systems based on the determined kinematic parame-
ters. The homogeneous transformation matrix can be determined with only four
kinematic parameters. Therefore, as long as these parameters are determined,
the coordinate transformation between the adjacent joints of the robot can be
completely described. Here, we suppose arbitrary two adjacent coordinate sys-
tems are
Oi−1 Xi−1 Yi−1 Zi−1 and Oi Xi Yi Zi
Firstly, establish a series of joint coordinate systems as shown in Fig.1, and
define D-H parameters as shown in Table I. Next, use the following four steps
to achieve a homogeneous transformation between adjacent coordinate systems:
- Move from Zi−1 to Zi along axis Xi−1 , and the distance is ai−1 ,
we use Transxi−1 (ai−1 ) to represent it :
 
1 0 0 ai−1
0 1 0 0 
T ransxi−1 (ai−1 ) = 
0 0 1
 (1)
0 
0 0 0 1
- Rotate from Zi−1 to Zi about axis Xi−1 , and the angle is αi−1 ,
we use Rotxi−1 (ai−1 ) to represent it :
 
1 0 0 0
0 cos(αi−1 ) 0 0
Rotxi−1 (αi−1 ) = 
0
 (2)
0 sin(αi−1 ) 0
0 0 0 1
- Move from Xi−1 to Xi along axis Zi , and the distance is di ,
we use T ranszi (di ) to represent it :
 
1 0 0 0
0 1 0 0
T ranszi (di ) = 
0
 (3)
0 1 di 
0 0 0 1
- Rotate from Xi−1 to Xi about axis Zi , and the angle is θi ,
we use Rotzi (θi ) to represent it :
 
cos(θi ) −sin(θi ) 0 0
sin(θi ) cos(θi ) 0 0
Rotzi (θi ) = 
 0
 (4)
0 1 0
0 0 0 1

2
 After multiplying the 4 matrices, we can get general form homogeneous
transformation matrix :
i−1
i T = T ransxi−1 (ai−1 )Rotxi−1 (αi−1 )T ranszi (di )Rotzi (θi ) (5)
Using D-H parametrization, we obtain the following list of values of DH
parameters :

Joint ai−1 αi−1 θi di

1 0 0 θ − π/2 d1
2 π/2 0 θ − π/2 d4
3 0 −a2 θ 0
4 0 −a3 θ −π/2 0
5 π/2 0 θ d5
6 −π/2 0 θ 0

Table 1: DH table

where :
• a2 = 0.612
• a3 = 0.5723

• d1 = 0.1273
• d4 = 0.163941
• d5 = 0.1157
• d6 = 0.0922

2.3 Direct kinematic model:

the direct kinematic model consist in using he kinematic equations of the robot
to compute the pose of the end-effector (postion + orientation)from specified
values for the joint parameters, to do so, first we calculate all the homogenous
matrices, from the R0 to Rn for this we use the general homogenous transfor-
mation matrix form with the D-H table then we have:
0
T6 (q) =0 T1 (q1 ).1 T2 (q2 ).2 T3 (q3 ).3 T4 (q4 ).4 T5 (q5 ).5 T6 (q6 ) (6)
The position of the end effector in the reference franme R0 is given by:
0  n 
On+1 0 On+1
= T6 (7)
1 1

3
 in our case:    
x 0
n
On+1 = y  =  0  (8)
z d6
The orientation of the end effector is given by the orientation matrix:
0
R6 =0 T6 (1 : 3, 1 : 3) (9)

2.4 Direct differential kinematic model (using the Geo-

metric Jacobien):
It allows to express the operational speed of the end effector as a function of
the joint speeds by the relationship :

V = J(q)q̇ (10)

with:
• J(q) is the jacobian matrix of dimension 6*6 in our case
• q̇ = [q̇1 q̇2 q̇3 q̇4 q̇5 q̇6 ]T
 
V
• V= with
w

– V = (ẋ ẏ ż)T : end effector translation speed

– w = (wx wy wz )T : end effector rotational speed
In order to compute the speed of the end effector first of all we calculate the
speed of the frame Rn .
 
Vn
Vn = (11)
wn
with:
• Vn : translation speed of On (origin of the frame Rn )
• wn : rotation speed of frame Rn
The contribution of the joint j to the translational and rotational speed of
Rn is given by:
Vj,n = (σj zj + σ̄j (zj ∧ pj,n ))q̇j (12)

wj,n = σ̄j q̇j zj (13)

Finally, the translational and rotational speed of Rn are:
n
X
Vn = (σj zj + σ̄j (zj ∧ pj,n ))q̇j (14)
j=1

4
 wn = sumnj=1 σ̄j q̇j zj (15)
in vector form we get:
 
Vn
= Jn q̇ (16)
wn
with

 
σ1 z1 + σ̄1 (z1 ∧ p1,n ) σ2 z2 + σ̄2 (z2 ∧ p2,n ) .
σn zn + σ̄n (zn ∧ pn,n )
Jn =
σ̄1 z1 σ̄2 z2 . σ̄n zn
(17)
in our case the speed Vn and wn are expressed in the reference frame R0 .
After calculating the speed of the frame Rn we then calculate the transla-
tional and rotational speed of the end effector using the following expression:
 
ẋ
 ẏ 
 
 ż 
V= w˙x  = J q̇
 (18)
 
w˙y 
w˙z
 
I D
with: J = 3×3 Jn the jacobian matrix of the robot.
03×3 I3×3
 
0 an xz + rn+1 rz −an xy − rn+1 zy
D = −an xz − rn+1 rz 0 an xx + rn+1 zx  (19)
+an xy + rn+1 zy −an xx − rn+1 zx 0

• the values xx , xy , xz , zx , zy and zz are determined from the rotation matrix

0
Rn .

2.5 Inverse differential kinematic model (using the Geo-

metric Jacobien):
the inverse differential kinematic model allows to express the joits speed as a
function of the end effector operational speed.
the IDKM can be obtained by reversing the DDKM, implementation can be
done analytically or numerically, in our case we use the numerical approach.
We have proceed this way (using the geometric jacobien) for the reason that
obtaining directly the inverse kinematic model for our system of 6 dof is com-
putationally expensive.

q̇ = J −1 (q)Ẋ (20)

5
2.6 results and validation:
2.6.1 Direct kinematic model:
we use the general homogenous transformation matrix form in equation(5) with
the DH table -1- in python to get: T01 ,T12 ,T23 ,T34 ,T45 ,T56 then using equation
6 we calculate: T01 ,T02 ,T03 ,T04 ,T05 ,T06 .

after calculating T06 we calculate 0 R6 the orientation matrix of the end ef-
fector and position of the end effector in reference frame using equation(7)

by running ’part1.py’ we get the following results:

 
0 1 0 0
−1 0 0 0 
T01 =  0 0 1 0.1273
 (21)
0 0 0 1
 
0 0 −1 −0.1639
 0 −1 0 0 
T02 = −1 0
 (22)
0 0.1273 
0 0 0 1
 
0 0 −1 −0.1639
 0 −1 0 0 
T03 = −1 0
 (23)
0 0.7393 
0 0 0 1
 
0 0 −1 −0.1639
1 0 0 0 
T04 = 0 −1 0
 (24)
1.3116 
0 0 0 1
 
0 −1 0 −0.1639
1 0 0 0 
T05 = 0 0 1 1.4273 
 (25)
0 0 0 1
 
0 0 −1 −0.1639
1 0 0 0 
T06 = 0 −1 0
 (26)
1.4273 
0 0 0 1
 
0 0 −1
0
R6 = 1 0 0 (27)
0 −1 0
0
On+1 = [−0.2561 0 1.4273]T (28)

6
 in order to verify this results, we use the code in the first part of the ’main.py’
and we get the following results:
0
On+1 = [−0.2221 0 1.428]T (29)
for the orientation, coppelia doesn’t represent the x axis the same way we
do, so added a π/2 to the yaw angle to get the same orientation of the end
effector frame, then we calculated the orientation matrix 0 R6 and we got:
 
0 0 −1
0
R6 = 1 0 0 (30)
0 −1 0
these results are the same ones that we got before using our model (’part1.py’)
and therefor we can say that our validate is correct.

7
3 Motion Planning
In this section, the goal is to implement a trajectory generation solution in
the operational space, that computes the robot’s instructions in the form of
successive positions of the end effector.

We want to generate a trajectory between an initial configuration and a final

configuration of the end effector that can be described by

Ri xi
 
Ti = (31)
0 0 0 1

Rf xf
 
f
T = (32)
0 0 0 1
Where the motion of the end effector is described by

Rd xd
 
d
T = (33)
0 0 0 1

3.1 Position motion planning

Translation motion is a straight line, given by the following equation:

xd (t) = xi + r(t)(xf − xi ) (34)

with r = r(t) = 10( ttf )3 − 15( ttf )4 + 6( ttf )5

3.2 Orientation motion planning

For the rotational movement, we perform a roration movement with an angle ϑ
around an axis u
the rotation is giving by:

Ri rot(u, ϑ) = Rf (35)
 
xx yx zx
rot(u, ϑ) = (Ri )T Rf = xy yy zy  (36)
xz yz zz

8
 with: p
Cϑ = 1/2(xx +yy +zz −1) and Sϑ = 1/2( (yz − zy )2 + (zx − xz )2 + (xy − yx )2 )
and so the angle ϑ and u are calculated with the following expressions:

ϑ = atan2(Sϑ, Cϑ) (37)

   
ux yz − zy
1 zx − xz 
u = uy  = (38)
2Sϑ
uz xy − yx
(this expressions are used for case of Sϑ ̸= 0 in the other case simply we
keep the same orientation during all the trajectory and therefor we don’t need
to calculate u and ϑ)
finnaly we have:
u2x (1 − cos(rϑ) + cos(rϑ)
 
ux uy (1 − cos(rϑ)) − uz sin(rϑ) ux uz (1 − cos(rϑ)) + uy sin(rϑ)
rot(u, r(t)θ) = ux uy (1 − cos(rϑ)) + uz sin(rϑ)
 u2y (1 − cos(rϑ) + cos(rϑ) uy uz (1 − cos(rϑ)) − ux sin(rϑ)
ux uz (1 − cos(rϑ)) − uy sin(rϑ) uy uz (1 − cos(rϑ)) − ux sin(rϑ) u2z (1 − cos(rϑ) + cos(rϑ)
(39)
we use this expression to calculate the rotation matrix at each step time:

Rd (t) = Ri rot(u, r(t)ϑ) (40)

3.3 test and validation:

to test the trajectory generation in python ’part2.py’ we suppose this trajectory:
(with the time vector goes from 0 to 20 with 100 samples.)
 
0
xi = 0
0
 
0.2
xf = 0.1
0.5
 
1 0 0
Ri = 0 1 0
0 0 1

which in eular angles is: [0. 0. 0.]

 
0.433 −0.75 0.5
Ri = 0.6495 −0.125 −0.75
0.625 0.6495 0.433

which in eular angles is: [0.9828 − 0.6751 0.9828]

9
we plot the evolution of the trajectory in time, and the evolution of x,y,z,roll,yaw,pitch
in time:

Figure 2: evolution of x,y and z in time

10
 Figure 3: evolution of roll, yaw and pitch in time

We can see here that the evolution of trajectory in space is a line as we

supposed, and the evolution of x,y,z,roll,yaw and pitch in time are smooth and
does reach their final values when t = tf , therefor the trajectory generation is
valid.

11
4 Motion Control
In this part, we’ll focus on the motion control based on kinematic control, using
the model and trajectory generation already developed.

4.1 Position control :

We want to control the end effector position of our robot defined by the model

q̇ = J −1 ẋ (41)

assuming that the actuators are correctly controlled :

q̇ = q̇ d (42)

The desired control that make the error position converge to zero

ẋ = ẋd + Kp ep =⇒ e˙p + Kp ep = 0 (43)

for Kp positif, is :
q̇ = J −1 (ẋd + Kp ep ) (44)

4.2 Orientation control :

Now, we need a similar control aproach for the end effector orientation, where
the error of orientation is defined as follow
1 e
eo = (n ∧ nd + se ∧ sd + ae ∧ ad ) (45)
2
such that
R d = nd sd ad


(46)
R e = ne se ae


(47)
And the desired orientation (obtained from trajectory generation) is

Rd = Ri rot(u, r(t)ϑ) (48)

The evolution of the orientation error over time is

e˙o = LT wd − Lwe (49)

with
−1
L= (S(nd )S(ne ) + S(sd )S(se ) + S(ad )S(ae )) (50)
2
And the matrix S is defined as follow
 
0 −nz ny
S(n) =  nz 0 −nx  (51)
−ny nx 0

12
to get a control law that make the orientation error converge to zero we proceed
as we did with position control, and for Ko positif the final control low for the
end effector pose is :

ẋd + Kp ep
 
q̇ d = J − 1 (52)
L− 1(LT wd + Ko eo )

4.3 results and validation :

In this part we want to apply the control strategy obtained to accomplish a
motion from  
1.06034935

 0.53317201 

 0.56651503 
X0 =   (53)
 0.30910149216651917 

−0.4955812096595764
0.5295810699462891
to  
−0.22134912

 0.14831257 

 1.37516093 
Xf = 
  (54)
 1.218836784362793 

−1.554880142211914
1.5740549564361572

(a) initial configuration (b) final configuration

Figure 4: Simulation of motion

Applying the control strategy with the following gains

 
0.5 0 0
kp =  0 0.5 0  (55)
0 0 0.5

13
  
1 0 0
Ko = 0 1 0 (56)
0 0 1
lead to this evolution of position and orientation error

Figure 5: Position error

We can notice that the error position on Y has growth in a large amount of
time, then converge to 0 while X and Z position are quite robuste.

Figure 6: Angle error

We can notice here, that the orientation error is going crazy on ψ and θ
while γ error is small.
Finally we can conclude that, the control strategy wasn’t really succesful
which can be illustrated on the non-smooth simulation we get, spending more
time tunning the right gain control may offer better results.

14