RTSS-Morelli PDF

An open-source package for analysis and control of
serial-link robotic manipulators
M. Morelli
mmorelli@users.sourceforge.net
Centro E.Piaggio
Facolt di Ingegneria
Universit di Pisa
Pisa, Italy
25/05/2011, A21, Robotica 1
M. Morelli (Centro E.Piaggio, UNIPI) the Robotics Toolbox for Scilab/Scicos 25/05/11, A21, Robotica 1 1 / 109
Outline
1 Robotic manipulators modelling with ScicosLab

Rigid body transformations
Building serial-link manipulator models
Analysis of serial-link manipulators
2 Robot control systems design using Scicos

Basic concepts
Motion control
Further applications
3 Robot control code generation for use with Linux RTAI

Developing RTAI-based centralized controllers with RTAI-Lab
RTSSthe

Robotics Toolbox for Scilab/Scicos
M ATLAB Robotics Toolbox (MRT) as source of inspiration
About MRT [Corke, 1996]

Developed by Dr. Peter Corke (CSIRO, Australia);
very popular toolbox (more than 10500 downloads!);
release 8 (December 2008) is under GNU LGPL;
available at http://petercorke.com/.
Purpose of MRT
Enable students and teachers to better understand the theoretical
concepts behind classical robotics.
RTSSthe

Features of MRT
Manipulation of fundamental datatypes such as:
homogeneous transformations;
quaternions;
trajectories.
Functions for serial-link rigid-body manipulators:
forward and inverse kinematics;
differential kinematics;
forward and inverse dynamics.
S IMULINK blockset library.
RTSSthe

Points of strenght
Based on M ATLAB , MRT takes advantage of:
a powerful environment for linear algebra;
a powerful environment for graphical presentation;
an easy integration with other toolboxes;
the S IMULINK environment for dynamic systems simulation.
Main drawback
MRT is an open source software, requiring a proprietary and
expensive software environment to run.
RTSSthe Robotics Toolbox for Scilab/Scicos
Milestone 1 (2008)
Development objective
Porting the functionalities of MRT to Scilab/Scicos version 4.0+, under
a free software license.
Achievements/Features
Porting process successfully completed;
free software licensed under GNU GPL.
Release history
Initial release, the 0.1.0, released in Oct. 2007;
release 0.3.0 available for Scilab-4.1.2 in Feb. 2008;
more than 3000 downloads.
Milestone 2 (2010)
Development objectives
Internal code refactoring and integration with the Scicoss code
generation tools.
Achievements/Features
Modular framework facilitating maintainability and portability;
fully integrated with Linux RTAI/Xenomai code generators.
Release history
Release 1.0.0b1 available for Scilab-BUILD4 in Sep. 2009;
more than 1300 downloads.
Application areas
Education
Gain insights on subjects such as:
homogeneous transformations;
Cartesian and joint-space trajectories;
forward and inverse kinematics;
differential motions and manipulator Jacobians;
forward and inverse dynamics.
Application areas
Potential applications in research

Robot design optimization
Singularity analysis and kinematic optimization;
torques analysis for different arm configurations;
design verification in real-time simulations.
Rapid control prototyping
Development and verification of control tasks;
automatic control code generation;
design optimization through HIL simulations.
ScicosLab Transformations
Outline


Basic concepts
Motion control

Representing 3D translations and orientations

Homogeneous transformations
Coordinate transformation
z0
0
O0
p = 0 o1 + 0 R 1 1 p

y0
x0
0
o1 y
0
p
z1 Compact representation
y1
x P 1 O1
p 0 0
0 1

p R1 o1 p

x1 =
1 0T 1 1
Point P represented in different

coordinate frames

Playing with homogeneous transformations
Example
Z
Create the homogeneous 1.0
0.8
transform 0.6
0.4
w 0.2
Tb = Translx (0.5m)Roty (/2) 0.0
Z
Y
0.2 Z
Solution 0.4
0.6
0.8
twb = rt_transl(0.5, 0, 0)*.. X 1
1.0
rt_roty(%pi/2),
1.0 0
twb = 0.6
0.2
0.2
0.6 1 X
0. 0. 1. 0.5 Y 1.0
0. 1. 0. 0.
- 1. 0. 0. 0.
0. 0. 0. 1. Orientation of frame {B} (colored)
with respect to frame {W} (black)

Other representations of orientation
Euler angles (ZYZ)

Find a vector of Euler angles
describing the rotational part of
RTSS also provides full support
w
for other representations: Tb = Translx (0.5m)Roty (/2)
Euler angles (ZYZ); Rotz (/2)
Roll/Pitch/Yaw angles; Solution
angle and axis;
twb = rt_transl(0.5, 0, 0) *..
unit quaternion. rt_roty(%pi/2) *..
rt_rotz(-%pi/2);
eul = rt_tr2eul(twb),
eul =
0. 1.5707963 - 1.5707963

Demonstration script
Transformations
demos/rt_rttrdemo.sce
ScicosLab Modelling
Outline


Basic concepts
Motion control

ScicosLab Modelling
On the robot modelling capabilities of RTSS

n-DOF manipulator modelling at a glance
RTSS allows to model the typical

industrial manipulator.
Modelling with RTSS

Rigid-body manipulators;
open kinematic chains;
arbitrary number of kinematic
pairs (n).
Typical industrial manipulator

TX90, courtesy of Stubli Group
ScicosLab Modelling

The robot is modelled as a

kinematic chain skeleton.
Kinematic chain representation

n + 1 rigid links connected by n
joint articulations;
single-DOF joints: revolute (R)
or prismatic (P);
one end constrained to a base;
an end-effector mounted to the
other end (EE).
Kinematic chain TX90
ScicosLab Modelling

The geometry of the robot is

defined by attaching reference
frames to each body.
Body frames
{W} is the world frame;
{L0 } is termed base frame;
{Li } is the body-fixed frame
attached to link i;
{E} is the tool frame.
Body frames TX90

ScicosLab Modelling

