w3p4

Acrobot Energy Shaping | Problem Set 3 | 6.832x Courseware | edX https://courses.edx.org/courses/MITx/6.832x/3T2014/courseware/wee...

1 of 4 10/01/2015 14:13

Edited with the trial version of Foxit Advanced PDF Editor

To remove this notice, visit:www.foxitsoftware.com/shopping

http://www.foxitsoftware.com/shopping

[t,x,x_grade]=AcrobotController.run

x_grade = [

% paste here

];

1

2

3

4


2 of 4 10/01/2015 14:13




% controller code

[f,df] = obj.p.dynamics(0,[pi;0;0;0],0);

A = df(:,2:5);

B = df(:,end);

[K,S] = lqr(A,B,eye(4),1);

xe = [q;qd] - [pi;0;0;0];

if xe'*S*xe < 1000

u = K*([pi;0;0;0] - [q;qd]);

else

% some useful functions + properties

% acceleration due to gravity

g = 9.81;

m = obj.p.getMass;

% computes center of mass [x;z] at any position

com = obj.p.getCOM(q);

% kinetic energy of a manipulator is 1/2 * qdot^T * H * qdot

[H,C,B] = obj.p.manipulatorDynamics(q,qd);

E_des = m*g*1.25;

E_current = m*g*com(2) + .5*qd'*H*qd;

M22bar = H(2,2) - H(2,1)/H(1,1)*H(1,2);

h2bar = C(2) - H(2,1)/H(1,1)*C(1);

k1 = 10;

k2 = 10;

k3 = 1;

ubar = (E_des - E_current)*qd(2);

u = M22bar*(-k1*q(2) - k2*qd(2)) + h2bar + k3*ubar;

end


3 of 4 10/01/2015 14:13





4 of 4 10/01/2015 14:13




w3p4

Documents