# Jumbo Crane Container Control

## Problem description

Time-optimal control of a Jumbo Container Crane avoiding an obstacle.

Crane dynamics described in the book: Informatics in control automation and robotics II DOI 10.1007/978-1-4020-5626-0 , Springer, 2007, pp.79-84. T.J.J. van den Boom, J.B. Klaassens, R. Meiland Real-time optimal control for a non linear container crane using a neural network

Optimal control problem by: W.L. De Koning, G Fitie and L.G. Van Willigenburg

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 = [10 10 40];

toms t tf % Free final time

toms ho1

for i=1:length(narr)
```
```    p = tomPhase('p', t, 0, tf, narr(i), [], 'cheb');
setPhase(p)

tomStates x1 x2 x3 x4 x5 x6
tomControls u1 u2

x = [x1; x2; x3; x4; x5; x6];

% Crane parameters
gr = 9.81;  He = 50;    Jh = 35.6;  Jt = 13.5;  le = 40;  ht = 50;
mc = 47000; mt = 33000; Nh = 26.14; Nt = 16.15; rh = 0.6; rt = 0.5;
xol = 8;    xor = 15;   hob = 15;

% Initial & terminal states
xi = [0;  0; 0; 0; 50; 0];
xf = [50; 0; 0; 0; 50; 0];

% Initial guess
if i==1;
x0 = {tf==20.4; ho1==0; icollocate({x1 == 50*t/20; x2 == xi(2)
x3 == xi(3); x4 == xi(4); x5 == xi(5); x6 == xi(6)})
collocate({u1 == -2000; u2 == -5000})};
else
x0 = {tf==tfopt;
icollocate({x1 == xopt1; x2 == xopt2
x3 == xopt3; x4 == xopt4; x5 == xopt5; x6 == xopt6})
collocate({u1 == uopt1; u2 == uopt2})};
end

% Box constraints
cbox = {15 <= tf <= 30; -4200 <= collocate(u1) <= 4200
-11490 <= collocate(u2) <= 11490};

% Boundary constraints
cbnd = {initial(x == xi), final(x == xf)};

Gt  = Jt*Nt*Nt/(rt*rt); Gh = Jh*Nh*Nh/(rh*rh);
st  = sin(x3); ct = cos(x3);
Ft  = (Nt/rt)*u1; Fh = (Nh/rh)*u2;
d2x = (mc+Gh)*Ft-mc*Fh.*st+mc*gr*Gh*st.*ct+mc*Gh*x5.*x4.*x4.*st;
d2x = d2x./((mc+Gh)*(mt+Gt)+mc*Gh*(1-ct.*ct));

% xc is the container x position against time
xc = x1+x5*sin(x3);
% hc is the height of the container against time
hc = ht-x5*cos(x3);
% ho is the height of the obstacle at the container x position.
%ho = ifThenElse(xc<=xol,0,ifThenElse(xc>=xor,0,hob));
% do is the distance to the obstacle from the container.
do = max(max(xol-xc,xc-xor),hc-ho1);

% Path constraint - Distance to obstacle should always be >= 0
% and height should always be >= 0.
% Test 300 points, evenly spaced in time.
pth = {atPoints(linspace(0,tf,300),{do>=0,hc>=0}),ho1>=hob};

% ODEs
ode = collocate({
dot(x1) == x2
dot(x2) == d2x
dot(x3) == x4
dot(x4) == (-2*x6.*x4-gr*st-d2x.*ct)./x5
dot(x5) == x6
dot(x6) == (Fh+mc*x5.*x4.*x4+mc*gr*ct-mc*d2x.*st)/(mc+Gh)
});

% Objective
objective = tf;
```

## Solve the problem

```    options = struct;
if i==1
% To improve convergece, we make the obstacle constraint softer,
% by including it in the ojbective rather than as a hard constraint
% in the first iteration.
% This is necessary because of the very nonlinear properties of this
% constraint.
pth = pth{1};
objective = objective - 2*ho1;
end
options.name = 'Crane with obstacle';
solution = ezsolve(objective, {cbox, cbnd, pth, ode}, x0, options);

tfopt = subs(tf,solution);
xopt1 = subs(x1,solution);
xopt2 = subs(x2,solution);
xopt3 = subs(x3,solution);
xopt4 = subs(x4,solution);
xopt5 = subs(x5,solution);
xopt6 = subs(x6,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: Crane with obstacle            f_k     -34.674715981432669000
sum(|constr|)      0.000000101978588531
f(x_k) + sum(|constr|)    -34.674715879454084000
f(x_0)     20.399999999999999000

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

FuncEv    1 ConstrEv  356 ConJacEv  356 Iter   95 MinorIter 8525
CPU time: 6.875000 sec. Elapsed time: 6.891000 sec.
```
```Problem type appears to be: lpcon
===== * * * =================================================================== * * *
TOMLAB - Tomlab Optimization Inc. Development license  999001. Valid to 2010-02-05
=====================================================================================
Problem: ---  1: Crane with obstacle            f_k      23.843382233623789000
sum(|constr|)      0.000000280437766914
f(x_k) + sum(|constr|)     23.843382514061556000
f(x_0)     30.000000000000000000

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

FuncEv    1 ConstrEv   17 ConJacEv   17 Iter   13 MinorIter  168
CPU time: 0.406250 sec. Elapsed time: 0.406000 sec.
```
```Problem type appears to be: lpcon
===== * * * =================================================================== * * *
TOMLAB - Tomlab Optimization Inc. Development license  999001. Valid to 2010-02-05
=====================================================================================
Problem: ---  1: Crane with obstacle            f_k      19.558349883431084000
sum(|constr|)      0.012120632528751712
f(x_k) + sum(|constr|)     19.570470515959837000
f(x_0)     23.843382233623789000

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

FuncEv    1 ConstrEv  102 ConJacEv  102 Iter   44 MinorIter 2859
CPU time: 8.187500 sec. Elapsed time: 8.234000 sec.
```
```end

% Get solution for 50 points, evenly distributed in time.
nt = 50;
topt = linspace(0,subs(tf,solution),nt);
xopt = subs(atPoints(topt,x),solution);

% Plot
[nt,nx]=size(xopt);

clf
axis([-10 60 0 50]);
axis image;

% Draw Obstacle
line([xol xol xor xor],[0 hob hob 0]);
title('Crane cable trajectory and obstacle');

if nx==6
xtop=xopt(:,1); ytop=50*ones(size(topt));
xbottom=xopt(:,1)+xopt(:,5).*sin(xopt(:,3));
ybottom=50-xopt(:,5).*cos(xopt(:,3));
elseif nx==4
xtop=xopt(:,1); ytop=50*ones(size(topt));
xbottom=xopt(:,1)+40*sin(xopt(:,3));
ybottom=50-40*cos(xopt(:,3));
else
error('Crane model should have 4 or 6 state variables');
end

% Draw cable trajectory
tl=5; lt=length(topt);
for k=1:lt
line([xtop(k) xbottom(k)],[ytop(k) ybottom(k)]);
if tl/lt>0.01; pause(tl/lt); end
end
hold on
ezplot(xc,hc);
hold off
xlabel('X coordinate'); ylabel('Y coordinate');
``` 