PROPT Acrobot
From TomWiki
Jump to navigationJump to search
Russ Tedrake. Underactuated Robotics: Learning, Planning, and Control for Efficient and Agile Machines. Working Draft of Course Notes for MIT 6.832 (Chapter 3).
Problem Formulation
The animation can be found here:
% Copyright (c) 2009-2009 by Tomlab Optimization Inc.
m1 = 1; m2 = 1;
l1 = 1; l2 = 2;
g = 9.81;
b1 = 0.1; b2 = 0.1;
lc1 = l1/2; lc2 = l2/2;
I1 = m1*l1^2/3; I2 = 1; % solid rod
xd = [pi; 0; 0; 0];
toms t
tf0 = 4;
p = tomPhase('p', t, 0, tf0, 80);
setPhase(p);
tomStates x1 x2 x3 x4
tomControls u
x = [x1;x2;x3;x4];
x0 = {icollocate({x == [pi*t/4;0;0;0]})
collocate({u == 0})};
cbox = {-10 <= collocate(u) <= 10};
% Boundary constraints
cbnd = {initial(x == 0), final(x == xd)};
% ODEs and path constraints
c1 = cos(x1); c2 = cos(x2);
s1 = sin(x1); s2 = sin(x2);
s12 = sin(x1+x2);
tempA = m2*g*l2;
tempB = m2*l1*lc2;
h11 = I1 + I2 + m2*l1^2 + 2*tempB*c2;
h12 = I2 + tempB*c2;
h22 = I2;
phi1 = 2*tempB*s2*x3*x4 + tempB*s2*x4^2 - (m1*lc1 + m2*l1)*g*s1 - tempA*s12 - b1*x3;
phi2 = u - tempB*s2*x3^2 - tempA*s12 - b2*x4;
detH = h11*h22 - h12^2;
ceq = collocate({
dot(x1) == x3; dot(x2) == x4
dot(x3) == (h22*phi1 - h12*phi2)/detH
dot(x4) == (-h12*phi1 + h11*phi2)/detH});
objective = integrate(u^2);
% Solve the problem
options = struct;
options.name = 'Acrobot';
solution = ezsolve(objective, {cbox, cbnd, ceq}, x0, options);
Problem type appears to be: qpcon Time for symbolic processing: 0.5786 seconds Starting numeric solver ===== * * * =================================================================== * * * TOMLAB - TOMLAB Development license 999007. Valid to 2011-12-31 ===================================================================================== Problem: --- 1: Acrobot f_k 15.819743055219059000 sum(|constr|) 0.000001842546637635 f(x_k) + sum(|constr|) 15.819744897765696000 f(x_0) 0.000000000000000000 Solver: snopt. EXIT=0. INFORM=1. SNOPT 7.2-5 NLP code Optimality conditions satisfied FuncEv 1 ConstrEv 301 ConJacEv 301 Iter 159 MinorIter 1143 CPU time: 5.834437 sec. Elapsed time: 5.802000 sec.