Geometry kinematics equation
n
Y
w
Te (q) = w T0 i1
Ai (qi ) n
Te
i=1
| {z }
0 T (q)
n
Required model informations

Base/tool transforms;
Geometric relationship
between consecutive links .
Coordinate transforms TX90

ScicosLab Modelling

Dynamics equations (based on Newton-Euler formulation)
q + n(q, q)
B(q) = JT (q)he
+ Fv q + Fs sgn(q)
Effective and coupling inertia

torques; Required model informations
centrifugal, Coriolis and Dynamic model of each
gravitational torques; joint-link pair;
viscous and static friction dynamics of the robot-
torques; environment interaction (if
robot-environment interaction any).
torques.
ScicosLab Modelling
Geometric model of a joint-link pair

The standard Denavit-Hartenberg notation
Axis i Axis i + 1
Axis i 1
ai i
zi0
zi {L }
x i0 i
Link i 1
i xi
Link i
di
zi1
{Li1 } xi1
Standard DH frame assignment

Specifying {Li } with respect to {Li1 }
i1
Ai (qi ) = Translz (di )Rotz (i )Translx (ai )Rotx (i )
ScicosLab Modelling
Geometric model of a joint-link pair

The modified Denavit-Hartenberg notation
Axis i Axis i + 1
Axis i 1
zi
{Li } xi
Link i 1
i
i1
Link i
zi1
{Li1 } xi1 di
zi0
ai1
xi0
Modified DH frame assignment

Specifying {Li } with respect to {Li1 }
i1
Ai (qi ) = Rotx (i1 )Translx (ai1 )Rotz (i )Translz (di )
ScicosLab Modelling
Dynamic model of a joint-link pair

Inertial and actuators/transmissions parameters
Inertial parameters (10)

m link mass
rx , ry , rz link COM
Ixx , Iyy , Izz , Ixy , Ixz , Iyz six components of the inertia tensor about
the link COM
Actuator/transmission model parameters (5)

Jm moment of inertia of the rotor
G reduction gear ratio: joint speed/link speed
B viscous friction coefficient
C+ Coulomb friction (positive rotation) coefficient
C Coulomb friction (negative rotation) coefficient
ScicosLab Modelling
Dynamic model of a joint-link pair

Newton-Euler Formulation (notes)
Approach
Balance of forces and moments
acting on each link.
Forward recursion
Velocities and accelerations.
Backward recursion
Forces and moments along the
Computational structure of the
structure.
Newton-Euler recursive algorithm
ScicosLab Modelling
Modelling a direct-drive planar elbow manipulator

Pelican: experimental robot arm at CICESE (Mexico), Robotics lab.
Geometric and kinematic properties

Vertical planar manipulator;
two rigid links connected via revolute
joints.
Link Notation Value (m)

1 l1 0.26
2 l2 0.26
The Pelican prototype robot
arm [Kelly et al., 2005] Link lengths of Pelican
ScicosLab Modelling

A kinematic model based on the standard Denavit-Hartenberg notation
y0 Joint angle of axis 1 has an offset

of /2 rad, according to DH
x0
notation.
l1
y1
y2 Link i ai i di
10 1 0 l1 10 ? 0
x1 2 0 l2 2? 0
2 x2
l2 Denavit-Hartenberg table
Standard DH frame assignment

ScicosLab Modelling

A kinematic model based on the standard Denavit-Hartenberg notation
Link i ai i di Building the kinematic model
1 0 l1 10 ? 0 -->L1 = rt_link([0,l1,0,0,0], "standard");

-->L2 = rt_link([0,l2,0,0,0], "standard");
2 0 l2 2? 0 -->CL = list(L1, L2);
-->pel = rt_robot(CL, "Pelican",..
"CICESE, Robotics Lab.",..
"Kelly et al. 2005");
Denavit-Hartenberg table -->pel.offset = [-%pi/2; 0];
(previous slide)
ScicosLab Modelling

Model validation by simulation: comparison of results with reasonable expectations
0.2
Example: Where is the tool?
0.1
Pose: q = [ /10 7/25 ] rad
0.0
Forward kinematics
Pelican
0.1 -->>q = [%pi/10, 7/25*%pi];
-->T02 = rt_fkine(pel, q),
T02 =
0.2
y
0.9298 0.3681 0. 0.3221
- 0.3681 0.9298 0. - 0.3430
z 0.3 0. 0. 1. 0.
x 0. 0. 0. 1.
0.4
0.1 0.0 0.1
X 0.2 0.3 0.4 0.5 Draw the Pelican pose
-->ws = [-0.1, 0.5, -0.4, 0.2, -1, 1];
Top view of Pelican pose at -->rt_plot(pel, q, "workspace", ws);
q = [ /10 7/25 ] rad
ScicosLab Modelling

An equivalent kinematic description using the modified Denavit-Hartenberg notation
y0 y1
x0
l1 y2
x1 Link i1 ai1 i di
y20
1 0 0 10 ? 0
10 2 0 l1 2? 0
x2
2 x20 Modified Denavit-Hartenberg table
l2
Modified DH frame assignment
ScicosLab Modelling

An equivalent kinematic description using the modified Denavit-Hartenberg notation
Building the kinematic model: the

Link i1 ai1 i di
quick way
1 0 0 10 ? 0
-->DH = [0,0,0,0;0,l1,0,0];
2 0 l1 2? 0 -->pelm = rt_robot(DH, "Pelican (2)",..
"CICESE,Robotics Lab.", "Modified DH");
-->pelm.mdh = 1;
-->pelm.tool = rt_transl(l2,0,0);
Modified Denavit-Hartenberg table -->pelm.offset = [-%pi/2; 0];
(previous slide)
ScicosLab Modelling

