Personal tools

You are here: Home / Matlab code pend.m

# Matlab code pend.m pend.m — Objective-C source code, 9 kB (9216 bytes)

## File contents

function result = pend(duration, targetangle, N, oldresult);

% Finds optimal motion of a torque-driven pendulum.  Task is to move from one
% static posture to another in a given time.
% Method: direct collocation with 3-point discretization formula for acceleration.

% Author: Ton van den Bogert <a.vandenbogert@csuohio.edu>

% Inputs:
%	duration		duration of the movement (s)
%	targetangle		target angle (deg)
%	N				number of collocation nodes to use
% 	oldresult		(optional) initial guess

% Notes:
% 1. This code may be useful as a template for solving other optimal control problems, such
%    as cart-pole upswing.
% 2. IPOPT will be used when it is installed, otherwise Newton's method.  IPOPT is recommended
%    because it is more robust.  Newton's method can still solve most problems, but you may
%    need to solve a sequence of problems of increasing difficulty to ensure convergence.
% 3. The solution may be a local optimum, especially for tasks that involve multiple
%    revolutions.  Try different initial guesses (maybe random) to check for this.

% The following examples all converge with IPOPT or Newton:
%	r = pend(1.0, 180, 100);		% swing up in 1 second, 100 collocation nodes
%   r = pend(3.0, 180, 100);		% now do it in 3 seconds, note the countermovement
%   r = pend(10.0, 180, 100);		% now do it in 10 seconds, multiple countermovements are seen
%   r = pend(5.0, 720, 300);		% do two full revolutions in 5 seconds, 300 collocation nodes
%   r2 = pend(..,..,..,r);			% use previous result r as initial guess

% settings
MaxIterations = 1000;
if exist('ipopt') == 3
method = 'ipopt';
else
disp('IPOPT is not installed.');
disp('Newton method will be used, may be more sensitive to initial guess.');
disp('Hit ENTER to continue...');
pause
method = 'newton';
end

% initializations
close all
tic
h = duration/(N-1);		% time interval between nodes
times = h*(0:N-1)';		% list of time points
Ncon = N-2 + 4;			% N-2 dynamics constraints and 4 task constraints

% model parameters
L = 1;			%	length of pendulum (m)
m = 1;			%	mass of pendulum (kg)
I = 1;			% 	moment of inertia relative to pivot (kg m^2)
g = 9.81;		%	gravity (m s^-2)

% state variable is x: angle relative to hanging down
% control variable is u: torque applied at joint

% if oldresult was provided, use it as initial guess, otherwise use zero initial guess (pendulum hangs down, no torque)
if (nargin == 4)
oldN = numel(oldresult.t);
oldreltime = (0:oldN-1)'/(oldN-1);			% sample times of old result, from 0 to 1
newreltime = (0:N-1)'/(N-1);				% sample times for new optimization, from 0 to 1
x = interp1(oldreltime, oldresult.x, newreltime);
u = interp1(oldreltime, oldresult.u, newreltime);
else
x = randn(N,1);
u = randn(N,1);
end

% encode initial guess of unknowns into a long column vector X
X0 = [x ; u];
ix = (1:N);				% index to elements in X where angles x are stored
iu = N + (1:N);			% index to elements in X where controls u are stored
NX = size(X0,1);		% number of unknowns
show(X0, confun(X0));	% show the initial guess
drawnow

% X0 = X0 + 0.001*randn(size(X0));		% perturb the initial guess a little before optimizing

if strcmp(method,'ipopt')
% solve the NLP with IPOPT
funcs.objective = @objfun;
funcs.constraints = @confun;
funcs.jacobian    = @conjac;
funcs.jacobianstructure = @conjacstructure;
options.cl = zeros(Ncon,1);
options.cu = zeros(Ncon,1);
options.ipopt.max_iter = MaxIterations;
options.ipopt.hessian_approximation = 'limited-memory';
[X, info] = ipopt(X0,funcs,options);
elseif strcmp(method, 'newton')
% solve the NLP using Newton iteration on the KKT conditions
X = X0;
ctol = 1e-4;		% constraint tolerance
ftol = 1e-4;		% cost function tolerance
xtol = 1e-4;		% solution tolerance
F = 1e10;
for iter=1:MaxIterations
Fprev = F;

% evaluate objective function F and constraint violations c
F = objfun(X);
H = objhess(X);
c = confun(X);
J = conjac(X);

