Contents

SCARA Robot Demo

Demonstrate the the SCARA robot arm. -------------------------------------------------------------------------

%	Reference: Tsai, "Robot Analysis," Example 9.8.2, p. 405.
%--------------------------------------------------------------------------

%--------------------------------------------------------------------------
%   Copyright (c) 1999, 2013 Princeton Satellite Systems, Inc.
%   All rights reserved.
%--------------------------------------------------------------------------

Initialize

%------------

% Simulation time control
%------------------------
tEnd     = 10;
nTest    = 4;
dT       = 0.025;
nSim     = tEnd/dT+1;

% Constants
%----------
gC          = 9.806;

% Robot parameters
%-----------------
a           = [3 2 1];
m           = [4 6 1];

% Assembly information data structure
%------------------------------------
inertia = cell(3);
for k = 1:2
	inertia{k} = m(k)*a(k)^2*diag([0 1 1])/12;
end
inertia{3} = m(3)*a(3)^2*diag([1 1 0])/12;

% Vectors from the hinges to the cm of each body
%-----------------------------------------------
c = zeros(3,3);
r = zeros(3,3);
for k = 1:2
  c(:,k) = [ 0; a(k)/2;0];
  r(:,k) = [ 0; a(k);  0];
end
c(:,3) = [0;0;0];
r(:,3) = [0;0;a(3)/2];

% Transformation matrices
%------------------------
R1 = eye(3);
R2 = eye(3);
R3 = [0 1 0;1 0 0;0 0 -1];

% Sliding friction data
%----------------------
cF          = struct('fStatic', [0;0;0],'kStatic', [1;1;1],...
                     'fCoulomb',[0;0;0],'kCoulomb',[1;1;1],...
                     'bViscous',[0;0;0]);

% The initialization function uses parameter pairs. The ones starting with
% name are links. Entering name creates a new link. All link parameters
% must be specified before the next name
%-------------------------------------------------------------------------
h  = RHSRobotInitialize( 'robot name','Robot',...
            'gravity vector',           [0;0;-9.806],...
            'friction function','FrictionSmooth',...
            'friction data',cF,...
            'name','Link1','mass',4,'radius',0.2,...
            'center of mass',c(:,1),'parent',0,'state',1,... % Link 1
            'link vector',r(:,1),'inertia',inertia{1},'hinge','revolute',...
            'transformation matrix',R1,...
            'name','Link2','mass',6,'radius',0.2,...
            'center of mass',c(:,2),'parent',1,'state',2,... % Link 2
            'link vector',r(:,2),'inertia',inertia{2},'hinge','revolute',...
            'transformation matrix',R2,...
            'name','Link3','mass',1,'radius',0.2,'center of mass',c(:,3),'parent',2,'state',3,... % Link 3
            'link vector',r(:,3),'inertia',inertia{3},'hinge','prismatic',...
            'transformation matrix',R3);

Simulation

%------------
x       = [.4; 1.2; 0.2; 0.3; 0.4; 0.1];

% Run the simulation
%-------------------
t       = dT*(0:(nSim-1));
xPlot	= zeros(length(x),nSim);

for k = 1:nSim

    % Plotting array
    %---------------
    xPlot(:,k)  = x;

    % Enter the motor torques into the dynamics model
    %-----------------------------------------------
    x           = RK4( 'RHSRobot', x, dT, t(k), h );

end

Plot the results

%------------------

% Plot labels
%------------
yL = {'\theta_1 (rad)' '\theta_2 (rad)' 'l (m)' '\omega_1 (rad/s)' '\omega_2 (rad/s)' 'v (m/s)'};

[t,tL] = TimeLabl(t);

Plot2D( t, xPlot, t, yL, 'Manipulator States' );

DrawSCARA( 'initialize' );
DrawSCARA( 'update', [xPlot;zeros(1,nSim)] );

%--------------------------------------
% PSS internal file version information
%--------------------------------------

Published with MATLAB® R2014b