Model validation through kinematic simulation
0.2
Show that the two approaches are
0.1 equivalent.
0.0 Forward kinematics
Pelican (2)
0.1 -->q = [%pi/10, 7*%pi/25];
-->T02m = rt_fkine(pelm, q),
T02m =
0.2
y 0.9298 0.3681 0. 0.3221
- 0.3681 0.9298 0. - 0.3430
0.3 0. 0. 1. 0.
z 0. 0. 0. 1.
x
-->T02 - T02m
0.4 ans =
0.1 0.0 0.1
X 0.2 0.3 0.4 0.5
0. 0. 0. 0.
0. 0. 0. 0.
0. 0. 0. 0.
MDH-based Pelican, at 0. 0. 0. 0.
q = [ /10 7/25 ] rad
ScicosLab Modelling

The dynamics of the Pelican arm: standard vs modified DH frame assignments
lc1
l1
m 1 , I1
10
Question
Which are the parameters that
m2 , I2 depend on the adopted frame
lc2 assignment?
2
l2
Diagram of the Pelican

ScicosLab Modelling

The dynamics of the Pelican arm: standard vs modified DH frame assignments
Parameter Is it convention dependent?

Link mass No
Inertia tensor about link COM No (in this case of study)
Distance to link COM Yes
Actuator/transmission No
Dependence of physical parameters on the frame assignments
ScicosLab Modelling

The dynamics of the Pelican arm: inertial parameters
Description Notation Value Units

Mass of link 1 m1 6.5225 kg
Mass of link 2 m2 2.0458 kg
Inertia rel. to COM (link 1) I1 0.1213 kg m2
Inertia rel. to COM (link 2) I2 0.0116 kg m2
Convention-independent inertial parameters
Link Notation Value (m)

Standard DH Modified DH
1 lc1 -0.1617 0.0983
2 lc2 -0.2371 0.0229
Distance to the link COM (convention-dependent)

ScicosLab Modelling

The dynamics of the Pelican arm: actuator and transmission parameters
Actuators of Pelican
Two brushless DC motors located at the base and at the elbow.
Description Motor/Joint 1 Motor/Joint 2 Units

Sym. Value Sym. Value
Inertia (COM) Jm1 0.012 Jm2 0.0025 kg m2
Gear ratio G1 1:1 G2 1:1
Viscous frict. B1 0.2741 B2 0.1713 Nm s/rad
Coulomb frict. C1 1.29 C2 0.965 Nm
Actuator/transmission parameters
ScicosLab Modelling

Simplified dynamic model based on the standard DH frame assignments
Robot model without actuators and transmissions.
Inertial parameters and gravity ac- Show 1st link data in detail
celeration vector
-->rt_showlink(pel.links(1)),
// kinematic data displayed here...
-->CL(1).I = [0,0,I1,0,0,0]; m = 6.5225
-->CL(2).I = [0,0,I2,0,0,0]; !r = -0.1617 !
-->CL(1).m = m1; ! !
-->CL(2).m = m2; ! 0 !
-->CL(1).r = [lc1;0;0]; ! !
-->CL(2).r = [lc2;0;0]; ! 0 !
-->pel = rt_robot(pel, CL); !I = 0 0 0 !
-->pel.gravity = [0;9.81;0]; // X-Y plane ! !
! 0 0 0 !
! !
! 0 0 0.1213 !
Jm =
G =
B = 0
!Tc = 0 0 !
ScicosLab Modelling

Robot dynamics model validation: reasonable expectations
l1 l2
lc1 m1 lc2 m2
m2 g
m1 g
Stretched Pelican (in X-direction)
Gravitational torque contribution at q = [ /2 0 ] (q = q = 0)

g1 g(m1 lc1 + m2 (l1 + lc2 ))
=
g2 gm2 (lc2 + l2 )
ScicosLab Modelling

Robot dynamics model validation: simulation results
Comparison between expectations and simulation results

-->etau = [g*(m1*lc1 + m2*(l1 + lc2)), g*m2*lc2],
etau =
11.967401 0.4595869
-->etau - rt_frne(pel, [%pi/2,0], [0,0], [0,0]),

ans =
0. - 5.551D-17
ScicosLab Modelling

Complete dynamic model based on the standard DH frame assignments
Robot model including the dynamics of actuators and transmissions.
Modelling motor parameters

-->CL(1).Jm = Jm1;
-->CL(2).Jm = Jm2;
-->CL(1).G = G1;
-->CL(2).G = G2;
Launch the movie!
-->CL(1).B = B1;
-->CL(2).B = B2;
-->CL(1).Tc = Tc1*[1,1];
-->CL(2).Tc = Tc2*[1,1];
ScicosLab Modelling
Modelling a 6-DOF industrial robot (notes)

The industrial robot Siemens Manutec r3
Robot description at a glance

arms mechanically very stiff;
six rotational joints;
current-controlled DC motors;
each motor embedded in the
preceeding arm;
encoders on the motors axes;
friction in the gears;
gear ratios of the base axes fairly
low;
payload of 15 kg. The robot Manutec r3
[Winkler and Such, 2005]
ScicosLab Modelling

A geometric model based on the standard Denavit-Hartenberg notation
Body frames
Kinematic chain
ScicosLab Modelling

A geometric model based on the standard Denavit-Hartenberg notation
Link i ai i di
1.0
1 /2 0 1 d1
0.8
2 0 a2 2? 0
3 /2 0 3? 0 0.6
Z
0.4
4 /2 0 4 d4 z
y
5 /2 0 5 0 0.2 x
6 0 0 6 0 0.0
R3
0.2
Denavit-Hartenberg tablea 0.40

1.00 0.1 0.3 0.5
a 0.3 0.1
Offsets to match the rest config. (the X 0.5
Y
robot is stretched-up at q = 0):
2? = 2 + /2, 3? = 3 /2 Visualization of the Manutec r3

robot at a given q
ScicosLab Modelling

The dynamics of the Manutec r3 robot: actuator and transmission parameters
Description Notation Values Units

Arm 1 Arm 2 Arm 3
Inertia (COM) Jmi 0.0013 0.0013 0.0013 kg m2
Gear ratio Gi 105 210 60
Viscous frict. 1 Bi 8.125 104 7.692 104 1.5385 103 Nm s/rad
Coulomb frict. Ci 0.4 0.5 0.7 Nm
Description Notation Values Units

