Problem

The robot has two rotational joints and one linear “joint.” You need to write the dynamics that link the forces and torques applied to the arm to its motion so that you can simulate the arm.

Solution

The equations of motion are written as second order differential equations. The right hand side involves solving a set of linear equations.

How It Works

The SCARA robot is shown in Figure 7-1. It has two arms that move in the xy-plane and a plunger that moves in the z direction. The angles θ ₁ and θ ₂ are measured around the z ₀ and z ₁ axes.

Figure 7-1. SCARA robot. The two arms move in a plane. The plunger moves perpendicular to the plane

The equations of motion for the SCARA robot are (7.1)

The first term is the product of the generalized inertia matrix and the acceleration vector. The second array contains the rotational coupling terms. The final array is the control vector. The generalized inertia matrix, I, is
(7.2)
where (7.3)
(7.4)
(7.5)
(7.6)

Note that in developing this inertia matrix, the author is treating the links as point masses and not solid bodies.

The matrix is symmetric, as it should be. There is coupling between the two rotational degrees of freedom but no coupling between the plunger and the rotational hinges.

First, define a data structure for the robot, defining the length and mass of the links, and with fields for the forces and torques that can be applied. The function can supply a default structure to be filled in, or the fields can be specified a priori.

% d = SCARADataStructure

% d = SCARADataStructure( a1, a2, d1, m1, m2, m3 )

%

%% Description

% Create a SCARA robot data structure . Type d = SCARADataStructure for

% default arguments. The forces and torques are set to zero.

%

%% Inputs

% a1 (1,1) Link 1 length

% a2 (1,1) Link 2 length

% d1 (1,1) Distance of link 1 from ground

% m1 (1,1) Link 1 mass

% m2 (1,1) Link 2 mass

% m3 (1,1) Link 3 mass

% t1 (1,1) Joint 1 torque

% t2 (1,1) Joint 2 torque

% f3 (1,1) Joint 3 force

%

%% Outputs

% d (.) Data structure

function d = SCARADataStructure( a1, a2, d1, m1, m2, m3 )

if ( nargin < 1 )

d = struct ('a1',0.1,'a2',0.1,'d1',0.05,'m1',1,'m2',1,'m3',1,'t1',0,'t2',0,'f3',0);

else

d = struct ('a1',a1,'a2',a2,'d1',d1,'m1',m1,'m2',m2,'m3',m3,'t1',0,'t2',0,'f3',0);

end

Then write the right-hand-side (RHS) function from the equations. You need to solve for the state derivatives , which you do with a left matrix divide. This is easily done in MATLAB with a backslash, which uses a QR, triangular, LDL, Cholesky, Hessenberg, or LU solver, as appropriate for the inputs. The function does not have a built-in demo, as this impacts performance in RHS functions, which are called repeatedly by integrators. Note the definition of the constant for gravity at the top of the file. The inertia matrix is returned as an additional output.

%% RHSSCARA Right hand side of the SCARA robot arm equations.

%

%% Form

% [xDot, i] = RHSSCARA( t, x, d )

%

%% Description

% Generates an acceleration vector given the current state, time, and data

% structure describing the SCARA robot's configuration.

% function [xDot, i] = RHSSCARA( t, x, d )

%

%% Description

% Generates an acceleration vector.

%

%% Inputs

% t (1,1) Time (s)

% x (6,:) State vector [theta1;theta2;d3;omega1;omega2;v3]

% d (.) SCARA data structure

%

%% Outputs

% xDot (6,:) State derivative

% i (3,3) Generalized inertia matrix

%

%% See also

% SCARADataStructure

function [xDot, i] = RHSSCARA( ∼, x, d )

g = 9.806; % The acceleration of gravity ( m/sˆ2 )

c2 = cos (x(2));

s2 = sin (x(2));

theta1Dot = x(4);

theta2Dot = x(5);

% Inertia matrix

i = zeros (3,3);

a1Sq = d.a1ˆ2;

a2Sq = d.a2ˆ2;

a12 = d.a1*d.a2;

m23 = 0.5*d.m2 + d.m3;

i(1,1) = (d.m1/3 + d.m2 + d.m3)*a1Sq + 0.5*m23*a12*c2 + (d.m2/3 + d.m3)*a2Sq;

i(2,2) = (d.m2/3 + d.m3)*a2Sq;

i(3,3) = d.m3;

