Acrobot

Russ Tedrake. Underactuated Robotics: Learning, Planning, and Control for Efficient and Agile Machines. Working Draft of Course Notes for MIT 6.832 (Chapter 3).

Contents

Problem Formulation

The animation can be found here: http://tomdyn.com/examples/acrobot.avi

% 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
===== * * * =================================================================== * * *
TOMLAB - Tomlab Optimization Inc. Development license  999001. Valid to 2010-02-05
=====================================================================================
Problem: ---  1: Acrobot                        f_k       4.443242195366545700
                                       sum(|constr|)      0.000003790677510858
                              f(x_k) + sum(|constr|)      4.443245986044056300
                                              f(x_0)      0.000000000000000000

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

FuncEv    1 ConstrEv 1105 ConJacEv 1105 Iter  433 MinorIter 2568
CPU time: 39.046875 sec. Elapsed time: 39.125000 sec.