« Previous « Start » Next »

35  MK2 5-Link robot

Singular time-optimal of the MK2 5-Link robot. Implementation without mass matrix inversion.

35.1  Problem description

The dynamic model of the MK2 robot was generated automatically by AUTOLEV that produces Fortran 77 code:

http://www.autolev.com/

The transfer to matlab code was performed partly automatically using

 1) to_f90: http://users.bigpond.net.au/amiller/
 2) f2matlab.m: http://www.mathworks.com/matlabcentral/fileexchange/5260

Programmer: Gerard Van Willigenburg (Wageningen University)

35.2  Problem setup

toms t t_f % Free final time

p = tomPhase('p', t, 0, t_f, 20);
setPhase(p);

global AP4AD
AP4AD = true; % Work-around to get more efficient code for this particular case.

% Dimension state and control vector
np = 5; nx = 2*np; nu = np;

% Define the state and control vector
tomStates a1 a2 a3 a4 a5 w1 w2 w3 w4 w5
phi = [a1; a2; a3; a4; a5];
omega = [w1; w2; w3; w4; w5];
tomControls u1 u2 u3 u4 u5
u     = [u1; u2; u3; u4; u5];

% Initial and terminal states
znp    = zeros(np,1);
phif = [0.975; 0.975; 0; 0; 0.975];

% Maximum values controls
umax = [15; 10; 5; 5; 5];

% Initial guess
x0 = {t_f==0.8;
    icollocate({phi == phif; omega == znp})
    collocate({u == 0})};

% Box constraints
cbox = {0.7 <= t_f <= 0.9;
    collocate({-umax <= u <= umax})};

% Boundary constraints
cbnd = {initial({phi == znp; omega == znp})
    final({phi == phif; omega == znp})};

% Compute mass matrix
[mass, rhs] = fiveLinkMK2Robotdyn([phi; omega], u);

% Equality differential equation constraints
ceq = collocate({dot(phi) == omega; mass*dot(omega) == rhs});

% Objective
objective = t_f;

35.3  Solve the problem

options = struct;
options.use_d2c = 0;
options.use_H   = 0;
options.type = 'lpcon';
options.name = 'Five Link MK2 Robot';
options.derivatives = 'automatic';
solution = ezsolve(objective, {cbox, cbnd, ceq}, x0, options);

% Plot intermediate result
subplot(2,1,1);
ezplot([phi; omega]);
title('Robot states');

subplot(2,1,2);
ezplot(u);
title('Robot controls');

clear global AP4AD

Starting numeric solver
===== * * * =================================================================== * * *
TOMLAB - Tomlab Optimization Inc. Development license  999001. Valid to 2011-02-05
=====================================================================================
Problem: ---  1: Five Link MK2 Robot            f_k       0.781121381435685990
                                       sum(|constr|)      0.000004003058181095
                              f(x_k) + sum(|constr|)      0.781125384493867040
                                              f(x_0)      0.800000000000000040

Solver: snopt.  EXIT=0.  INFORM=1.
SNOPT 7.2-5 NLP code
Optimality conditions satisfied
MAD TB Automatic Differentiation estimating: gradient and constraint gradient

FuncEv    1 ConstrEv  133 ConJacEv   61 Iter   40 MinorIter 1236
CPU time: 67.109375 sec. Elapsed time: 55.922000 sec.

« Previous « Start » Next »