i(1,2) = m23*a12*c2 + (d.m2/3 + d.m3);

i(2,1) = i(1,2);

% Right hand side

u = [d.t1;d.t2;d.f3];

f = [−(d.m2 + 2*d.m3)*a12*s2*(theta1Dot*theta2Dot + 0.5*theta2Dotˆ2);...

0.5*m23*a12*s2*theta1Dotˆ2;...

−d.m3*g];

xDot = [x(4:6);i(f − u)];

Problem

You would like to be able to visualize the motion of the robot arm in three dimensions.

Solution

You will write a function to draw a 3D SCARA robot arm. This will allow you to easily visualize what is happening with the robot arm.

How It Works

This function demonstrates the use of the low-level plotting functions patch and light. You create box and cylinder shapes for the components of the robot arm. This function also demonstrates how to produce MATLAB movies of the robot motion using getframe. The resulting visualization is shown in Figure 7-2.

Figure 7-2. SCARA robot visualization using patch

This function’s first argument is an action. You define an initialization action to generate all the patch objects, which are stored in a persistent variable. Then, during the update action, you only need to update the patch vertices. The function has one output, which is movie frames from the animation. Note that the input x is vectorized, meaning you can pass a set of states to the function and not just one at a time. The following is the header of the function.

%% DRAWSCARA Draw a SCARA robot arm.

%

%% Forms

% DrawSCARA( ' initialize ' , d )

% m = DrawSCARA( ' update ' , x )

%

%% Description

% Draws a SCARA robot using patch objects. A persistent variable is used to

% store the graphics handles in between update calls.

%

% The SCARA acronym stands for Selective Compliance Assembly Robot Arm

% or Selective Compliance Articulated Robot Arm.

%

% Type DrawSCARA for a demo.

%

%% Inputs

% action (1,:) Action string

% x (3,:) [theta1;theta2;d3]

% or

% d (1,1) Data structure for dimensions

% .a1 (1,1) Link arm 1 joint to joint

% .a2 (1,1) Link arm 2 joint to joint

% .d1 (1,1) Height of link 1 and link2

%

%% Outputs

% m (1,:) If there is an output it makes a movie using getframe

Next, show the body of the main function. Note that the function has a built-in demo demonstrating a vector input with 100 states. The function initializes itself with default data if the data structure d is omitted.

function m = DrawSCARA( action, x )

persistent p

% Demo

%−−−−−

if ( nargin < 1 )

DrawSCARA( 'initialize' );

t = linspace (0,100);

omega1 = 0.1;

omega2 = 0.2;

omega3 = 0.3;

x = [ sin (omega1*t); sin (omega2*t);0.01* sin (omega3*t)];

m = DrawSCARA( 'update', x );

if ( nargout < 1 )

clear m;

end

return

end

switch ( lower (action) )

case 'initialize'

if ( nargin < 2 )

d = SCARADataStructure;

else

d = x;

end

p = Initialize( d );

case 'update'

if ( nargout == 1 )

m = Update( p, x );

else

Update( p, x );

end

end

Note that subfunctions were used for the Initialize and Update actions. This keeps the switch statement clean and easy to read. In the Initialize function, you define additional parameters for creating the box and cylinder objects you use to visualize the entire robot arm. Then you use patch to create the graphics objects, using parameter pairs instead of the (x,y,z) input. Specifying the unique vertices this way can reduce the size of the data needed to define the patch, and it is simple conceptually. See the “Introduction to Patch Objects” and the “Specifying Patch Object Shapes” sections of MATLAB help for more information. The handles are stored in the persistent variable p.

%−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

% Initialize the picture. The robot is defined using these parameters:

% .b (1,3) Box dimensions [x y z] See Box

% .l1 (1,5) Link arm 1 dimensions [x y z t d] See UChannel

% .l2 (1,3) Link arm 2 dimensions [x y z] See Box

% .c1 (1,2) Cylinder 1 [r l]

% .c2 (1,2) Cylinder 2 [r l]

% .c3 (1,2) Cylinder 3 [r l]

% .c4 (1,2) Cylinder 4 [r l]

%−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

function p = Initialize( d )

p.fig = figure ( 'name','SCARA' );

% Create parts

c = [0.5 0.5 0.5]; % Color

r = [1.0 0.0 0.0];

% Store for use in updating

p.a1 = d.a1;

p.a2 = d.a2;

