Commit 54ad5984 authored by Amira Abdel-Rahman's avatar Amira Abdel-Rahman
Browse files

change control law, and first path for optimization

parent 0e819fbc
......@@ -238,7 +238,7 @@ function animateSol(tspan, x,p)
h_title = title('t=0.0s');
axis equal
axis([-.2 .2 -.3 .1]);
% axis([-.2 .2 -.3 .1]);
%Step through and update animation
for i = 1:length(tspan)
......
......@@ -51,7 +51,7 @@ function [cineq ceq] = constraints(x,z0,p,ground,tf)
%
% ceq = [x(2) - t(indices(1)), maxy-0.4,vel]; % prob 6
cineq=[];
q1_start=z0(1) ;
q2_start=z0(2) ;
......@@ -71,6 +71,8 @@ function [cineq ceq] = constraints(x,z0,p,ground,tf)
dx_end= z(7,end);
dy_end=z(8,end);
cineq=[x_start-x_end];
ceq=[q1_start-q1_end,q2_start-q2_end, ...
y_start-y_end, ...
dq1_start-dq1_end,dq2_start-dq2_end, ...
......
function f = objective(x,z0,p,ground,tf)
% Inputs:
% x - an array of decision variables.
% z0 - the initial state
% p - simulation parameters
%
% Outputs:
% f - scalar value of the function (to be minimized) evaluated for the
% provided values of the decision variables.
%
% Note: fmincon() requires a handle to an objective function that accepts
% exactly one input, the decision variables 'x', and returns exactly one
% output, the objective function value 'f'. It is convenient for this
% assignment to write an objective function which also accepts z0 and p
% (because they will be needed to evaluate the objective function).
% However, fmincon() will only pass in x; z0 and p will have to be
% provided using an anonymous function, just as we use anonymous
% functions with ode45().
numCtrlPoints=5;
ctrl1.tf = tf; % control time points
ctrl1.T = x(1:numCtrlPoints);
ctrl2.tf = tf; % control time points
ctrl2.T = x(numCtrlPoints+1:end);
[t, z, u] = simulate_leg(z0,p,ground,tf,ctrl1,ctrl2,false); % run simulation
x_end= z(3,end);
f = -x_end; % negative of height
f = -max(z(4,:)); % negative of height
% alternate objective functions:
% f = tf; % final time
% ctrl_t = linspace(0, ctrl.tf, 50);
% ctrl_pt_t = linspace(0, ctrl.tf, length(ctrl.T));
% n = length(ctrl_t);
% ctrl_input = zeros(1,n);
%
% for i=1:n
% ctrl_input(i) = BezierCurve(x(3:end),ctrl_t(i)/ctrl.tf);
% end
%
% f = sum(sum(ctrl_input)); % minimize T^2 integral
% size(ctrl_input);
% cc=ctrl_input*ctrl_input'/50;
% size(u);
% uu=u*u';
% f = uu;
end
\ No newline at end of file
......@@ -32,7 +32,7 @@ function f = objective(x,z0,p,ground,tf)
x_end= z(3,end);
f = -z(3,end); % negative of height
f = -x_end; % negative of height
f = -max(z(4,:)); % negative of height
......
function plot_control(ctrl,u)
function plot_control(ctrl,u,figNum)
prob='6.';
id=2;
figure(1) % control input profile
figure(figNum) % control input profile
ctrl_t = linspace(0, ctrl.tf, length(u));
ctrl_pt_t = linspace(0, ctrl.tf, length(ctrl.T));
n = length(ctrl_t);
......
img/6.2_cntrl.png

32.9 KB | W: | H:

img/6.2_cntrl.png

36.4 KB | W: | H:

img/6.2_cntrl.png
img/6.2_cntrl.png
img/6.2_cntrl.png
img/6.2_cntrl.png
  • 2-up
  • Swipe
  • Onion skin
No preview for this file type
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment