Robotics LAB – SSIR’08

Modeling, Trajectory Generation and

Control of Robot Arms

Dr. Lounis ADOUANE

LASMEA-CNRS
Lounis.Adouane@lasmea.univ-bpclermont.fr
 23th June - 4th July Robotics Lab

Modeling, Trajectory Generation and

Control of Robot Arms

Overview
The objective of this robotics lab is to address the main modeling and control notions for robot arms
using MATLAB/Simulink. The transition from joint to working space and vice versa will be treated
pointing up the advantages and drawbacks in using each space. Kinematical and dynamical models for
SACRA robot with 2 rotational articulations will be used. Other notions will be also treated such as:
polynomial methods for trajectory generation; the use of PID or Computed-Torque controller to
follow robot set-point; and finally the application of obstacle avoiding strategy. For necessary models
or definitions to answers to the questions, you can use the Appendix.

It is to note that the simulation of the studied robot will be made firstly without using the robot
dynamical model (Part I). It is only thereafter (Part II) that the dynamical model will be used in order
to address accurately robot control problem.

INTRODUCTION
The SCARA robot (Selective Compliance Assembly Robot Arm) (Figure. 1(a)) is one of the more
used robots in industry. The version with two degrees of freedom (2 dof) of SCARA is going to serve
us in this Lab as basis for the modeling and control of robotics system. The configuration of the robot
is determined by the joint variables θ1 and θ2 (Figure 1(b)).

yb
E

l2
θ2

β
l1
α θ1 xb

(a) (b)

Figure 1

Lounis ADOUANE 2
 23th June - 4th July Robotics Lab

This Lab intends firstly to implement under MATLAB/Simulink the different elementary modeling
related to a SCARA robot, and thereafter to attach the set of these programs to other programs
(previously provided) in the aim to obtain user-friendly and interactive robotics mini-simulator.

Before performing any task, download the directory “RoboticsLAB” (from \\pcservnt02\SSIR08)
toward your workspace. The files and their content are described briefly below. Test and explore
attentively all recovered programs and Simulink models in order to understand the methods
adopted to achieve the simulation. In certain cases you must add to some of these programs parts of
codes (for m-files) or schemas-blocks (for Simulink models) which will permits to enhance the mini-
simulator functionalities. Note that, for a better readability of your programs, it is strongly
recommended to comment them.

Function (m-file .m or Simulink file .mdl) Description

Main.m Permits to open one user interface (Figure 2) to
simulate the robotics system with or without using
Simulink blocks. One graphical representation of
the robot SCARA 2dof as well as the possibility to
animate it is accessible via this interface. This
interface permits also to recover the mouse events.
In this case, the mouse will allow you, in the setting
of your Lab, to give position set-point (x, y) to be
reached by the end-effector.

ForwardKinematics.m Defines the positions (xT, yT) of the SCARA end-

effector according to its joints coordinates.

InverseKinematics.m Defines the joints coordinates of the SCARA

according to the position of its end-effector.
The output function gives [Q1, Q2, err] with Q1 the
solution when θ2>0 (low elbow), Q2 the second
solution. To satisfy some constraints linked to the
robot workspace one imposes that:
- the position of the end-effector belongs to
the working space (cf. Figure 2)
- the values of θ1 and θ2 vary in the robot
joint domain (i.e. 0°≤θ1≤180°;
-180°<θ2<180°).
Thus InverseKinematics.m function gives: err(1)=0
if the first solution Q1 is possible and =1 if the
solution is not possible, and in the same way err(2)
indicates the feasibility of the second solution.

InverseKinematicsUsingGeometry.m Defines, using simple geometrical construction, the

joint coordinates of the SCARA according to the
position of its end-effector.

SetPointTrajectory.m Gives the set-point to follow by the robot end-

effector in the work space domain.

GUI_Management.m Manage all events (click of mouse, pressed button,

etc.) of the GUI (Graphical User Interface)
window.

Lounis ADOUANE 3
 23th June - 4th July Robotics Lab

SetDispaly.m Updates the graphical representation of the SCARA

robot as well as the display of the joint information
and the position of the end-effector (Figure 2).

SimulinkRobotControlWithoutDyMo.mdl Simulink model (Figure 3) that permits while

interfacing it with the programs described above to
control the movement of the SCARA in order to
follow for example a trajectory.