% Physical parameters for drawing

d.b = [1 1 1]*d.d1/2;

d.l1 = [0.12 0.02 0.02 0.005 0.03]*10*d.a1;

d.l2 = [0.12 0.02 0.01]*10*d.a2;

d.c1 = [0.1 0.4]*d.a1;

d.c2 = [0.06 0.3]*d.a1;

d.c3 = [0.06 0.5]*d.a2;

d.c4 = [0.05 0.6]*d.a2;

% Base

[vB, fB] = Box( d.b(1), d.b(2), d.b(3) );

[vC, fC] = Cylinder( d.c1(1), d.c1(2) );

F = [fB;fC+ size (vB,1)];

vB(:,3) = vB(:,3) + d.b(3)/2;

vC(:,3) = vC(:,3) + d.b(3);

v = [vB;vC];

p.base = patch ('vertices', v, 'faces', f,...

'facecolor', c, 'edgecolor', c,...

'facelighting', 'phong' );

% Link 1

% Arm

[vA, fA] = UChannel( d.l1(1), d.l1(2), d.l1(3), d.l1(4), d.l1(5) );

vA(:,3) = vA(:,3) + d.d1;

vA(:,1) = vA(:,1) − d.b(1)/2;

% Pin

[vC, fC] = Cylinder( d.c2(1), d.c2(2) );

vC(:,3) = vC(:,3) + d.d1 − d.c2(2)/2;

vC(:,1) = vC(:,1) + d.a1 − d.c2(1);

p.v1 = [vC;vA];

f = [fC;fA+ size (vC,1)];

p.link1 = patch ('vertices', p.v1, 'faces', f,...

'facecolor', r, 'edgecolor', r,...

'facelighting', 'phong' );

% Find the limit for the axes

zLim = max (vC(:,3));

% Link 2

[vB, fB] = Box(d.l2(1), d.l2(2), d.l2(3) );

[vC, fC] = Cylinder( d.c3(1), d.c3(2) );

vC(:,1) = vC(:,1) + d.l2(1)/2 − 2*d.c3(1);

vC(:,3) = vC(:,3) − d.c3(2)/2;

p.v2 = [vC;vB];

p.v2(:,1) = p.v2(:,1) + d.l2(1)/2 − 2*d.c3(1);

p.v2(:,3) = p.v2(:,3) + d.d1;

f = [fC;fB+ size (vC,1)];

v2 = p.v2;

v2(:,1) = v2(:,1) + d.a1;

p.link2 = patch ('vertices', v2, 'faces', f,...

'facecolor', r, 'edgecolor', r,...

'facelighting', 'phong' );

% Link 3

[vC, fC] = Cylinder( d.c4(1), d.c4(2) );

p.v3 = vC;

p.r3 = d.l2(1) − 4*d.c3(1);

p.v3(:,3) = p.v3(:,3) + d.d1/4;

vC(:,1) = vC(:,1) + p.r3 + d.a1;

f = fC;

p.link3 = patch ('vertices', vC, 'faces', f,...

'facecolor', c, 'edgecolor', c,...

'facelighting', 'phong' );

xLim = 1.3*(d.a1+d.a2);

xlabel ('x');

ylabel ('y');

zlabel ('z');

grid on

rotate3d on

axis ([−xLim xLim − xLim xLim 0 zLim])

set ( gca ,'DataAspectRatio',[1 1 1],'DataAspectRatioMode','manual')

