SimulateCBTwolink.m

% SimulateCBTwolink.m uses inverse dynamics to simulate the torque
% trajectories required for a counterbalanced 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:
%
% 'Plant-Limited Co-Design of an Energy-Efficient Counterbalanced Robotic
% Manipulator' Presented at the 2012 ASME iDETC/CIE Conference, August
% 2012, and submitted for publication to the ASME Journal of Mechanical
% Design.
%
% These papers will be available from:
%
% http://systemdesign.illinois.edu/publications.html
%
% The primary file is SimulateCBTwolink.m. Design and simulation parameters
% can be specified in this script. Two other files are required by
% SimulateTwolink.m: fwd_kin.m performs forward kinematic calculations, and
% twolink.m performs an inverse dynamics simulation on the two link
% manipulator using the settings specified in SimulateCBTwolink.m.
%
% 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 the
% 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 paper described above 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, because it requires TOMLAB, a third-party MATLAB toolbox. 
%
% A video illustrating the motion of each of these five cases is available
% on YouTube:
%
% http://www.youtube.com/user/DesignImpact1
%
% Author: James T. Allison, Assistant Professor, University of Illinois at
% Urbana-Champaign
% Date: 7/30/12
% Copyright 2012, James T. Allison

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;

% Case number:
cn = 5;

switch cn
    case 1  % Nominal system design (sequential design, optimal control)
        % Task A:
        p.task = 1;
        % plant design:
        Xplant = [1.0 1.0 0.3 0.3 10 10]';
        % trajectory design:
        Xtraj = [0.3336 0.1918 0.3768 -0.8754]'; 
        % maximum torque requirement:
        p.Tallow = 210;     
    case 2  % Task A optimal co-design
        % Task A:
        p.task = 1;
        % plant design:
        Xplant = [0.838073 0.710543 0.216103 0.885098 3.88609 20.747]';
        % trajectory design:
        Xtraj = [0.35879 0.16298 0.0082864 -0.024898]';
        % maximum torque requirement:
        p.Tallow = 160;
    case 3  % Task B optimal co-design
        % Task A:
        p.task = 2;
        % plant design:
        Xplant = [1.6944 0.94656 2.4576 0.70784 28.205 29.567]';
        % trajectory design:
        Xtraj = [0.49992 1.4571 -0.59256 -0.12468]';
        % maximum torque requirement:
        p.Tallow = 160;
    case 4  % Task B PLCD min E
        % Task A:
        p.task = 2;
        % plant design:
        Xplant = [1.61043 0.710543 0.126474 0.885098 15.6845 19.6277]';
        % trajectory design:
        Xtraj = [0.56461 1.476 -0.59509 -0.17252]';
        % maximum torque requirement:
        p.Tallow = 160;
    case 5  % Task B PLCD min cost 
        % Task B:
        p.task = 2;
        % plant design:
        Xplant = [1.55632 0.710543 0.362572 0.885098 3.88578 20.7462]';
        % trajectory design:
        Xtraj = [0.57551      1.5145    -0.52233    -0.28815]';
        % maximum torque requirement:
        p.Tallow = 160;
end

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

% save simulation data:
if exist('CBTwolinkData','file')
    load CBTwolinkData
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}.Xplant = Xplant;
AD{cn}.xtraj = Xtraj;
AD{cn}.energy = minE;
AD{cn}.torque = torque;
save CBTwolinkData 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)]',Xplant)';
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)')