Prev: Getting data from a C++ program into simulink
Next: How to get debug to catch errors in listener callbacks?
From: Guilherme on 28 Oct 2009 00:01
I have been trying to use PROPT for a few days.
I use the solution for the acrobot (http://tomdyn.com/examples/acrobot.html) and try to simulate it, by applying the control signal (solution of PROPT) in a dynamic model of the acrobot (the same that was used by propt to generate the solution).

I already tried to interpolate the Propt solution in because I thought it had too large steps, but did not work.

The simulation fails though. Does someone here had the same problem? Any explanation?

* below I pasted my code.

------------- MAIN FUNCTION ------------------------------------
clear; clc;
close all;

%% Load model parameters
Acrobot.xG = [pi 0 0 0]';
Acrobot.m1 = 1;
Acrobot.m2 = 1;
Acrobot.l1 = 1;
Acrobot.l2 = 2;
Acrobot.g = 9.81;
Acrobot.lc1 = .5;
Acrobot.lc2 = 1;
Acrobot.I1 = Acrobot.m1*Acrobot.l1^2/3;
Acrobot.I2 = 1; %shouldnt it be m2*l2^2/3?
Acrobot.b1 = .1;
Acrobot.b2 = .1;
Acrobot.tau_max = 10;


%% Preparing PROPT inputs
xd = Acrobot.xG;
m1 = Acrobot.m1;
m2 = Acrobot.m2;
l1 = Acrobot.l1;
l2 = Acrobot.l2;
g = Acrobot.g;
lc1 = Acrobot.lc1;
lc2 = Acrobot.lc2;
I1 = Acrobot.I1;
I2 = Acrobot.I2;
b1 = Acrobot.b1;
b2 = Acrobot.b2;
umax = Acrobot.tau_max;



%% PROPT solution

% User inputs

% initial position
xi = [0; 0; 0; 0];
% final time
tf0 = 4;
% steps
ns = 80;
% max actuator input

toms t

p = tomPhase('p', t, 0, tf0, ns);
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 = {-umax <= collocate(u) <= umax};

% Boundary constraints
cbnd = {initial(x == xi), final(x == xd)};

c1 = cos(x1);
c2 = cos(x2);
s1 = sin(x1);
s2 = sin(x2);
s12 = sin(x1+x2);

% corrected equations
% h11 = I1 + I2 + m2*l1^2 + 2*m2*l1*lc2*c2;
% h12 = I2 + m2*l1*lc2*c2;
% h22 = I2;
% phi1 = 2*m2*l1*lc2*s2*x3*x4 + m2*l1*lc2*s2*x4^2 - (m1*lc1 + m2*l1)*g*s1 - m2*g*lc2*s12 - b1*x3;
% phi2 = u - m2*l1*lc2*s2*x3^2 - m2*g*lc2*s12 - b2*x4;

% propt website copy and paste
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;

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);


%% Prepare answer
tSolution(:,1) = subs(collocate(p,t),solution);
xSolution(:,1) = subs(collocate(p,x1),solution);
xSolution(:,2) = subs(collocate(p,x2),solution);
xSolution(:,3) = subs(collocate(p,x3),solution);
xSolution(:,4) = subs(collocate(p,x4),solution);
uSolution = subs(collocate(p,u),solution);

% Prepare to save solution
clear data
data = tSolution;
data = [data uSolution xSolution];
%save up01.dat data -ASCII -TABS


%% view solution from PROPT

figure(1)
subplot(3,1,1); hold on; grid on;
plot (tSolution(:,1), xSolution(:,1), 'ko-')
ylabel('Angle link 1(rad)')
subplot(3,1,2); hold on; grid on;
plot (tSolution(:,1), xSolution(:,2), 'ks-')
ylabel('Angle link 2(rad)')
subplot(3,1,3); hold on; grid on;
plot (tSolution(:,1), uSolution(:,1), 'ks-')
ylabel('Torque (Nm)')
xlabel('Time (s)')
figure(2)
subplot(3,1,1); hold on; grid on;
plot (tSolution(:,1), xSolution(:,3), 'ko-')
ylabel('Angular vel link 1(rad/s)')
subplot(3,1,2); hold on; grid on;
plot (tSolution(:,1), xSolution(:,4), 'ks-')
ylabel('Angular vel link 2(rad/s)')
subplot(3,1,3); hold on; grid on;
plot (tSolution(:,1), uSolution(:,1), 'ks-')
ylabel('Torque (Nm)')
xlabel('Time (s)')