Arm 4 Arm 5 Arm 6
Inertia (COM) Jmi 1.6 104 1.8 104 4.3 105 kg m2
Gear ratio Gi 99 79.2 99
Viscous frict. 1 Bi 71.2 106 82.6 106 36.7 106 Nm s/rad
Coulomb frict. Ci 0.22 0.38 0.11 Nm
Actuator/transmission parameters, base and wrist axes

1
These parameters have been given reasonable values
ScicosLab Analysis
Outline


Basic concepts
Motion control

ScicosLab Analysis
Analysis of a 6-DOF industrial robot

Positioning/orientation of the Manutec r3 robot
Forward kinematics
// robot configuration Inverse kinematics
q = [0.377, -0.754, -1.711,..
0.754, 2.011, -0.440];
// solve the inverse kinematics (IKP)
q0 = qz; q0(2:3) = [-1, -1];
// pose of the EE
modulo(rt_ikine(r3, twe, q0), 2*%pi),
twe = rt_fkine(r3, q),
ans =
twe =
0.377 - 0.754 - 1.711
0.680 0.572 0.458 0.743
0.754 2.011 - 0.440
- 0.348 0.802 - 0.485 0.294
- 0.645 0.170 0.745 0.465
0. 0. 0. 1.
ScicosLab Analysis

Kinetostatics of the Manutec r3 robot
Differential kinematics
The Jacobian matrix maps velocity be-
ve = J(q)q
tween joint and Cartesian space.
Jacobian analysis
// manipulators Jacobian matrix in base coordinates, and its determinant
j0 = rt_jacob0(r3, q),
j0 =
- 0.294 0.190 0.529 0. 0. 0.
0.743 0.075 0.210 0. 0. 0.
0.000 0.799 0.457 0. 0. 0.
0.000 0.368 0.368 0.582 - 0.228 0.458
0.000 - 0.930 - 0.930 0.231 - 0.874 - 0.485
1. 0.000 0.000 - 0.780 - 0.429 0.745
det(j0), // j0 is not singular

ans =
- 0.261
ScicosLab Analysis

Those configurations at which J(q) is rank-deficient are termed

kinematic singularities.
Problems at (near) a singularity

The mobility of the robot is Classification of singularities
reduced; Boundary, the robot is
infinitely many solutions to stretched/retracted;
the IKP may exist; internal, occur inside the
small velocities at the EE reachable workspace.
cause large joint velocities.
ScicosLab Analysis

r3s shoulder singularity y

zx
// singular robot configuration
qs = [2.011, -1.068, 1.711,..
1.6
0.251, 2.073, -1.257];
1.4
// manipulators Jacobian matrix
1.2
js = rt_jacob0(r3, qs);
1.0
// singular values
0.8
svd(js).,
Z
ans = 0.6 R3
0.3
0.4
1.949 1.571 0.744 0.1
0.529 0.342 0.000 0.2
0.1
0.0
// large joint velocities
dqs = inv(js)*[0.1; 0; 0; 0; 0; 0]; dqs., 0.2 0.3
ans = X
0.3 0.5
0.1 0.1
Y 0.3 0.5
- 219.6 0.052 0.000
105.6 32.66 - 145.6
The Manutec r3 at shoulder

singularity
ScicosLab Analysis

Statics: the manipulator is at an equilibrium configuration

The Jacobian transpose matrix maps the
= JT (q)we
force between Cartesian and joint space.
Torques acting at the actuators

// manipulators Jacobian matrix (wrt EE)
jn = rt_jacobn(r3, q);
// forces acting on the EE (wrt EE)

wn = [-2.887; 2.56; -4.998;.. The Jacobian matrix characterize
-1.697; 1.654; 1.284];
completely the kinetostatics of the
// joint torques to be exerted to keep
// the robot in static equilibrium manipulator.
tn = jn*wn; tn,
ans =
7.228 - 2.323 - 2.044
- 1.299 - 2.219 1.284
ScicosLab Analysis
Trajectory generation and visualization (notes)

A a number of toolbox functions can operate on trajectories
0.2
Trajectories, Animation 0.0

0.2
Joint 2 (rad.)
0.4
demos/rt_rttgdemo.sce 0.6
0.8
demos/rt_rtandemo.sce 1.0
1.2
1.4
Joint space trajectory 1.6

0.0 0.2 0.4 0.6 0.8 1.0 1.2 1.4 1.6 1.8 2.0
1.6
// initial configuration 1.4
qi = [0, -%pi/2, %pi/2, 0, 0, 0]; 1.2
Joint 3 (rad.)
1.0
0.8
// final configuration
0.6
qf = [0, 0, 0, 0, 0, 0]; 0.4
0.2
// travelling time 0.0
t = [0:.056:2]; 0.2
0.0 0.2 0.4 0.6 0.8 1.0 1.2 1.4 1.6 1.8 2.0
Time (s)
// 5th order interpolating polynomial
[q, qd, qdd] = rt_jtraj(qi, qf, t);
Joint space trajectory
ScicosLab Analysis

The inverse dynamics problem for the Manutec r3 robot
8
6
Torque 1 (Nm)
4
2
0
2
4
6
8
0.0 0.2 0.4 0.6 0.8 1.0 1.2 1.4 1.6 1.8 2.0
800
700
Torque 2 (Nm)
600
500
Joint-torques analysis 400
300
200
100
0
100
// joint torques computation 0.0 0.2 0.4 0.6 0.8 1.0 1.2 1.4 1.6 1.8 2.0
tau = rt_frne(r3, q, qd, qdd);
10
0
Torque 3 (Nm)
// contribution of gravitational torques 10
20
taug = rt_gravload(r3, q);
30
40
50
60
0.0 0.2 0.4 0.6 0.8 1.0 1.2 1.4 1.6 1.8 2.0
Time (s)
Joint torques for the given joint