Figure 2

Figure 3

Lounis ADOUANE 4
 23th June - 4th July Robotics Lab

PART I - MODELING AND TRAJECTORY GENERATION

I FORWARD AND INVERSE KINEMATICS
I.1) For the case of simple robotics structures such as the one used in this Lab, it is possible to find
the inverse kinematics model by only merely geometrical reasoning. That is what is implemented in
InverseKinematicsUsingGeometry.m function.

Use the InverseKinematics.m function so that no one of the robot joints leaves the robot workspace
(cf. figure 2) during the movement execution.

Indication: use the possibility to choose what solution (Q1 (low elbow) or Q2 (high elbow)) to assign
to the robot final position.

II FORWARD AND INVERSE INSTANTANEOUS KINEMATICS

II.1) Knowing the Forward Instantaneous Kinematics Model (FIKM) of the SCARA robot given by
ForwardInstantaneousKinematics.m function, program the Inverse Instantaneous Kinematics Model
(IIKM) InverseInstantaneousKinematics.m.

While interfacing your IIKM to the Simulink diagram (cf. Figure 4), simulate a rectilinear
displacement with constant speed of the end-effector according to its x axis (use the provided interface
to observe the result).

II.2) It is imperative to manage correctly the singular robot configurations in order to warn all erratic
movement of the robot. What are the singular positions of the studied robot? Use this knowledge so
that the robot avoids these singular configurations.

Indications:
- the only program to modify is the one where you defined the robot IIKM,
- for the singularities in the robot workspace limits, one can impose software stops of the robot
angles evolutions in order to stop rightly before the configuration “completely tense arm” or
“completely folded arm”.

Figure 4

Lounis ADOUANE 5
 23th June - 4th July Robotics Lab

III TRAJECTORY GENERATION (WORKING AND JOINT SPACE

CONFIGURATION)
III.1) Basing on the SetPointTrajectory.m function, give the end-effector a circular trajectory (with
ray = 2 and center C=(0, 7.5)).

III.2) Basing on the Order1Interpolation.m file, write a 5 degrees interpolator generator for the
SCARA robot between 2 points of the joint space qi=[100° 100°] to qf=[6° 60°].

PART II – ROBOT ARM CONTROL

This part of the Lab introduces the dynamical model of the SCARA robot in order to address some
problematic linked to robot arm control. You will find below the description of a set of programs
attached to those given previously.

ForwardDynamicalModel.m Defines the acceleration of the robot joints according to

the torques applied by its actuators.

SimulinkLabLibrary.mdl Contains Simulink blocks to be used directly in your

Lab.

SimulinkRobotControlWithDyMo.mdl Simulink model (cf. Figure 5) that permits to use a PID

controller to control the robot in the working space.

Figure 5

IV PID CONTROL
IV.1) The present parameters of the PID (figure 5) have been fixed empirically. Synthesize PID
controllers adapted to the used SCARA robot basing on the Annex 5. Once the parameters of the PID
controllers are obtained, you must automatically affect them to the corresponding blocks.

Lounis ADOUANE 6
 23th June - 4th July Robotics Lab

Indications:
- take wj = 2.5 to find the Kpi=1..2, Kdi=1..2, Kii =1..2 parameters,
- a part of the code to obtain the PID parameters is already provided, you have just to complete
the code lines in the Main.m file, more specifically between “%% Beginning PID controller
code %%” and “%% End PID controller code%%”,
- you can display the features characterizing the SACRA robot control when it follow the path
while removing the comments in the GUI_Management.m file and more precisely between
“%% Beginning display figures code %%” and “%% End display figures code %%”.

IV.2) Modify the SimulinkRobotControlWithDyMo.mdl and GUI_Management.m files in order to

permit to control the SCARA robot in its joint space using a polynomial interpolation of 5 degrees.

V COMPUTED-TORQUE CONTROL
Using the functions used above and annex 6, implement the Computed-Torque control on the SCARA
robot.

VI OBSTACLES AVOIDING (GENERATION OF MOVEMENT WITH

INTERMEDIATE POINTS CROSSING)

In some applications, the robot arm is faces to the presence of obstacles in the environment. Thus, to
enable these robots to achieve the movements without collision, it is imperative to have an appropriate
strategy for obstacle avoiding. In the following section, we aim to study a simple case where the
environment is cluttered with two circular obstacles (cf. Figure 6) and we want to displace the end-
effector from its initial position Pi=[10,7 9,06]T toward the final position Pf=[-1,8 12,9 ]T.