%% Simulating PROPT solution

optODE = odeset('RelTol', 1e-12,'AbsTol', 1e-12 );

clear z0 tsim xsim
z0 = [ xi ];
for ks = 1: (ns-1)
[tODE, xODE] = ode45( @(tsim,xsim)MyAcrobotDynamics(tsim,xsim, uSolution, ks, Acrobot), [tSolution(ks), tSolution(ks+1)], z0, optODE);

figure(1);
subplot(3,1,1); hold on; grid on;
title('PROPT output X ODE output')
plot(tODE, xODE(:,1), 'b-', 'LineWidth',1.5);
subplot(3,1,2); hold on; grid on;
plot(tODE, xODE(:,2), 'b-', 'LineWidth',1.5)
subplot(3,1,3); hold on; grid on;
plot( [tSolution(ks) tSolution(ks+1)], [uSolution(ks) uSolution(ks)], 'bo-', 'LineWidth',1.5)

figure(2)
subplot(3,1,1); hold on; grid on;
title('PROPT output X ODE output')
plot(tODE, xODE(:,3), 'b-', 'LineWidth',1.5)
ylabel('Angle link 1(rad/s)')
subplot(3,1,2); hold on; grid on;
plot(tODE, xODE(:,4), 'b-', 'LineWidth',1.5)
ylabel('Angle link 2(rad/s)')
subplot(3,1,3); hold on; grid on;
plot( [tSolution(ks) tSolution(ks+1)], [uSolution(ks) uSolution(ks)], 'bo-', 'LineWidth',1.5)
ylabel('Torque (Nm)')
xlabel('Time (s)')

z0 = xODE(end,:)'; %update initial conditions from the last point.

end


------------- ODE FUNCTION FILE ------------------------------------

function [xdot] = MyAcrobotDynamics(t,x,up,ks,obj, model)

m1=obj.m1;
m2=obj.m2;
l1=obj.l1;
l2=obj.l2;
g=obj.g;
lc1=obj.lc1;
lc2=obj.lc2;
I1=obj.I1;
I2=obj.I2;
b1=obj.b1;
b2=obj.b2;

%% propt model
%disp('propt model')
% ODEs and path constraints
c1 = cos(x(1));
c2 = cos(x(2));
s1 = sin(x(1));
s2 = sin(x(2));
s12 = sin(x(1)+x(2));

h11 = I1 + I2 + m2*l1^2 + 2*m2*l1*lc2*c2;
h12 = I2 + m2*l1*lc2*c2;
h22 = I2;

% phi1 = 2*m2*l1*lc2*s2*x(3)*x(4) + m2*l1*lc2*s2*x(4)^2 - (m1*lc1 + m2*l1)*g*s1 - m2*g*lc2*s12 - b1*x(3);
% phi2 = up(ks) - m2*l1*lc2*s2*x(3)^2 - m2*g*lc2*s12 - b2*x(4);

phi1 = 2*m2*l1*lc2*s2*x(3)*x(4) + m2*l1*lc2*s2*x(4)^2 - (m1*lc1 + m2*l1)*g*s1 - m2*g*l2*s12 - b1*x(3);
phi2 = up(ks) - m2*l1*lc2*s2*x(3)^2 - m2*g*l2*s12 - b2*x(4);

detH = h11*h22 - h12^2;

xdot = [ x(3)
x(4)
(h22*phi1 - h12*phi2)/detH
(-h12*phi1 + h11*phi2)/detH];
end
 | 
Pages: 1
Prev: Getting data from a C++ program into simulink
Next: How to get debug to catch errors in listener callbacks?