space trajectory (grav. torque in
red)
ScicosLab Analysis

Study of joint inertia of Manutec r3 robot
The gear ratios of the base axes are fairly low, so the actuator inertia
do not dominate the total joint inertia.
This results in significant variations of joint inertia, most important in

joint 1, where the total inertia for the horizontal, loaded arm is more
than three times higher than for the vertical arm.
ScicosLab Analysis

Inertia of joint 1 90
80
70
// manipulators pose definition as a
b11 (kgms^2)
60
// function of joint angles q2 and q3 50
[q2, q3] = meshgrid(-%pi:0.2:%pi); 40
[r, c] = size(q2); 30
q=[zeros(r*c,1) q2(:) q3(:) zeros(r*c,3)]; 20
4
3
// compute inertia matrix & plot results 2
4
b = rt_inertia(r3, q); b11 = b(1,1,:); 1
0
3
2
b11 = matrix(b11,r,c); surf(q2, q3, b11); 1
0
1
2 1
q3 (rad) 3 2
3 q2 (rad)
4
// factor of variation over the path 4
bM = max(b11); bm = min(b11); bM/bm,

ans =
3.5667326 Inertia seen by the joint 1 of the r3
as a function of joint angles q2 and
q3
ScicosLab Analysis

Furthermore, the inertial coupling effects, in particular between joint 2

and 3, cannot be neglected.
30
25
Inertial coupling between joints 2, 3 20
b23 (kgms^2)
// manipulators pose definition as a 15
// function of joint angle q3

q3 = -%pi:%pi/16:%pi; 10
[r, c] = size(q3);
q = [zeros(r*c,2) q3(:) zeros(r*c,3)];
5
// compute inertia matrix & plot results

b = rt_inertia(r3, q); b23=b(2,3,:); 0
4 3 2 1 0 1 2 3 4
b23=matrix(b23,r,c); plot(q3, b23);
q3 (rad)
Coupling effects between joint 2

and 3, while joint 3 is free
ScicosLab Analysis

In short, the properties of this particular six-axis manipulator are a

cross between direct drive robots and high-geared, well-balanced
types that perform well enough with ordinary, robust, independent joint
control.
This makes it a good platform for experimental evaluation of

sophisticated model-based control algorithms.
ScicosLab Analysis
Inner beauties of RNE (notes)

Implementation details of robot dynamics
Dynamics equation (simplified)
q + G(q) =
q + C(q, q)
B(q)
Equivalent code in RTSS

tau = rt_frne(robot, q, Dq, DDq);
Question
How to use the (recursive) balance of all the forces acting on each link
to compute:
G(q);
B(q);
q;
C(q, q)
(argh!).
C(q, q)
q;
B(q)
ScicosLab Analysis
Ready-to-use manipulator models provided by RTSS

The Unimation Puma 560 arm
The Unimation Puma 560

0.5
0.4
z
Kinematic and dynamic data;
x y
0.3
0.2
Standard and modified
0.1 Puma 560
DH-based models;
Z
0.0
0.1 quantities in standard SI units.
0.2
0.3
0.3 0.2
Create a Puma 560 robot
0.1 0.0
// standard DH-based robot object (p560)
0.2
0.1 exec <RTSS-DIR>/models/rt_puma560.sce;
0.4
Y X
0.3 0.6 // modified DH-based robot object (p560m)
exec <RTSS-DIR>/models/rt_puma560akb.sce;
Standard DH-based Puma 560 at

its zero angle pose
ScicosLab Analysis
Ready-to-use manipulator models provided by RTSS

The Stanford manipulator
0.8
0.6
0.4 The Stanford manipulator

Kinematic and dynamic data;
Z
0.2 y
z x
0.0
Standard DH-based model;
0.2
quantities in standard SI units.
0.3 Stanford arm
0.2 Create a Stanford manipulator
0.1
0.0 // standard DH-based robot object (stanf)
0.1 0.2 exec <RTSS-DIR>/models/rt_stanford.sce;
Y 0.0
0.1 0.2 0.1
0.4 0.3 X
Standard DH-based Stanford arm

at a generic pose
Scicos Basics
Outline


Basic concepts
Motion control

Scicos Basics
The Robotics palette

A library of ready-to-use blocks for robotics simulations
Others Trajectory Homogeneous Dynamics

Kinematics
Blocks categories available with

RTSS-1.0.0b1
List of the available palettes

after the installation of
RTSS
Scicos Basics

RTSS
XYZ2Tr
RTSS
RPY2Tr
RTSS
Sine RTSS
Wave RTSS Eul2Tr
Backward
Euler
JTraj RTSS
RTSS
Tr2XYZ
Square
Wave
Forward
Euler Category RTSS
Tr2RPY
Trajectory
Category Others
RTSS
Tr2Eul
Category
Homogeneous
Scicos Basics

RTSS
FKine
RTSS RTSS
noname
RNE Robot
noname noname
RTSS
0
J
noname
RTSS RTSS
Coriolis Accel
noname noname
RTSS
n
J RTSS
noname Gravload
noname
RTSS
RTSS
Inertia
noname
Tr2Diff
Category Dynamics
Category Kinematics
Scicos Basics
Proper setting of model-based blocks

How to set the robot model to be simulated in a block operating on robot objects
Blocks in Kinematics and Dynamics need a model to work.
RTSS
The user must specify the robot
Robot model to be simulated as block
parameter;
noname
the robot model must be a
symbolic parameter defined in
A block which operates on a robot the context of the diagram.
model: the Forward Dynamics
Block (FDB)
Scicos Basics

r3 robot model defined in the context

of the diagram
// robot model definition
// (pseudocode)
r3 = rt_robot( ... );
// Other symbolic parameters

...
FDBs block properties dialog r3

must be entered in the field Robot
object
Scicos Basics

RTSS
Robot
R3
FDB ready to simulate the forward dynamics of the Manutec r3
Scicos Motion control
Outline