VI.1) Propose a method to join the position Pf from the position Pi while avoiding the two obstacles?

Indication: in order to permit to display the obstacles on the MATLAB interface, uncomment the
section between “%% Beginning definition obstacles %%” and “%% End definition obstacles %%” in
the Main.m program.

Final Obstacle 2
Obstacle1
position Pf
Initial
position Pi

Figure 6

Lounis ADOUANE 7
 23th June - 4th July Robotics Lab

APPENDIX AND BIBLIOGRAPHY

Annex 1 (Forward and Inverse Kinematics Models)

Forward Kinematics
The Forward Kinematics Model (FKM) for a serial-chain manipulator consists to find the position and
orientation of end-effector relative to the base given the positions of all of the joints and the values of
all of geometric link parameters. In the case of SCARA robot given in Figure 1, we have:
X = f (θ )
Where:
T
θ = ⎡⎣θ1,θ2 ⎤⎦ Vector of the joint space variables
T
X = ⎡⎣ x , y
T T
⎤⎦ Vector of the work space variables

Inverse Kinematics
The Inverse Kinematics Model (IKM) for a serial-chain manipulator consists to find the values of the
joint positions given the positions and orientations of the end-effector.
θ = f −1 ( X )

Annex 2 (Forward and Inverse Instantaneous Kinematics Models)

Forward Instantaneous Kinematics

The Forward Instantaneous Kinematics Model (FIKM) for serial-chain manipulators can be
formulated as: given the positions of all members of the chain and the rates of motion about all the
joints, find the total velocity of the end-effector. This is given by:
X = J (θ )θ
Where: J (θ ) Jacobian matrix (m×n)

Inverse Instantaneous Kinematics

The Inverse Instantaneous Kinematics Model (IIKM) for serial-chain manipulators can be formulated
as: given the positions of all members of the chain and the total velocity of the end-effector, find the
rates of the motion of all joints. The general case (when m ≠ n) gives.
θ = J + (θ ) X
Where: J + = ( J T J ) −1 J T pseudo-inverse matrix of J.
θ is called singular joint configuration when J + (θ ) = 0 .

You can get more information via http://wwwlasmea.univ-bpclermont.fr/Control/ssir08/martinet.pdf

(p21 to 24) or in the Lab folder “DocForAppendix”.

Annex 3 (Trajectory generation)

You can get information about this subject via http://wwwlasmea.univ-

bpclermont.fr/Control/ssir08/martinet.pdf (p14 to 20) or in the Lab folder “DocForAppendix”.

Lounis ADOUANE 8
 23th June - 4th July Robotics Lab

Annex 4 (Dynamical model)

You can get information about this subject via http://wwwlasmea.univ-

bpclermont.fr/Control/ssir08/martinet.pdf (p25 to 26) or in the Lab folder “DocForAppendix”.

Annex 5 (PID Control)

In this type of control, the values of PID actions are the following:
⎧ K Pj = 3a jω 2j
⎪⎪
⎨ K Dj = 3a jω j
⎪
⎪⎩ K Ij = a jω j
3

Where:
- “j” indicates the index of the corresponding joint,
- aj = Ajj max indicates the maximal value of the Ajj element of the inertia matrix A of the robot,
- ω j > 0 are chosen the biggest possible, however, this pulsation should not be superior to the
pulsation of resonance of the mechanical system in order to not destabilize it.

You can get more information via http://wwwlasmea.univ-bpclermont.fr/Control/ssir08/martinet.pdf

(p27 to 28) or in the Lab folder “DocForAppendix”.

Annex 6 (Computed-Torque Control)

You can get information about this subject via http://wwwlasmea.univ-

bpclermont.fr/Control/ssir08/martinet.pdf (p28 to 30) or in the Lab folder “DocForAppendix”.

Bibliography

Modeling, Identification and Control of Robots, Authors: Khalil W., Dombre E., Editor: Penton,
ISBN10: 1-9039-9613-9, 2002

Springer Handbook of Robotics, Siciliano B.; Khatib O., Editor: Springer, ISBN: 978-3-540-23957-4,
2008

Lounis ADOUANE 9