### 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); ... ... 32.9 KB | W: | H:

36.4 KB | W: | H:  • 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