SimulateTwolink.m

% SimulateTwolink.m uses inverse dynamics to simulate the torque
% trajectories required for a two-link planar robotic manipulator to follow
% a prescribed trajectory. It also computes total energy consumption. This
% code is provided as supplementary material for the paper:
%
% 'Engineering System Co-Design with Limited Plant Redesign'
% Presented at the 8th AIAA Multidisciplinary Design Optimization
% Specialist Conference, April 2012.
%
% The paper is available from:
%
% http://systemdesign.illinois.edu/publications/All12a.pdf
%
% Here both the physical system design and control system design are
% considered simultaneously. Manipulator link length and trajectory
% specification can be specified, and torque trajectory and energy
% consumption are computed based on this input. It was found that maximum
% torque and total energy consumption calculated using inverse dynamics
% agreed closely with results calculated using feedback linearization, so
% to simplify optimization problem solution an inverse dynamics approach
% was used, which reduces the control design vector to just the trajectory
% design. 
%
% In the conference paper several cases are considered, each with its own
% manipulator task, manipulator design, and trajectory design. The
% specifications for each of these five cases are provided here, and can be
% explored by changing the case number variable (cn).
%
% This code was incorporated into a larger optimization project. The code
% presented here includes only the analysis portion of the code, no
% optimization. 
%
% A video illustrating the motion of each of these five cases is available
% on YouTube:
%
% http://www.youtube.com/watch?v=OR7Y9-n5SjA
%
% Author: James T. Allison, Assistant Professor, University of Illinois at
% Urbana-Champaign
% Date: 4/10/12

clear;clc

% simulation parameters:
p.dt = 0.0005;      % simulation step size
tf = 2; p.tf = tf;  % final time
p.ploton = 0;       % turn off additional plotting capabilities
p.ploton2 = 0;
p.Tallow = 210;     % maximum torque requirement

% Case number:
cn = 4;

switch cn
    case 1  % Nominal system design (sequential design, optimal control)
        % Task A:
        p.task = 1;
        % plant design:
        L = [0.6 0.6];
        % trajectory design:
        Xtraj = [0.18333 -0.083583 0.014569 0.14199]';        
    case 2  % Task A optimal co-design
        % Task A:
        p.task = 1;
        % plant design:
        L = [1.7715 1.63455];
        % trajectory design:
        Xtraj = [0.11281 0.120718 0.0502807 -0.43749]';
    case 3  % Task B with Task A optimal design
        % Task A:
        p.task = 2;
        % plant design:
        L = [1.7715 1.63455];
        % trajectory design:
        Xtraj = [0.81030 0.99999 -1.09068 -0.0056275]';
    case 4  % Task B co-design
        % Task A:
        p.task = 2;
        % plant design:
        L = [2.2797 1.13688];
        % trajectory design:
        Xtraj = [0.69136 1.5248 -0.75799 -0.061826]';
    case 5  % Task B PLCD  
        % Task B:
        p.task = 2;
        % plant design:
        L = [1.7715   1.100];
        % trajectory design:
        Xtraj = [0.69891 1.0943 -1.8420 0.310821]';
end

% plant design parameters:
mp = 20;            % payload mass
E = 71.7e9;         % Elastic modulus, Pa
delta_allow = 4e-6; % allowable deflection 
t1 = 0.003;         % link 1 wall thickness (m)
t2 = 0.002;         % link 2 wall thickness (m)
rho = 2810;         % Material density (kg/m^3) 7075 Aluminum
tau1_max = 140;     % Joint 1 nominal torque
tau2_max = 80;      % Joint 2 nominal torque

% intermediate plant calculations (link radius and mass):
l1 = L(1); l2 = L(2);
r1 = ((4*tau1_max*l1^2)/(3*pi*E*delta_allow) + t1^2)/(2*t1);
r2 = ((4*tau2_max*l2^2)/(3*pi*E*delta_allow) + t2^2)/(2*t2);
m1 = l1*rho*pi*(r1^2-(r1-t1)^2);
m2 = l2*rho*pi*(r2^2-(r2-t2)^2);

% extended plant design vector:
Xplant = [l1 l2 m1 m2 mp]';

% perform inverse dynamics calculations:
[minE,t,q1,q2,qd1,qd2,qdd1,qdd2,torque] = twolink(Xtraj,Xplant,p);

% save simulation data:
if exist('TwolinkData','file')
    load TwolinkData
end    
AD{cn}.t = t;
AD{cn}.q1 = q1;
AD{cn}.q2 = q2;
AD{cn}.qd1 = qd1;
AD{cn}.qd2 = qd2;
AD{cn}.qdd1 = qdd1;
AD{cn}.qdd2 = qdd2;
AD{cn}.L = L;
AD{cn}.xtraj = Xtraj;
AD{cn}.energy = minE;
AD{cn}.torque = torque;
save TwolinkData AD

% generate payload workspace trajectory using forward kinematics:
Xws = zeros(length(t),2);
for j=1:length(t)
    Xws(j,:) = fwd_kin([q1(j) q2(j)]',L)';
end

% maximum torque magnitude for each joint for tf(i)
Tmax1 = max(abs(torque(:,1)));
Tmax2 = max(abs(torque(:,2)));

figure(1);clf;figure(2);clf;figure(3);clf;figure(4);clf;figure(5);clf

% plot workspace trajectory
figure(1);
plot(Xws(:,1),Xws(:,2),'k-','LineWidth',2); hold on
title('Workspace Trajectory')

% plot torque trajectories
figure(2);
plot(t,torque(:,1),'k-','LineWidth',2);hold on
title('Torque Trajectory Joint 1')
xlabel('time (s)');ylabel('\tau_1 (N-m)')
figure(3);
plot(t,torque(:,2),'k-','LineWidth',2);hold on
title('Torque Trajectory Joint 2')
xlabel('time (s)');ylabel('\tau_2 (N-m)')

% plot torque/speed curves
figure(4) 
plot(qd1,torque(:,1),'k-','LineWidth',2); hold on
plot(qd1(1),torque(1,1),'ko','MarkerSize',10,...
    'MarkerFaceColor',[.49 1 .63]); 
figure(5)
plot(qd2,torque(:,2),'k-','LineWidth',2); hold on
plot(qd2(1),torque(1,2),'ko','MarkerSize',10,...
    'MarkerFaceColor',[.49 1 .63]);

figure(4);
title('Torque-Speed Curve for Joint 1')
xlabel('speed (rad/s)')
ylabel('torque (Nm)')
figure(5);
title('Torque-Speed Curve for Joint 2')
xlabel('speed (rad/s)')
ylabel('torque (Nm)')