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