Two-Link Robot

Contents

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 tf % Free final time

for n=narr

    p = tomPhase('p', t, 0, tf, 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 = {tf==1; icollocate({x1 == xi(1); x2 == xi(2)
            x3 == xi(3); x4 == xi(4)})
            collocate({u1 == 0; u2 == 0})};
    else
        x0 = {tf==tfopt; icollocate({x1 == xopt1; x2 == xopt2
            x3 == xopt3; x4 == xopt4})
            collocate({u1 == uopt1; u2 == uopt2})};
    end

    % Box constraints
    cbox = {0.75 <= tf <= 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;

    det  = ms1.*mm22-ms2.*ms2;
    ms11 = mm22./det; ms12=-ms2./det; ms22=ms1./det;

    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 = tf;

Solve the problem

    options = struct;
    options.name = '2-Link-Robot';
    solution = ezsolve(objective, {cbox, cbnd, ceq}, x0, options);

    tfopt = subs(tf,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
===== * * * =================================================================== * * *
TOMLAB - Tomlab Optimization Inc. Development license  999001. Valid to 2010-02-05
=====================================================================================
Problem: ---  1: 2-Link-Robot                   f_k       1.225664454423204000
                                       sum(|constr|)      0.000001093433115014
                              f(x_k) + sum(|constr|)      1.225665547856319000
                                              f(x_0)      1.000000000000000000

Solver: snopt.  EXIT=0.  INFORM=1.
SNOPT 7.2-5 NLP code
Optimality conditions satisfied

FuncEv    1 ConstrEv 2685 ConJacEv 2685 Iter  599 MinorIter 5285
CPU time: 6.593750 sec. Elapsed time: 6.625000 sec. 

Problem type appears to be: lpcon
===== * * * =================================================================== * * *
TOMLAB - Tomlab Optimization Inc. Development license  999001. Valid to 2010-02-05
=====================================================================================
Problem: ---  1: 2-Link-Robot                   f_k       1.223303492296659200
                                       sum(|constr|)      0.000000649417532893
                              f(x_k) + sum(|constr|)      1.223304141714192100
                                              f(x_0)      1.225664454423204000

Solver: snopt.  EXIT=0.  INFORM=1.
SNOPT 7.2-5 NLP code
Optimality conditions satisfied

FuncEv    1 ConstrEv   37 ConJacEv   37 Iter   13 MinorIter  321
CPU time: 0.296875 sec. Elapsed time: 0.297000 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');