You are here: Home / Resources / Human motion seminar Jan 23, 2014 / Matlab code pend.m

Matlab code pend.m

Objective-C source code icon 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>
	
	% This work is licensed under a Creative Commons Attribution 3.0 Unported License.
	% http://creativecommons.org/licenses/by/3.0/deed.en_US
	
	% 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.gradient  = @objgrad;
		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);
			G = objgrad(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

	%=========================================================
	function G = objgrad(X);
		% 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
		
		% task constraints

		% 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
		
		% task constraints

		% 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
		
		% task constraints

		% 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