PROPT Two-Link Robot
From TomWiki
Jump to navigationJump to search
This page is part of the PROPT Manual. See PROPT Manual. |
Problem description
Singular time-optimal 2 Link robot control
From the paper: L.G. Van Willigenburg, 1991, Computation of time-optimal controls applied to rigid manipulators with friction, Int. J. Contr., Vol. 54, no 5, pp. 1097-1117
Programmers: Gerard Van Willigenburg (Wageningen University) Willem De Koning (retired from Delft University of Technology)
% Copyright (c) 2009-2009 by Tomlab Optimization Inc.
Problem setup
% Array with consecutive number of collocation points
narr = [20 40];
toms t t_f % Free final time
for n=narr
p = tomPhase('p', t, 0, t_f, n);
setPhase(p)
tomStates x1 x2 x3 x4
tomControls u1 u2
% Initial & terminal states
xi = [0; 0; 0; 0];
xf = [1.5; 0; 0; 0];
% Initial guess
if n==narr(1)
x0 = {t_f==1; icollocate({x1 == xf(1); x2 == xf(2)
x3 == xf(3); x4 == xf(4)})
collocate({u1 == 0; u2 == 0})};
else
x0 = {t_f==tfopt; icollocate({x1 == xopt1; x2 == xopt2
x3 == xopt3; x4 == xopt4})
collocate({u1 == uopt1; u2 == uopt2})};
end
% Box constraints
cbox = {0.75 <= t_f <= 1.5; -25 <= collocate(u1) <= 25
-9 <= collocate(u2) <= 9};
% Boundary constraints
cbnd = {initial({x1 == xi(1); x2 == xi(2)
x3 == xi(3); x4 == xi(4)})
final({x1 == xf(1); x2 == xf(2)
x3 == xf(3); x4 == xf(4)})};
% ODEs and path constraints
% Robot parameters
mm11 = 5.775; mm12 = 0.815; mm22 = 0.815;
hm11 = 1.35; m1 = 30.0; m2 = 15;
% Variables for dynamics
c1 = cos(x1); c2 = cos(x2);
s2 = sin(x2); c12 = cos(x1+x2);
ms1 = mm11+2*hm11*c2; ms2 = mm12+hm11*c2;
mdet = ms1.*mm22-ms2.*ms2;
ms11 = mm22./mdet; ms12=-ms2./mdet; ms22=ms1./mdet;
qg1 = -hm11*s2.*(x4.*x4+2*x3.*x4);
qg2 = hm11*s2.*x3.*x3;
dx1 = x3; dx2=x4;
dx3 = ms11.*(u1-qg1)+ms12.*(u2-qg2);
dx4 = ms12.*(u1-qg1)+ms22.*(u2-qg2);
ceq = collocate({
dot(x1) == dx1
dot(x2) == dx2
dot(x3) == dx3
dot(x4) == dx4});
% Objective
objective = t_f;
Solve the problem
options = struct;
options.name = '2-Link-Robot';
solution = ezsolve(objective, {cbox, cbnd, ceq}, x0, options);
tfopt = subs(t_f,solution);
xopt1 = subs(x1,solution);
xopt2 = subs(x2,solution);
xopt3 = subs(x3,solution);
xopt4 = subs(x4,solution);
uopt1 = subs(u1,solution);
uopt2 = subs(u2,solution);
Problem type appears to be: lpcon Time for symbolic processing: 0.56603 seconds Starting numeric solver ===== * * * =================================================================== * * * TOMLAB - TOMLAB Development license 999007. Valid to 2011-12-31 ===================================================================================== Problem: --- 1: 2-Link-Robot f_k 1.225664454232960000 sum(|constr|) 0.000001175408507726 f(x_k) + sum(|constr|) 1.225665629641467600 f(x_0) 1.000000000000000000 Solver: snopt. EXIT=0. INFORM=1. SNOPT 7.2-5 NLP code Optimality conditions satisfied FuncEv 1 ConstrEv 2651 ConJacEv 2651 Iter 592 MinorIter 5437 CPU time: 2.293215 sec. Elapsed time: 2.297000 sec.
Problem type appears to be: lpcon Time for symbolic processing: 0.55817 seconds Starting numeric solver ===== * * * =================================================================== * * * TOMLAB - TOMLAB Development license 999007. Valid to 2011-12-31 ===================================================================================== Problem: --- 1: 2-Link-Robot f_k 1.223303478413072100 sum(|constr|) 0.000000031553586574 f(x_k) + sum(|constr|) 1.223303509966658700 f(x_0) 1.225664454232960000 Solver: snopt. EXIT=0. INFORM=1. SNOPT 7.2-5 NLP code Optimality conditions satisfied FuncEv 1 ConstrEv 44 ConJacEv 44 Iter 16 MinorIter 360 CPU time: 0.124801 sec. Elapsed time: 0.122000 sec.
end
figure(1)
subplot(2,1,1);
ezplot([x1; x2; x3; x4]); legend('x1','x2','x3','x4');
title('Robot states');
subplot(2,1,2);
ezplot([u1; u2]); legend('u1','u2');
title('Robot controls');