% form the linearized KKT system K*x = b
K = [H J'; J sparse(Ncon,Ncon)];
b = [-G; -c];

% solve the linear system K*dZ=b
% Z is a vector containing the unknowns X and the Lagrange multipliers.  dZ is the change in this iteration
dZ = K\b;
dX = dZ(1:NX);					% the first NX are the elements of X

% do a half Newton step (converges slower than full Newton step, but more likely to converge)
% for more robust convergence, we should do a line search here to make sure we always have progress
X = X + dX/2;
rmsC = sqrt(mean(c.^2));
rmsdX = sqrt(mean(dX.^2));
fprintf('Iter: %3d  F=%10.5e  rms(c)=%10.5e   rms(dX)=%10.5e\n', iter,F,rmsC,rmsdX);

if (max(abs(c)) < ctol) && (abs(F-Fprev)<ftol) && (mean(abs(dX))<xtol)
break;
end
end
if iter >= MaxIterations
disp('Maximum number of iterations exceeded.');
else
disp('Optimal solution found');
end
else
error('method not recognized');
end

% plot results
show(X, confun(X));

% make movie of the solution
disp('Hit ENTER to generate animation...');
pause
fps = N/duration;
avi = VideoWriter('pend.avi');
open(avi);
figure(2);
clf;
set(gcf,'Position',[5 100 650 650]);
set(gcf, 'color', 'white');
s = 1.5*L;
for i=1:N
plot([-s s],[0 0],'k','LineWidth',2);
hold on
plot([0 L*cos(X(i)-pi/2)], [0 L*sin(X(i)-pi/2)],'b-o','LineWidth',2);
axis('equal');
axis('square');
axis([-s s -s s]);
title(['t = ' num2str(times(i),'%8.3f')]);
if (i==1)
F = getframe(gca);
frame = [1 1 size(F.cdata,2) size(F.cdata,1)];
else
F = getframe(gca,frame);
end
writeVideo(avi,F);
drawnow;
hold off;
end
close(avi);
%	close(2);

% store results
result.t = times;
result.x = X(1:N);
result.u = X(N+(1:N));

% start of embedded functions

%=========================================================
function F = objfun(X);
% objective function: integral of squared controls
F = h * sum(X(iu).^2);
end

%=========================================================
% gradient of the objective function coded in objfun
G = zeros(NX,1);
G(iu) = 2 * h * X(iu);
end

%=========================================================
function H = objhess(X);
% hessian of objective function coded in objfun
H = spalloc(NX,NX,N);
H(iu,iu) = 2 * h * speye(N,N);
end

%=========================================================
function c = confun(X)

% constraint function (dynamics constraints and task constraints)

% size of constraint vector
c = zeros(Ncon,1);

% dynamics constraints
% Note: torques at node 1 and node N do not affect movement and will therefore
% always be zero in a minimal-effort solution.
for i=1:N-2
x1 = X(i);
x2 = X(i+1);
x3 = X(i+2);
u2 = X(N+i+1);
xdd = (x3 - 2*x2 + x1)/h^2;							% three-point formula for angular acceleration
c(i) =  xdd - ( -m * g * L*sin(x2) + u2) / I;		% equation of motion must be satisfied
end

% initial position must be zero:
c(N-1) 	= X(1);
% initial velocity must be zero:
c(N) 	= X(2)-X(1);
% final position must be at target angle:
c(N+1) 	= X(N) - targetangle*pi/180;
% final velocity must be zero:
c(N+2) 	= X(N)-X(N-1);

% show current iterate, every 0.1 second
%		if toc > 0.1
show(X,c);
tic;
%		end

end
%=========================================================
function J = conjac(X)

% size of Jacobian
J = spalloc(Ncon,NX,4*(N-2) + 6);

% dynamics constraints
for i=1:N-2
% Jacobian matrix: derivatives of c(i) with respect to the elements of X
x2 = X(i+1);
J(i,i) 		= 1/h^2;
J(i,i+1) 	= -2/h^2 + m * g * L * cos(x2) / I;
J(i,i+2) 	= 1/h^2;
J(i,N+i+1) 	= -1/I;

end

% initial position must be zero:
J(N-1,	1) = 1;
% initial velocity must be zero:
J(N,2) = 1;
J(N,1) = -1;
% final position must be at target angle:
J(N+1,N) = 1;
% final velocity must be zero:
J(N+2,N) = 1;
J(N+2,N-1) = -1;

end
%=========================================================
function J = conjacstructure(X)

% size of Jacobian
J = spalloc(Ncon,NX,4*(N-2) + 6);

% dynamics constraints
for i=1:N-2
% Jacobian matrix: derivatives of c(i) with respect to the elements of X
J(i,i) 		= 1;
J(i,i+1) 	= 1;
J(i,i+2) 	= 1;
J(i,N+i+1) 	= 1;
end

% initial position must be zero:
J(N-1,	1) = 1;
% initial velocity must be zero:
J(N,2) = 1;
J(N,1) = 1;
% final position must be at target angle:
J(N+1,N) = 1;
% final velocity must be zero:
J(N+2,N) = 1;
J(N+2,N-1) = 1;

end
%============================================================
function show(X,c)
% plot the current solution
x = X(ix);
u = X(iu);
figure(1)
subplot(3,1,1);plot(times,x*180/pi);title('angle')
subplot(3,1,2);plot(times,u);title('torque');
subplot(3,1,3);plot(c);title('constraint violations');
end
end