Basic concepts
Motion control

Model-based centralized controller for tracking

The inverse dynamics control: short review of the mathematics involved
The dynamic model of a robot

q
y
arm can be rewritten as
B(q)
X
MANIPULATOR q
=
q + n(q, q)
B(q)

n(q, q)
Inverse dynamics control law

y
Z
q
Z
q
= B(q)y + n(q, q)
which leads the system to
Exact lineatization performed by
=y
q
inverse dynamics control

The inverse dynamics control: short review of the mathematics involved
Typical choice for y is
y=q + Kp q
d + Kv q
leading to the error dynamics equation

+ Kp q
+ Kv q
q =0
which converges to zero with a speed depending on the matrices Kv

and Kp chosen.

Inverse dynamics controller design
The inner feedback loop is based on the robot dynamic model.
d
q
q d
q
Kv
X
q
y
B(q)
X X
MANIPULATOR q
qd
q
Kp
X

n(q, q)
STABILIZING LINEAR CONTROL
NONLINEAR COMPENSATION AND DECOUPLING
Block scheme of joint space inverse dynamics control


Scicos implementation of the inverse dynamics controller for the Manutec r3
Key component: the inverse dynamics block (IDB) of the Robotics

palette.
4
~
q q
. RTSS
1
. Kp
q
~ RNE 1
q y
2 Kd R3
..
qd
3
Inverse dynamics controller for the Manutec r3 robot
First-order closed-loop inverse kinematics

On the generation of joint space reference inputs to the motion control system
Considerations regarding the above control scheme

Vectors qd , q d and q
d were assumed available;
joint space references were computed from two joint coordinate
poses directly specified by the user.
However, motion specifications are usually assigned in the operational
space.
Inverse kinematics algorithms transform task space references into

joint space references.

Kinematic inversion of a simple three-link RRR planar arm
l1,2,3 = 0.5 m
x3
y3 Link i ai i di
3
1 0 l1 1 0
l3
x2 2 0 l2 2 0
y2
3 0 l3 3 0
2
y1 Denavit-Hartenberg table
x 1 l2
Robot kinematic model
y0 l1
x 0 1
dh_123=[0,l_123,0,0];
R3arm = rt_robot([dh_123;dh_123;dh_123],..
"RRR arm", "", "Sciavicco-Siciliano");
RRR arm at generic pose


Kinematic inversion of a simple three-link RRR planar arm
Simulation scenario [Sciavicco and Siciliano, 2000]

T
q(0) = /2 /2 [rad];
circular desired motion trajectory for 0 t 4 s:

0.25(1 cos t)
pd (t)
xd (t) = = 0.25(2 + sin t) ;

d (t)
sin t
24
forward Euler numerical integration scheme (t = 1 ms);
final simulation time tend = 5 s.

