Université de Carthage

Ecole Polytechnique de Tunisie

Mini-Projet
Robotique – PUMA560

Fehmi NAJAR
Professor of Mechanical Engineering
 3/1/2023

Text Book
◼ Peter Corke, Robotics, Vision and Control, Springer, 2011.
◼ Matlab Robotics toolbox (Peter Corke)
Online Youtube course on Robotics (Oussama Khatib, Stanford university) :
https://www.youtube.com/watch?v=0yD3uBshJB0&list=PL65CC0384A1798ADF

2
 3/1/2023

Homogeneous transformation

◼ Rotation matrix ◼ Rotation matrix to Euler angles

❑ T=rotx(pi/2) ❑ gammaE = tr2eul(T2)

❑ trplot(T) ◼ Rotation matrix to RPY angles

❑ tranimate(T) ❑ gammaRPY = tr2rpy(T3)

◼ Combination of rotations ◼ Equivalence RPY vs Euler angles

❑ T1=rotx(pi/2) * roty(pi/2) ❑ gammaRPY = tr2rpy(T2)

❑ tranimate(T1)

◼ Euler angles to Rotation matrix

❑ T2 = rotz(1) * roty(1.5) * rotz(1)

❑ T2 = eul2r(1, 1.5, 1)

❑ tranimate(T2)

◼ RPY angles to Rotation matrix

❑ T3 = rotx(1) * roty(1.5) * rotz(1)

❑ T3 = rpy2r(1,1.5,1)

3
 3/1/2023

Homogeneous transformation

◼ Homogeneous transformation
❑ H = transl(1, 0, 0) * trotx(pi/2) * transl(0, 1, 0)

❑ trplot(H)

❑ tranimate(H)

◼ Extract rotation matrix

❑ t2r(H)

◼ Extract translation
❑ transl(H)'

4
 3/1/2023

Forward Kinematics

◼ DH parameter to Homogeneous transformation: Order theta, d, a, alpha

❑ L = Link([0, 0.1, 0.2, pi/2, 0])

❑ L = Link([0, 0.1, 0.2, pi/2, 0], 'modified')

❑ L.A(0.5) calculate the matrix for theta=0.5

❑ L.RP indicates if the link is prismatic or revolute

❑ L.a returns the kinematic parameter a

◼ Create a Robot (RR):

◼ First we define the DH parameters
❑ L(1) = Link([0 0 1 0]);

❑ L(2) = Link([0 0 1 0]);

◼ We construct the robot using the command SerialLink

❑ two_link = SerialLink(L, 'name', 'two link')

❑ two_link.n : number of joints

❑ two_link.fkine([0 0]) : forward kinematics transformation

❑ two_link.plot([0 0]) or two_lin.plot([3*pi/4 pi/4])

5
 3/1/2023

Kinematics of an industrial robots: PUMA560

◼ PUMA (Programmable Universal Manipulation Arm) is one of the first industrial

robot arm developed by Victor Scheinman at Stanford University, then
commercialized by IBM, Nokia, Unimation, …
◼ PUMA 560 is a robot with six degrees of freedom and all rotational joints (6R
mechanism).
◼ It is a Anthropomorphic robot, i.e. having human-like characteristics.

6
 3/1/2023

Kinematics of an industrial robots: PUMA560

◼ PUMA 560 (RRR)

i i 1
ai 1 di i
SC
1 0 0 0 1
0
2 2
0 0 2
0
3 0 a2 d3 3
0
4 2
a3 d4 4
0
5 2
0 0 5
0
6 2
0 0 6
0

7
 3/1/2023

Kinematics of an industrial robots: PUMA560

◼ PUMA 560
❑ mdl_puma560akb

❑ p560m

❑ p560m.plot(qz) zero angle configuration

❑ p560m.plot(qr) nominal configuration

❑ p560m.fkine(qz) forward kinematics for qz configuration

❑ p560m.tool = transl(0, 0, 0.2) we can add a tool representing a 200 mm extension

in the z direction
❑ p560m.fkine(qz) the new forward kinematics

❑ p560m.base = transl(0,0,1) * trotx(pi) move the base : This robot is now hanging

from the ceiling.

8
 3/1/2023

Inverse Kinematics

Puma 560: Analytical solution