s = 10* max (Mag(v'));

light('position',s*[1 1 1])

view ([1 1 1])

The Update function updates the vertices for each patch object. The nominal vertices are stored in the persistent variable p and are rotated using a transformation matrix calculated from the sine and cosine of the link angles. If there is an output argument, the function uses getframe to grab the figure as a movie frame.

%−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

% Update the picture and get the frame if requested

%−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

function m = Update( p, x )

for k = 1: size (x,2)

% Link 1

c = cos (x(1,k));

s = sin (x(1,k));

b1 = [c − s 0;s c 0;0 0 1];

v = (b1*p.v1')';

set (p.link1,'vertices',v);

% Link 2

r2 = b1*[p.a1;0;0];

c = cos (x(2,k));

s = sin (x(2,k));

b2 = [c − s 0;s c 0;0 0 1];

v = (b2*b1*p.v2')';

v(:,1) = v(:,1) + r2(1);

v(:,2) = v(:,2) + r2(2);

set (p.link2,'vertices',v);

% Link 3

r3 = b2*b1*[p.r3;0;0] + r2;

v = p.v3;

v(:,1) = v(:,1) + r3(1);

v(:,2) = v(:,2) + r3(2);

v(:,3) = v(:,3) + x(3,k);

set (p.link3,'vertices',v);

if( nargout > 0 )

m(k) = getframe (p.fig);

else

drawnow;

end

end

The subfunctions Box, Cylinder, and UChannel create the vertices and faces for each type of 3D object. Faces are defined using indices of the vertices; in this case, triangles. The Box function is shown here to demonstrate how the vertices and faces matrices are created.

%−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

% Inputs

% −−−−−−

% Box

% x (1,1) x length

% y (1,1) y length

% z (1,1) z length

%

% Outputs

% −−−−−−−

% v (:,3) Vertices

% f (:,3) Faces

%−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

function [v, f] = Box( x, y, z )

f = [2 3 6;3 7 6;3 4 8;3 8 7;4 5 8;4 1 5;2 6 5;2 5 1;1 3 2;1 4 3;5 6 7;5 7 8];

x = x/2;

y = y/2;

z = z/2;

v = [−x x x −x −x x x −x;...

−y −y y y −y −y y y;...

−z −z −z −z z z z z]';

Problem

The goal of the robot controller is to place the end effector at a desired position. You need to know the link states corresponding to this position.

Solution

You will use a numerical solver to compute the robot states. The MATLAB solver is fminsearch, which implements a Nelder − Mead minimizer .

How It Works

The goal of our control system is to position the end effector and a desired position[x,y,z]. z is determined by d ₁−d ₃ from Figure 7-1 in Recipe 7-1. x and y are found from the two angles, a ₁ and a ₂. The position vector for the arm end effector is
(7.7)

While these equations don’t seem complicated, they can’t be used to solve for x and y directly. First of all, if a ₂ is less than a ₁ there will be a region around the origin that cannot be reached. In addition, there may be more than one solution for each x, y pair.

You use this equation for the position to create a cost function that can be passed to fminsearch. This computes the state, which results in the desired position. The resulting function demonstrates a nested cost function, a built-in demo, and a default plot output. Note that you use a data structure as returned by optimset to pass parameters to fminsearch.

%% SCARAIK Generate SCARA states for desired end effector position and angle.

%

%% Form:

% x = SCARAIK( r, d )

%

%% Description

% Uses fminsearch to find the link states given the effector location. The

% cost function is embedded. Type SCARAIK for a demo which creates a plot

% and a video.

%

%% Inputs

% r (3,:) End effector position [x;y;z]

% d (.) Robot data structure

% .a1 (1,1) Link 1 length

% .a2 (1,1) Link 2 length

% .d1 (1,1) Distance of link 1 from ground

%

%% Outputs

% x (3,:) SCARA states [theta1;theta2;d3]

function x = SCARAIK( r, d )

% Demo

% -----

if ( nargin < 1 )

r = [ linspace (0,0.2); zeros (2,100)];

d = SCARADataStructure;

SCARAIK( r, d );

return;

end

n = size (r,2);

xY = zeros (2,n);

TolX = 1e − 5;

TolFun = 1e − 9;

MaxFunEvals = 1500;

Options = optimset ('TolX',TolX,'TolFun',TolFun,'MaxFunEvals',MaxFunEvals);

x0 = [0;0];

for k = 1:n

d.xT = r(1:2,k);

xY(:,k) = fminsearch (@Cost, x0, Options, d );

x0 = xY(:,k);

end

x = [xY;d.d1 − r(3,:)];

% Default output is to create a plot

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

if ( nargout == 0 )

DrawSCARA( 'initialize', d );

m = DrawSCARA( 'update', x );

vidObj = VideoWriter('SCARAIK.avi');

open(vidObj);

writeVideo(vidObj,m);

end

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

% Cost function

% The cost is the difference between the position as computed from the

% states and the target position xT.

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

function y = Cost( x, d )

xE = d.a1* cos (x(1)) + d.a2* cos (x(1)+x(2));

yE = d.a1* sin (x(1)) + d.a2* sin (x(1)+x(2));

y = sqrt ((xE-d.xT(1))ˆ2+(yE-d.xT(2))ˆ2);

The function creates a video using a VideoWriter object and the frame data returned by DrawSCARA. Before VideoWriter was introduced, this could be done with movie2avi.

Problem

Robot arm control is a critical technology in robotics. You need to be able to smoothly and reliably change the location of the end effector. The speed of the control determines the number of operations that you can do in a given amount of time, thus determining the productivity of the robot.

Solution

This problem is solved using the inverse kinematics function discussed earlier, and then feed the desired angles into two PD controllers, as developed in Chapter 6.

How It Works

Apply the PD controller described in Chapter 6 using the c array as the desired angle and position vector. You compute control accelerations, not torques, and then multiply by the inertia matrix to get control torques,
(7.8)
where T is the control torque, I is the inertia matrix, and a is the computed control acceleration. You need to do this because there are cross-coupling terms in the inertia matrix, and I ₁₁ changes as the position of the outer arm changes. You neglect the nonlinear terms in the equations of motion. These terms are functions of the angles and the angular rates. If you move slowly, this should not pose a problem. If you move quickly, you could feedforward the nonlinear torques and cancel them.

The first step is to specify a desired position for the end effector and use the inverse kinematics function to compute the target states corresponding to this location.

% Pick the location to place the end effector, [x;y;z]

r = [4;2;0];

% Find the two angles for the joints

setPoint = SCARAIK( r, d );

Next is the code that designs the controllers, one for each joint, using PDControl. Note that identical parameters are used for both controllers. Set the damping ratio, zeta, to 1.0 to avoid overshoot. Recall that wN, the undamped natural frequency, is the bandwidth of the controller; the higher this frequency, the faster the response. wD, the derivative term filter cutoff, is set to 5–10 times wN, so that filter doesn’t cause lag below wN. The dT variable is the timestep of the simulation.

%% Control Design

% We will use two PD controllers, one for each rotational joint.

% Controller parameters

dC1 = PDControl( 'struct' );

dC1.zeta = 1.0;

dC1.wN = 0.6;

dC1.wD = 60.0;

dC1.tSamp = dT;

dC2 = dC1;

% Create the two controllers

dC1 = PDControl( 'initialize', dC1 );

dC2 = PDControl( 'initialize', dC2 );

This is the portion that computes and applies the control. You eliminate the inertia coupling by computing joint accelerations and multiplying by the inertia matrix, which is computed each time step, to get the desired control torques. Note that you use the feature of the RHS that computes the inertia matrix from the current state.

[acc(1,1), dC1] = PDControl('update',thetaError(1),dC1);

[acc(2,1), dC2] = PDControl('update',thetaError(2),dC2);

torque = inertia(1:2,1:2)*acc;

You can run these lines at the command line to see what the acceleration and torque magnitude looks like for an example robot. Note that, assuming MKS units, you have links of 1 meter in length and masses of 1 kg.

>> dC1 = PDControl( 'struct' );

>> dC1.zeta = 1.0;

>> dC1.wN = 0.6;

>> dC1.wD = 60.0;

>> dC1.tSamp = 0.025;

>> dC2 = dC1;

>> dC1 = PDControl( 'initialize', dC1 );

>> dC2 = PDControl( 'initialize', dC2 );

>> d = SCARADataStructure(1,1,1,1,1,1);

>> x = zeros (6,1);

>> [˜,inertia] = RHSSCARA( 0, x, d );

>> inertia

inertia =

4.4167 2.8333 0

2.8333 1.3333 0

0 0 1

>> thetaError = [0.1;0.1];

>> [acc(1,1), dC1] = PDControl('update',thetaError(1),dC1);

>> [acc(2,1), dC2] = PDControl('update',thetaError(2),dC2);

>> acc

acc =

-7.236

-7.236

>> torque = inertia(1:2,1:2)*acc

torque =

-52.461

-30.15

Problem

You want to test the robot arm under control. The input will be the desired xy coordinates of the end effector.

Solution

The solution is to build a MATLAB script in which you design the PD controller matrices as you did earlier, and then simulate the controller in a loop, applying the calculated torques until the states match the desired angles. You will not control the vertical position of the end effector in this recipe.

How It Works

This is a discrete simulation, with a fixed-time step and the control torque calculated separately from the dynamics. The simulation runs in a loop, calling first the controller code from Recipe 7-4 and the right-hand side from the fourth-order Runge-Kutta function. When the simulation ends, the angles and angle errors are plotted and a 3D animation is displayed. You could plot more variables, but all the essential information is in the angles and errors.

With a very small time step of 0.025 seconds, you could have increased the bandwidth of the controller to speed the response. Remember that the cutoff frequency of the filter must also be below the Nyquist frequency. Any signals above the Nyquist Frequency will be observed at an alias of their true frequency, not their true frequency.

Notice that you do not handle large angle errors; that is, errors greater than 2π. In addition, if the desired angle is 2π − e and the current position is 2π + e, it will not necessarily go the shortest way. This can be handled by adding code that computes the smallest error between two points on the unit circle. The reader can add code for this to make the controller more robust.

The script SCARARobotSim.m follows, skipping the control design lines from Recipe 7-4. First, you initialize the simulation data, including the time parameters and the robot geometry. You initialize the plotting arrays using zeros before entering the simulation loop. There is a control flag that allows the simulation to be run open loop or closed loop. The integration occurs in the last line of the loop.

%% Initialize

% Specify the time, robot geometry and the control target.

% Simulation time settings

tEnd = 20.0; % sec

dT = 0.025;

nSim = tEnd/dT+1;

controlIsOn = true;

% Robot parameters

d = SCARADataStructure(3,2,1,4,6,1);

% Set the initial arm states

x0 = zeros (6,1);

%x0(5) = 0.05;

% Pick the location to place the end effector, [x;y;z]

r = [4;2;0];

% Find the two angles for the joints

setPoint = SCARAIK( r, d );

%% Simulation

% The simulation can be run with or without control, i.e. closed or open

% loop.

x = x0;

xPlot = zeros (4,nSim);

tqPlot = zeros (2,nSim);

inrPlot = zeros (2,nSim);

for k = 1:nSim

% Control error

thetaError = setPoint(1:2) - x(1:2);

[˜,inertia] = RHSSCARA( 0, x, d );

acc = zeros (2,1);

% Apply the control

if ( controlIsOn )

[acc(1,1), dC1] = PDControl('update',thetaError(1),dC1);

[acc(2,1), dC2] = PDControl('update',thetaError(2),dC2);

torque = inertia(1:2,1:2)*acc;

else

torque = zeros (2,1);

end

d.t1 = torque(1);

d.t2 = torque(2);

% Plotting array

xPlot(:,k) = [x(1:2);thetaError];

tqPlot(:,k) = torque;

inrPlot(:,k) = [inertia(1,1);inertia(2,2)];

% Enter the motor torques into the dynamics model

x = RungeKutta( @RHSSCARA, 0, x, dT, d );

end

%% Plot the results

% Plot a time history and perform an animation.

% Plot labels

yL = {' heta_1⊔(rad)' ' heta_2⊔(rad)' 'Error⊔ heta_1⊔(rad)' 'Error⊔ heta_2⊔(rad)'};

% Time histories

[t,tL] = TimeLabel(dT*(0:(nSim-1)));

PlotSet( t, xPlot, 'y⊔label', yL, 'x⊔label', tL );

PlotSet( t, tqPlot, 'y⊔label', {'T_x','T_y'}, 'x⊔label', tL );

PlotSet( t, inrPlot, 'y⊔label', {'I_{11}','I_{22}'}, 'x⊔label', tL );

% Animation

DrawSCARA( 'initialize', d );

Figure 7-3 shows the transient response of the two joints. Both converge to their set points, but look different than the double integrator response that you saw in the previous chapter. This system is nonlinear due to the coupling between the links. For instance, in a double integrator, you would expect no overshoot of the target angle for a damping ratio of 1.0. However, you do see some in the second subplot, of θ ₂, and otherwise the shape is similar to a double integrator response. You see that θ ₁, in contrast, reverses direction as it reacts to the motion of the outer joint; after about 2 seconds, when the θ ₂ has peaked, θ ₁ also resembles a double integrator. Keep in mind that the two controllers are independent and are working at cross-purposes in some ways.

Figure 7-3. SCARA robot angles showing the transient response

Figure 7-4 shows the resulting inertia components. I ₁₁ was expected to change and I ₂₂ to remain constant, which is, in fact, the case.

Figure 7-4. SCARA robot inertia as the arm moves

After the simulation is done, the script runs an animation of the arm motion. Both 2D plots and the 3D animation are needed to debug the controller and for production runs.

The same script could be extended to show a sequence of commands to the arm.