The Jacobian inverse algorithm
The Jacobian inverse algorithm integrates the joint velocity vector
q = J1 d + Ke)
A (q)(x
x d
xd q q
Z
e
J1
X X
K A (q)

xe
k( )
Jacobian inverse algorithm


Scicos block diagram for the Jacobian inverse algorithm
The Jacobian inverse algorithm and the blocks J1

A (q) and k( )

Analysis of simulation results
1e05 0
0.8e05 1e08
0.6e05 2e08
[rad]
[m]
0.4e05 3e08
0.2e05 4e08
0 5e08
0 1 2 3 4 5 0 1 2 3 4 5
[s]
[s]
Time history of the norm of Time history of the end-effector

end-effector position error orientation error
RRR planar arm performing the task Launch the movie!

Redundancy resolution: short review of the mathematics involved
If the EE orientation is not constrained, a redundant degree of mobility

is available.
The solution of the Jacobian inverse algorithm is generalized into
q = JA (q)(x d + Ke) + (I JA JA )q 0
Convenient utilization of redundant DOFs

w(q)
T k0 > 0;
q 0 = k0
q w(q) secondary objective function.

Solution which maximizes the distance from mechanical joint limits of the RRR arm
Distance from joint limits maximizing the distance;

qi current joint angle;
3 2 qiM maximum joint limit;
1 X qi
qi
w(q) = qim minimum joint limit;
6 qiM qim
i=1
qi middle value.

Simulation scenario
q1m = 2, q1M = 2;
q2m = /2, q2M = /2;
q3m = 3/2, q3M = /2;
k0 = 250.

Solution which maximizes the distance from mechanical joint limits of the RRR arm
Computational code of block

q 0
double * y, * u;
double pi = 3.1415927;
// in/out pointers
y = GetRealOutPortPtrs(block, 1);
u = GetRealInPortPtrs(block, 1);
// computational code
y[0] = -250/3 * u[0]/(16*pi*pi);
y[1] = -250/3 * u[1]/(pi*pi);
y[2] = -250/3 * (u[2]+pi)/(pi*pi);
The Jacobian pseudo-inverse algorithm
with redundancy resolution

Analysis of simulation results: time history of the joint trajectories
Joints 2 and 3 keep far from their min. and max. limit, respectiv.
Joint 2 Position Joint 2 Position

5 5
[rad]
[rad]
0 0
5 5
0 1 2 3 4 5 0 1 2 3 4 5
Joint 3 Position Joint 3 Position

5 5
[rad]
[rad]
0 0
5 5
0 1 2 3 4 5 0 1 2 3 4 5
[s] [s]
Without redundancy resolution With utilization of redundancy


Analysis of simulation results: time history of the norm of EE position error
Such an effort does not appreciably influences the position tracking

error.
Norm of Positional Error Norm of Positional Error
5e06 2e04
4e06
1.5e04
3e06
[m]
[m]
1e04
2e06
0.5e04
1e06
0 0
0 1 2 3 4 5 0 1 2 3 4 5
[s] [s]
Without redundancy resolution With utilization of redundancy


Kinematic inversion of a 7-DOF industrial manipulator with non-spherical wrist
A different definition of the orientation error must be used.
Simulation scenario
[Caccavale et al., 1996]
// initial configuration
tri = rt_roty(%pi)*rt_rotz(-2/3*%pi);
tpi = rt_transl([.975, -2.194, 1.288]);
ti = tpi*tri;
// final configuration
trf = rt_roty(%pi)*rt_rotz(3/4*%pi);
tpf = rt_transl([.975, -.594, 1.288]);
tf = tpf*trf;
// travelling time 6 sec.
Kinematic structure of the Comau

SMART 3-S robot

Kinematic inversion of a 7-DOF industrial manipulator with non-spherical wrist
RTSS
Tr2NOA
RTSS
Tr2XYZ
Mux +
1
RTSS
Tr2XYZ
Mux
Mux
1/2*( n x nd + o x od + a x ad )
2
RTSS
Tr2NOA
Error block for axis-angle representation of the orientation

2.5e06
Norm of position error (m)
2.0e06
1.5e06
1.0e06
5.0e07
0.0e+00
0 1 2 3 4 5 6
Launch the movie!

3.0e06
Norm of the orientation error (rad)
2.5e06
2.0e06 Comau SMART 3-S performing the

1.5e06
1.0e06
task
5.0e07
0.0e+00
0 1 2 3 4 5 6
Time (s)
Time history of the EE error
Scicos Developments
Outline


Basic concepts
Motion control

Scicos Developments
Blocks under developments
RTSS Applications
0. Second order CLIK
J algorithms;
operational space control;
noname interaction control.
Time-derivative Jacobian block
Scicos Developments
RTSS
RNE+
Applications
interaction control.
noname
Inverse dynamics block plus

environment interaction
Scicos Developments
Unit quaternion blocks;

Applications
other blocks for
Countless.
homogeneous transforms.
Code Generation RTSS/Scicos_RTAI/RTAI-Lab
Outline


Basic concepts
Motion control

Real-Time inverse dynamics controller for the Pelican

Desired reference trajectories in joint space
1.6
joint 1
joint 2
1.4
1.2
Positions 1.0
Sinusoidal term (010 s); 0.8
[rad]
homing traj. (1012 s); 0.6
0.4
sampling time: 5 ms. 0.2
0.0
Velocities and accelerations 0.2

0 2 4 6 8 10 12
Time [s]
By direct differentiation.
Graphs of the reference positions
against time

On the digital implementation of the control system
Digital implementation of the control law amounts to discretizing the

inner and outer control loops.
dk
q
qk
A/D
q dk X k
q
Kv,c
q
X yk X k
k)
B(q D/A MANIPULATOR q
qdk X k
q
Kp,c
(qk , q k )
n

qk
q k A/D
INVERSE DYNAMICS CONTROLLER

Software Hardware
Digital implementation of the inverse dynamics control scheme


Scicos block diagram for simulation
Trajectory generator uses a set of From Workspace blocks from the

standard Sources palette.
To workspace To workspace
pos_err comp_torq
qd ~qk
TRAJECTORY. qd
+
~q.k
q
S/H
qk
.. +
k rt_robot
GENERATOR qd
qk
INVERSE DYNAMICS
S/H .q .qk
.qk CONTROLLER
Pelican S/H
Scicos block diagram for the control system (simulation)


A look inside the Inverse dynamics controller block
Context of the diagram

Coded forms of the PD gains
pelnf = rt_nofriction(pel, coulomb);
Kp,c = diag{1500, 14000} [s2 ] pelp = rt_perturb(pelnf, 0.25);
Kpc = diag([1500, 14000]);
Kv,c = diag{76.21, 353.71} [s1 ] Kvc = diag([76.21, 353.71]);
4
~
qk qk
1 Kpc . k
.
~
qk
qk
yk
rt_rne
P/NF/Pelican 1
2 Kvc
..q
dk
3
Block diagram for the digital controller


Position errors
3e04
joint 1
2e04 joint 2
1e04
[rad]
0e+00
1e04
2e04
3e04
0 2 4 6 8 10 12
Computed torques
10
6
[Nm]
2
0 2 4 6 8 10 12
Time [s]
Graph of position errors and computed torques against the time


Scicos block diagram for real time code generation
Difference between the diagrams for simulation and real time code
generation: trajectory generator and scope blocks.
Scope Scope
POSERR1 TORQUE2
Scope Scope
Demux
POSERR2 TORQUE1
TRAJECTORY.qqdd +

~qk
~q.k
q
S/H
qk
.. +
k rt_robot
GENERATOR qd
qk
INVERSE DYNAMICS
S/H .q .qk
.qk CONTROLLER
Pelican S/H
Scicos block diagram for the control system (real time control)

A look inside the trajectory generator
Trajectory generator uses a set of extdata blocks from the RTAI-Lib

palette.
dats/DDqd2.dat
extdata
dats/DDqd1.dat
extdata
dats/Dqd2.dat
extdata
dats/Dqd1.dat
extdata
dats/qd2.dat
extdata
dats/qd1.dat
extdata
Mux
Mux
Mux
qd
.
qd
..q
d
1
3
Block diagram for the trajectory generator (real time control)

What if the robot arm were physically available?
The mathematical representation of the robot should be substituted by

COMEDI DAC/ADC blocks from RTAI-Lib.
Scope Scope
POSERR1 TORQUE2
Scope Scope
Demux
POSERR2 TORQUE1
TRAJECTORY.qqdd +

~qk
~q.k
COMEDI A/D
qk
.. +
k
GENERATOR qd
qk
INVERSE DYNAMICS
COMEDI D/A .qk
.qk CONTROLLER
COMEDI A/D
Block diagram for real time control with a set of COMEDI blocks

Standalone, hard real time controller generation in two steps (1)
Step 1: Excluding the Clock, construct a super block (SB) out of

the diagram for real time control (RTC).
Actions
1 Use the
Region-to-Super-block
facility to construct the SB;
2 click the RTAI CodeGen button;
3 click on the SB.
RTC diagram after step 1


Standalone, hard real time controller generation in two steps (2)
Step 2: Adjust the properties for code generation by setting

rtss_rtai as Toolchain and press OK.
RTAI CodeGen dialog box


Standalone, hard real time controller generation
The compilation starts and completes. An executable file called

pelicanidc is created.
Compilation output in the Scilab window


Monitoring the real time controller with the QRtaiLab Graphical User Interface
QRtaiLab with the scope manager and the position errors scopes

Monitoring the real time controller with the QRtaiLab Graphical User Interface
QRtaiLab with the scope manager and the computed torques scopes
Summary
Main features of RTSS

Modelling and simulation of robotic manipulators in the
ScicosLab/Scicos environment;
Development of soft/hard real time control systems with the
Scicos-HIL toolbox and the Scicos RTAI Code Generator.
Current and future developments in RTSS

Development of new blocks;
support for robot hands and closed-chain systems;
Further readings about RTSS
General information: http://rtss.sourceforge.net/

Introduction and key features;
software download and licensing information;
support and contributions.
Development reference source:

http://sourceforge.net/apps/mediawiki/rtss/
Roadmap and updates about the status of development;
technical documentation for developers and advanced users;
notes about the compatibility among different Scilab versions.
References
Generic references
P.I. Corke.
A robotics toolbox for MATLAB.
IEEE Robotics and Automation Magazine, vol. 3, pp. 2432,
Mar. 1996.
A. Winkler and J. Such.
Novel joint space force guidance algorithm with laboratory robot
system.
Proceedings of the 16th IFAC World Congress, 2005.
References
Technical Literature of the Pelican arm model treated in this presentation
R. Kelly, V. Santibez and A. Lora.

Control of Robot Manipulators in Joint Space.
Advanced Textbooks in Control and Signal Processing,
Springer-Verlag, London, 2005.
References
Technical Literature of the Manutec r3 robot model treated in this presentation
M. Otter and S. Trk.

The DFVLR Models 1 and 2 of the Manutec r3 Robot.
Technical Report DFVLRMitt.8813, DLR, Institut fr Robotik und
Systemdynamik, Postfach 11 16, D82234 Wessling, May 1988.
J. Franke and M. Otter.
The Manutec r3 Benchmark Models for the Dynamic Simulation of
Robots.
Technical Report TR R101-93, DLR, Institut fr Robotik und
Systemdynamik, Postfach 11 16, D82234 Wessling, March 1993.
References
Technical Literature of the Manutec r3 robot model treated in this presentation
P. Anell.
Modeling of multibody systems in Omola.
Master thesis ISRN LUTFD2/TFRT5516SE, Department of
Automatic Control, Lund Institute of Technology, Lund, Sweden,
Sep 1994.
M. Vukobratovic et al.
Dynamics and Robust Control of Robot-environment Interaction.
World Scientific Publishing Co., Inc., River Edge, NJ, USA, 2009.
D. Katic and M. Vukobratovic.
Intelligent Control of Robotic Systems.
Kluwer Academic Publishers, 2003.
References
Technical Literature of the other models and simulation scenarios presented in this work
F. Caccavale et al.
Experiments of kinematic control on a redundant robot manipulator
with non-spherical wrist.
Laboratory Robotics and Automation, vol. 8, pp. 2536, 1996.
L. Sciavicco and B. Siciliano.
Modelling and Control of Robot Manipulators.
Advanced Textbooks in Control and Signal Processing, London,
UK: Springer-Verlag, 2nd ed., 2000.

RTSS-Morelli PDF

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

RTSS-Morelli PDF

Uploaded by

Copyright:

Available Formats

An open-source package for analysis and control of

serial-link robotic manipulators

25/05/2011, A21, Robotica 1

1 Robotic manipulators modelling with ScicosLab

2 Robot control systems design using Scicos

3 Robot control code generation for use with Linux RTAI

About MRT [Corke, 1996]

Potential applications in research

1 Robotic manipulators modelling with ScicosLab

2 Robot control systems design using Scicos

3 Robot control code generation for use with Linux RTAI

Representing 3D translations and orientations

Point P represented in different

Representing 3D translations and orientations

Representing 3D translations and orientations

Euler angles (ZYZ)

Representing 3D translations and orientations

1 Robotic manipulators modelling with ScicosLab

2 Robot control systems design using Scicos

3 Robot control code generation for use with Linux RTAI

On the robot modelling capabilities of RTSS

RTSS allows to model the typical

Modelling with RTSS

Typical industrial manipulator

On the robot modelling capabilities of RTSS

The robot is modelled as a

Kinematic chain representation

On the robot modelling capabilities of RTSS

The geometry of the robot is

Body frames TX90

On the robot modelling capabilities of RTSS

Geometry kinematics equation

Required model informations

Coordinate transforms TX90

On the robot modelling capabilities of RTSS

Dynamics equations (based on Newton-Euler formulation)

Effective and coupling inertia

Geometric model of a joint-link pair

Standard DH frame assignment

Geometric model of a joint-link pair

Modified DH frame assignment

Dynamic model of a joint-link pair

Inertial parameters (10)

Actuator/transmission model parameters (5)

Dynamic model of a joint-link pair

Modelling a direct-drive planar elbow manipulator

Geometric and kinematic properties

Link Notation Value (m)

Modelling a direct-drive planar elbow manipulator

y0 Joint angle of axis 1 has an offset

Standard DH frame assignment

Modelling a direct-drive planar elbow manipulator

Link i ai i di Building the kinematic model

1 0 l1 10 ? 0 -->L1 = rt_link([0,l1,0,0,0], "standard");

Modelling a direct-drive planar elbow manipulator

q = [ /10 7/25 ] rad

Modelling a direct-drive planar elbow manipulator

Modified DH frame assignment

Modelling a direct-drive planar elbow manipulator

Building the kinematic model: the

Modelling a direct-drive planar elbow manipulator

q = [ /10 7/25 ] rad

Modelling a direct-drive planar elbow manipulator