◼ mdl_puma560
◼ qn=[0 pi/4 pi 0 pi/4 0] : the nominal joint coordinates
◼ p560.plot(qn)
◼ T = p560.fkine(qn) : calculating the end-effector pose
◼ qi = p560.ikine6s(T) : Since the Puma 560 is a 6-axis robot arm with a spherical wrist we use the
method ikine6s to compute the inverse kinematics using a closed-form solution
◼ p560.fkine(qi) : joint coordinates are different but lead to the same end-effector pose
◼ p560.plot(qi) : left hand and right hand solutions of the arm with respect to the waist
◼ qiu = p560.ikine6s(T, 'ru') this will force the right handed configuration with the elbow up. (left
or right handed 'l', 'r‘ elbow up or down 'u', 'd')
◼ p560.plot(qiu)
◼ qid = p560.ikine6s(T, 'rd') this will force the right handed configuration with the elbow up.
left or right handed 'l', 'r‘ elbow up or down 'u', 'd' wrist flipped or not flipped 'f', 'n'
◼ p560.plot(qid)
◼ qp=p560.ikine6s( transl(0.5, 0.5, 0.5) ) : inverse solution for the pose (0.5 0.5 0.5)
◼ p560.plot(qp)
Puma 560: Numerical Solution
◼ qi = p560.ikine(T)
◼ p560.fkine(qi) : no control of the arm configuration
◼ qi = p560.ikine(T, [0 0 3 0 0 0]) implicit control via the initial value of joint coordinates

9
 3/1/2023

Jacobian of the PUMA 560

◼ mdl_puma560 declaring the model

◼ T0 = p560.fkine(qn) Forward kinematics in the nominal configuration
◼ J = p560.jacob0(qn) Jacobian with respect to the base frame
◼ p560.jacobn(qn) Jacobian with respect to the end-effector frame
◼ p560.jacob0(qn, 'eul') using the Euler angle to compute the Jacobian

10
 3/1/2023

Singularities of the PUMA560

◼ mdl_puma560
◼ J = p560.jacob0(qr)
◼ rank(J)
◼ jsingu(J) detect singularity
◼ q = qr
◼ q(5) = 5 * pi/180 theta5=5deg : ill-conditioned position
◼ J=p560.jacob0(q)
◼ det(J)
◼ qd = inv(J)*[0 0 0.1 0 0 0]’
◼ qd‘ : the elbow have to move at 9.85 rad.s–1 or nearly 600 deg.s–1

11
 3/1/2023

Dynamics of the PUMA 560 using the Robotic Toolbox

M (q)q + C(q, q)q + F(q) + G (q) + J (q)T f = Q

◼ M is the joint-space inertia matrix, C is the Coriolis and centripetal coupling matrix, F
is the friction force, G is the gravity loading, and J is the manipulator Jacobian. The
last term of the left hand side are the joint forces due to a wrench f applied at the
end effector.
◼ mdl_puma560 load the robot
◼ p560.links(1).dyn displays the dynamic properties of link 1
◼ p560.links(2).dyn displays the dynamic properties of link 2
◼ m is the mass of the link, r is the position of the COM with respect to the link
coordinate frame I is the inertia matrix about the COM with respect to axes aligned
with the link frame.
◼ Jm is the motor inertia, Bm is the motor friction, Tc is the Coulomb friction and G the
gear ratio.
◼ p560.gravload(qn) gravity load at joints at nominal position
◼ p560.gravity = p560.gravity/6 change gravity to the lunar value
◼ p560.gravload(qs) arm is stretched out horizontally
◼ p560.gravload(qr) arm straight up
12
 3/1/2023

Dynamics of the PUMA 560 using the Robotic Toolbox

◼ p560.inertia(qn) mass matrix at nominal position

◼ qd = 0.5*[1 1 1 1 1 1] all joints moving at 0.5 rad s–1
◼ C=p560.coriolis(qn, qd) Coriolis matrix
◼ C*qd’ joint torques
◼ p560.rne(qn, qz, qz) recursive Newton-Euler algorithm. Torques at the nominal pose,
and with zero joint velocity and acceleration.
◼ Forward Dynamics
◼ Q = p560.gravload(qn); Payload
◼ qd = 0.5*[1 1 1 1 1 1]; velocities
◼ qdd = p560.accel(qn, qd, Q) forward dynamics
◼ Puma560 collapsing under gravity
◼ sl_ztorque
◼ r = sim('sl_ztorque');
◼ t = r.find('tout'); the joint angles as a function of time
◼ q = r.find('yout'); the joint angles as a function of time
◼ p560.plot(q)
◼ plot(t, q(:,1:3))
13
 3/1/2023

Trajectory generation using the Robotic Toolbox

◼ s = tpoly(0, 1, 50); generates a quintic

polynomial trajectory from 0 to 1 with
50 time steps.
◼ plot(s)
◼ [s,sd,sdd] = tpoly(0, 1, 50); position,
velocity and acceleration
◼ s = tpoly(0, 1, 50, 0.5, 0); initial and
final velocities are set to 0.5 and 0.
◼ mean(sd) / max(sd)
◼ s = lspb(0, 1, 50); Linear function with
parabolic blends.
◼ [s,sd,sdd] = lspb(0, 1, 50);
◼ s = lspb(0, 1, 50, 0.035); max speed
◼ x = mtraj(@lspb, [0 2], [1 -1], 50);
multidimensional case: 2DOF

14
 3/1/2023

Trajectory generation using the Robotic Toolbox

◼ The Toolbox function mstraj generates a multi-segment multi-axis trajectory based on a

matrix of via points with constant blend time (2tb).
◼ For example 2-axis motion with four points can be generated by
◼ via = [ 4,1; 4,4; 5,2; 2,5 ]; matrix of via points, one row per point.
◼ q = mstraj(via, [2,1], [], [0,0], 0.05, 0.5); the arguments are (via, vector of maximum
speeds per axis, vector of durations for each segment “zero matrix if the first is
specified”, initial axis coordinates, sample interval, acceleration time 2tb)
◼ q = mstraj(via, [2,1], [], [0,0], 0.05, (1,5));

◼ Interpolation of orientation in 3D 4.5

◼ R0 = rotz(-1) * roty(-1); 3.5

◼ R1 = rotz(1) * roty(1); 2.5

◼ rpy0 = tr2rpy(R0); 1.5

◼ rpy1 = tr2rpy(R1); 0.5

0
0 50 100 150 200 250

◼ rpy = mtraj(@tpoly, rpy0, rpy1, 50);

◼ tranimate( rpy2tr(rpy) );

15
 3/1/2023

Trajectory generation using the Robotic Toolbox

◼ Cartesian Motion
◼ T0 = transl(0.4, 0.2, 0) * trotx(pi);
◼ T1 = transl(-0.4, -0.2, 0.3) * troty(pi/2)*trotz(-pi/2);
◼ Ts = trinterp(T0, T1, [0:49]/49); translation is linearly
interpolated, rotation is spherically interpolated using
quaternion (4x4x50)
◼ P = transl(Ts); translational part
◼ plot(P);
◼ tranimate(Ts);
◼ rpy = tr2rpy(Ts); then plot(rpy);
◼ Ts = trinterp(T0, T1, lspb(0,1, 50) ); Using
parabolic blend.
◼ Or Ts = ctraj(T0, T1, 50);
◼ P = transl(Ts); translational part.
◼ plot(P);
◼ tranimate(Ts);
◼ rpy = tr2rpy(Ts); then plot(rpy);
16
 3/1/2023

Trajectory generation using the Robotic Toolbox

PUMA 560 Robot: Joint-Space Motion

◼ Consider the end-effector moving between two Cartesian poses
◼ T1 = transl(0.4, 0.2, 0) * trotx(pi);
◼ T2 = transl(0.4, -0.2, 0) * trotx(pi/2);
◼ initial and final joint coordinate vectors associated with these poses are
◼ q1 = p560.ikine6s(T1);
◼ q2 = p560.ikine6s(T2);
◼ t = [0:0.05:2]'; time period of 2 seconds in 50 ms time steps
◼ A joint-space trajectory is formed by smoothly interpolating between two
configurations q1 and q2. The scalar interpolation functions tpoly or lspb can be
used in conjunction with the multi-axis driver function mtraj
◼ q = mtraj(@tpoly, q1, q2, t);
◼ q = mtraj(@lspb, q1, q2, t);
◼ q = jtraj(q1, q2, t); or [q,qd,qdd] = jtraj(q1, q2, t); is equivalent to mtraj with tpoly
interpolation but optimized for the multiaxis case and also allowing initial and final
velocity to be set.
◼ q = p560.jtraj(T1, T2, t); direct application to the manipulator
17
 3/1/2023

Trajectory generation using the Robotic Toolbox

PUMA 560 Robot: Joint-Space Motion

◼ p560.plot(q)
◼ plot(t, q(:,2)) second angle only vs. time
◼ qplot(t, q); all angles vs. time
◼ T = p560.fkine(q); end-effector behavior in
Cartesian Space.
◼ plot(t, tr2rpy(T))
◼ p = transl(T);
◼ plot(t,p)
0.5
◼ plot(p(:,1), p(:,2))
0.4

0.3

0.2

0.1

-0.1

-0.2
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
18
 3/1/2023

Trajectory generation using the Robotic Toolbox

PUMA 560 Robot: Cartesian-Space Motion

◼ Ts = ctraj(T1, T2, length(t));
◼ plot(t, transl(Ts));
◼ plot(t, tr2rpy(Ts));
◼ qc = p560.ikine6s(Ts);
◼ qplot(t, qc);
◼ What should we use to look at the trajectory of
the end-effector?
4
q1
q2
3
q3
q4
2 q5
q6

1
q

-1

-2

-3
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
time
19
 3/1/2023

Trajectory generation using the Robotic Toolbox

PUMA 560 Robot: Motion through singularity

◼ T1 = transl(0.5, 0.3, 0.44) * troty(pi/2);
◼ T2 = transl(0.5, -0.3, 0.44) * troty(pi/2);
◼ Ts = ctraj(T1, T2, length(t));
◼ qc = p560.ikine6s(Ts); analytical solution
◼ qplot(t, qc);
◼ qc = p560.ikine(Ts); numerical solution
◼ qplot(t, qc);
◼ Plot the trajectories using a joint-space approach
Configuration Change
◼ T = transl(0.4, 0.2, 0) * trotx(pi);
◼ qr = p560.ikine6s(T, 'ru');
◼ ql = p560.ikine6s(T, 'lu');
◼ q = jtraj(qr, ql, t);
◼ p560.plot(q) Although the initial and final
end-effector pose is the same, the robot makes
some quite significant joint space motion
20