From 5e77c3beedfa2e825a9cd0ea88384faf90c04704 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Sat, 21 Sep 2024 11:46:33 +0200 Subject: [PATCH] abbandon correction, linearize around trajectory try three different approaches, with questionable results --- control_act.m | 262 ++++++++---------- plot_all.m | 37 +-- set_initial_conditions.m | 2 + set_trajectory.m | 2 +- tesi.m | 51 ++-- .../chill_errortheta_3deg/common.mat | Bin 0 -> 738 bytes 6 files changed, 143 insertions(+), 211 deletions(-) create mode 100644 tests/straightline/chill_errortheta_3deg/common.mat diff --git a/control_act.m b/control_act.m index 33e7b1b..3597788 100644 --- a/control_act.m +++ b/control_act.m @@ -1,166 +1,134 @@ -function [u, ut, uc, U_corr_history, q_pred] = control_act(t, q, sim_data) - dc = decouple_matrix(q, sim_data); - ut = utrack(t, q, sim_data); - ut = dc*ut; - - [uc, U_corr_history, q_pred] = ucorr(t, q, sim_data); - - uc = dc*uc; - u = ut+uc; - % saturation - u = min(sim_data.SATURATION, max(-sim_data.SATURATION, u)); -end - -function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data) +function [u, ut, q_pred] = control_act(t, q, sim_data) pred_hor = sim_data.PREDICTION_HORIZON; + + % track only + if eq(pred_hor, 0) + + dc = decouple_matrix(q, sim_data); + ut = utrack(t, q, sim_data); + u = dc*ut; + + % saturation + u = min(sim_data.SATURATION, max(-sim_data.SATURATION, u)); + prob = []; + q_pred = []; + return + end + + % mpc SATURATION = sim_data.SATURATION; PREDICTION_SATURATION_TOLERANCE = sim_data.PREDICTION_SATURATION_TOLERANCE; tc = sim_data.tc; - - u_corr = zeros(2,1); - U_corr_history = zeros(2,1,sim_data.PREDICTION_HORIZON); - q_act = q; - u_track_pred=zeros(2,1, pred_hor); - T_inv_pred=zeros(2,2, pred_hor); - - q_pred = []; - s_ = SATURATION - ones(2,1)*PREDICTION_SATURATION_TOLERANCE; - if eq(pred_hor, 0) - return - elseif eq(pred_hor, 1) - T_inv = decouple_matrix(q_act, sim_data); - ut = utrack(t, q_act, sim_data); - TH = T_inv; - H = 2 * (TH') * [1, 0; 0, 0] * [1, 0; 0, 0] * TH; - %H = eye(2); - f = zeros(2,1); - A = [T_inv; -T_inv]; - %A = [eye(2); -eye(2)]; + % prediction + %T_invs = zeros(2,2, pred_hor); + %Qs = zeros(3,1,pred_hor); + %drefs = zeros(2,1, pred_hor); + %refs = zeros(2,1, pred_hor); - d = T_inv*ut; - b = [s_-d;s_+d]; + % optim problem + prob = optimproblem('ObjectiveSense', 'minimize'); + % objective + obj = 0; + % decision vars + ss_ = repmat(s_, [1,1, pred_hor]); + ucorr = optimvar('ucorr', 2, pred_hor,'LowerBound', -ss_, 'UpperBound', ss_); + % state vars + Q = optimvar('state', 3, pred_hor); + % initial conditions + prob.Constraints.evo = Q(:, 1) == q'; + + + % linearization around robot trajectory + % only needs to be calculated once + theta = q(3); + st = sin(theta); + ct = cos(theta); + T_inv = decouple_matrix(q, sim_data); + + + for k=1:pred_hor + t_ = t + tc * (k-1); - % solve qp problem - options = optimoptions('quadprog', 'Display', 'off'); - u_corr = quadprog(H, f, A, b, [],[],[],[],[],options); - - q_pred = q_act; - U_corr_history(:,:,1) = u_corr; - return - else - %if pred_hor > 1 - % move the horizon over 1 step and add trailing zeroes - U_corr_history = cat(3, sim_data.U_corr_history(:,:, 2:end), zeros(2,1)); - %end - - %disp('start of simulation') - % for each step in the prediction horizon, integrate the system to - % predict its future state - - for k = 1:pred_hor - % start from the old (known) state - - % compute the inputs, based on the old state - - % u_corr is the prediction done at some time in the past, as found in U_corr_history - u_corr_ = U_corr_history(:, :, k); - % u_track can be computed from q - t_ = t + tc * (k-1); - u_track_ = utrack(t_, q_act, sim_data); - - T_inv = decouple_matrix(q_act, sim_data); - % compute inputs (v, w)/(wr, wl) - u_ = T_inv * (u_track_ + u_corr_); - + % reference trajectory and derivative + ref_s = double(subs(sim_data.ref, t_)); + dref_s = double(subs(sim_data.dref, t_)); - % if needed, map (wr, wl) to (v, w) for unicicle - if eq(sim_data.robot, 1) - u_ = diffdrive_to_uni(u_, sim_data); - end - - % integrate unicycle - theta_new = q_act(3) + tc*u_(2); - % compute the state integrating with euler - %x_new = q_act(1) + tc*u_(1) * cos(q_act(3)); - %y_new = q_act(2) + tc*u_(1) * sin(q_act(3)); - % compute the state integrating via runge-kutta - x_new = q_act(1) + tc*u_(1) * cos(q_act(3) + 0.5*tc*u_(2)); - y_new = q_act(2) + tc*u_(1) * sin(q_act(3) + 0.5*tc*u_(2)); - - q_new = [x_new; y_new; theta_new]; - - % save history - q_pred = [q_pred; q_new']; - u_track_pred(:,:,k) = u_track_; - T_inv_pred(:,:,k) = T_inv; - - % Prepare old state for next iteration - q_act = q_new; - end - %{ - Now setup the qp problem - It needs: - - Unknowns, u_corr at each timestep. Will be encoded as a vector of - vectors, in which every two elements is a u_j - i.e. (u_1; u_2; u_3; ...; u_C) = (v_1; w_1; v_2, w_2; v_3, w_3; ... - ; v_C, w_C) - It is essential that the vector stays a column, so that u'u is the - sum of the squared norms of each u_j - - - Box constraints: a constraint for each timestep in the horizon. - Calculated using the predicted state and inputs. They need to be - put in matrix (Ax <= b) form + % linearization around trajectory trajectory, hybrid approach + % theta from reference position and velocity + if eq(k, 1) + % only for the first step, theta from the current state + theta = q(3); + else + % then linearize around reference trajectory + % proper way + theta = mod( atan2(dref_s(2), dref_s(1)) + 2*pi, 2*pi); + % or, derivative using difference. Seem to give the same + % results + %ref_s1 = double(subs(sim_data.ref, t_ + tc)); + %theta = atan2(ref_s1(2)-ref_s(2), ref_s1(1)-ref_s(1)); + end + st = sin(theta); + ct = cos(theta); + T_inv = decouple_matrix([ref_s(1); ref_s(2); theta], sim_data); %} - - % box constrains - % A becomes sort of block-diagonal - % A will be at most PREDICTION_HORIZON * 2 * 2 (2: size of T_inv; 2: - % accounting for T_inv and -T_inv) by PREDICTION_HORIZON (number of - % vectors in u_corr times the number of elements [2] in each vector) - A_deq = []; - b_deq = []; - H1 = []; - for k=1:pred_hor - T_inv = T_inv_pred(:,:,k); - u_track = u_track_pred(:,:,k); - d = T_inv*u_track; - TH = [1, 0; 0, 0] * T_inv; - H1 = blkdiag(H1, TH); + %{ + % linearization around trajectory trajectory, "correct" approach + theta = mod( atan2(dref_s(2), dref_s(1)) + 2*pi, 2*pi); + st = sin(theta); + ct = cos(theta); + T_inv = decouple_matrix([ref_s(1); ref_s(2); theta], sim_data); + %} + + + % not at the end of the horizon + if k < pred_hor - 1 + if eq(sim_data.robot, 0) + % inputs for unicycle + v = ucorr(1,k); + w = ucorr(2,k); + else + % inputs for differential drive + v = sim_data.r * (ucorr(1,k) + ucorr(2,k)) / 2; + w = sim_data.r * (ucorr(1,k) - ucorr(2,k)) / sim_data.d; + end - A_deq = blkdiag(A_deq, [T_inv; -T_inv]); - b_deq = [b_deq; s_ - d; s_ + d]; + % evolution constraints + c = Q(:, k+1) == Q(:, k) + [v*tc*ct; v*tc*st; w*tc]; + prob.Constraints.evo = [prob.Constraints.evo; c]; end - H = H1'*H1; - - %A_deq = kron(eye(pred_hor), [eye(2); -eye(2)]); - %A_deq - %b_deq - % unknowns - - % squared norm of u_corr. H must be identity, - % PREDICTION_HORIZON*size(u_corr) - %H = eye(pred_hor*2)*2; - %H = kron(eye(pred_hor), [1,0;0,0]); - % no linear terms - f = zeros(pred_hor*2, 1); - - % solve qp problem - options = optimoptions('quadprog', 'Display', 'off'); - U_corr = quadprog(H, f, A_deq, b_deq, [],[],[],[],[],options); - %U_corr = lsqnonlin(@(pred_hor) ones(pred_hor, 1), U_corr_history(:,:,1), [], [], A_deq, b_deq, [], []); - - % reshape the vector of vectors to be an array, each element being - % u_corr_j as a 2x1 vector - % and add the prediction at t_k+C - U_corr_history = reshape(U_corr, [2,1,pred_hor]); - % first result is what to do now - u_corr=U_corr_history(:,:, 1); + % objective + % sum of squared norms of u-q^d + + % feedback + tracking input + % cannot use the utrack function, or the current formulation makes + % the problem become non linear + err = ref_s - [Q(1, k) + sim_data.b*ct; Q(2, k) + sim_data.b*st ]; + ut_ = dref_s + sim_data.K*err; + %ut = utrack(t_, Q(k, :), sim_data); + qd = T_inv*ut_; + ud = ucorr(:, k)-qd; + obj = obj + (ud')*ud; end + % end linearization around trajectory + + + prob.Objective = obj; + %show(prob) + %disp("to struct") + %prob2struct(prob); + + opts=optimoptions(@quadprog,'Display','off'); + sol = solve(prob, 'Options',opts); + u = sol.ucorr(:, 1); + q_pred = sol.state'; + + % ideal tracking for the predicted state + ut = decouple_matrix(q_pred, sim_data)*utrack(t, q_pred, sim_data); end function u_track = utrack(t, q, sim_data) diff --git a/plot_all.m b/plot_all.m index 28d4773..306651c 100644 --- a/plot_all.m +++ b/plot_all.m @@ -8,25 +8,8 @@ disp('Photos will start in 3s') pause(3) PLOT_TESTS = [ - "results-diffdrive/straightline/chill/11-09-2024-16-57-01"; - "results-diffdrive/straightline/chill_errortheta_pisixths/11-09-2024-16-57-43"; - "results-diffdrive/straightline/chill_errory/11-09-2024-16-59-04"; - "results-diffdrive/straightline/toofast/11-09-2024-16-58-24"; - "results-diffdrive/circle/start_center/11-09-2024-16-59-50"; - "results-diffdrive/square/11-09-2024-17-06-14"; - "results-diffdrive/figure8/chill/11-09-2024-17-00-53"; - %"results-diffdrive/figure8/fancyreps/11-09-2024--45-28"; - "results-diffdrive/figure8/toofast/11-09-2024-17-01-43"; - - "results-unicycle/straightline/chill/11-09-2024-17-07-51"; - "results-unicycle/straightline/chill_errortheta_pisixths/11-09-2024-17-08-35"; - "results-unicycle/straightline/chill_errory/11-09-2024-17-10-00"; - "results-unicycle/straightline/toofast/11-09-2024-17-09-18"; - "results-unicycle/circle/start_center/11-09-2024-17-10-48"; - "results-unicycle/square/11-09-2024-17-17-21"; - "results-unicycle/figure8/chill/11-09-2024-17-11-53"; - %"results-unicycle/figure8/fancyreps/11-09-2024--45-28"; - "results-unicycle/figure8/toofast/11-09-2024-17-12-45"; + "results-unicycle-costfun2-soltraj/circle/start_center/21-09-2024-15-29-40" + "results-diffdrive-costfun2-soltraj/circle/start_center/21-09-2024-12-16-00" ] s_ = size(PLOT_TESTS) @@ -37,7 +20,7 @@ for i = 1:s_(1) PLOT_TEST = [sPLOT_TEST, '/workspace_composite.mat'] load(PLOT_TEST) - dir = ['images-', ROBOT, '/', sPLOT_TEST, '/'] + dir = ['images-', ROBOT, '-costfun2-soltraj/', sPLOT_TEST, '/'] mkdir(dir); for n=1:3 @@ -50,14 +33,6 @@ for i = 1:s_(1) pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_trajectory.png']) pause(1); clf; plot_error(t{n}, ref_t{n}, q{n}) pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_error.png']) - pause(1); clf; plot_doubleinput(t{n}, sim_data{n}.SATURATION, U_track{n}, U_corr{n}, 0, in1, in2, m1, m2) - pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_double_input_1x2.png']) - pause(1); clf; plot_doubleinput(t{n}, sim_data{n}.SATURATION, U_track{n}, U_corr{n}, 1, in1, in2, m1, m2) - pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_double_input_2x1.png']) - pause(1); clf; plot_tripleinput(t{n}, sim_data{n}.SATURATION, U{n}, U_track{n}, U_corr{n}, 0, in1, in2, m1, m2) - pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_triple_input_1x2.png']) - pause(1); clf; plot_tripleinput(t{n}, sim_data{n}.SATURATION, U{n}, U_track{n}, U_corr{n}, 1, in1, in2, m1, m2) - pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_triple_input_2x1.png']) %pause(1); clf; plot_input(t{n}, sim_data{n}.SATURATION, U_track{n}, 'track') %pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_track_input.png']) @@ -75,12 +50,6 @@ for i = 1:s_(1) pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_error_out.png']) end - % correction difference (multistep, 1-step) - pause(1); clf; plot_input_diff(t{3}, t{2}, U_corr{3}, U_corr{2}, 0, ['\textbf{$$' in1 '^{corr, multistep}$$}'], ['\textbf{$$' in1 '^{corr, 1step}$$}'], ['\textbf{$$' in2 '^{corr, multistep}$$}'], ['\textbf{$$' in2 '^{corr, 1step}$$}'], m1, m2) - pause(0.5); export_fig(gcf, '-transparent', [dir, 'corr_input_diff_1x2.png']) - pause(1); clf; plot_input_diff(t{3}, t{2}, U_corr{3}, U_corr{2}, 1, ['\textbf{$$' in1 '^{corr, multistep}$$}'], ['\textbf{$$' in1 '^{corr, 1step}$$}'], ['\textbf{$$' in2 '^{corr, multistep}$$}'], ['\textbf{$$' in2 '^{corr, 1step}$$}'], m1, m2) - pause(0.5); export_fig(gcf, '-transparent', [dir, 'corr_input_diff_2x1.png']) - % input difference (saturated track only, 1-step) pause(1); clf; plot_input_diff(t{1}, t{2}, U{1}, U{2}, 0, ['\textbf{$$' in1 '^{trackonly-sat}$$}'], ['\textbf{$$' in1 '^{1step}$$}'], ['\textbf{$$' in2 '^{trackonly-sat}$$}'], ['\textbf{$$' in2 '^{1step}$$}'], m1, m2) pause(0.5); export_fig(gcf, '-transparent', [dir, 'input_diff_track_1step_1x2.png']) diff --git a/set_initial_conditions.m b/set_initial_conditions.m index d046245..d28dfd6 100644 --- a/set_initial_conditions.m +++ b/set_initial_conditions.m @@ -20,5 +20,7 @@ switch i q0 = [0;0;pi/2]; case 9 q0 = [2.5; 0; pi/2]; + case 10 + q0 = [0;0;deg2rad(3)]; end end \ No newline at end of file diff --git a/set_trajectory.m b/set_trajectory.m index ebe3398..e27d0fd 100644 --- a/set_trajectory.m +++ b/set_trajectory.m @@ -6,7 +6,7 @@ switch i % a straigth line trajectory at v=0.2m/s xref = 0.2*s; yref = 0; - case 1 + case 1 % a straigth line trajectory at v=0.8m/s xref = 0.8*s; yref = 0; diff --git a/tesi.m b/tesi.m index 27483a9..829f1da 100644 --- a/tesi.m +++ b/tesi.m @@ -5,6 +5,7 @@ close all % options ROBOT = 'diffdrive' %TESTS = ["straightline/chill", "straightline/chill_errortheta_pisixths", "straightline/toofast", "straightline/chill_errory", "circle/start_center", "figure8/chill", "figure8/toofast", "square"] +%TESTS = ["straightline/chill", "straightline/chill_errortheta_3deg", "circle/start_center", "square", "figure8/chill"] TESTS = ["circle/start_center"] % main @@ -24,7 +25,6 @@ for i = 1:length(TESTS) sim_data.(fn{1}) = test_data.(fn{1}); end - sim_data.r = 0.06 % set trajectory and starting conditions sim_data.q0 = set_initial_conditions(sim_data.INITIAL_CONDITIONS); [ref dref] = set_trajectory(sim_data.TRAJECTORY, sim_data); @@ -51,6 +51,10 @@ for i = 1:length(TESTS) for fn = fieldnames(data)' sim_data.(fn{1}) = data.(fn{1}); end + + if sim_data.PREDICTION_HORIZON > 1 + sim_data.PREDICITON_HORIZON = 40 + end % initialize prediction horizon sim_data.U_corr_history = zeros(2,1,sim_data.PREDICTION_HORIZON); @@ -58,7 +62,7 @@ for i = 1:length(TESTS) % simulate robot tic; - [t, q, y, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = simulate_discr(sim_data); + [t, q, y, ref_t, U, U_track, Q_pred] = simulate_discr(sim_data); toc; disp('Done') @@ -66,7 +70,7 @@ for i = 1:length(TESTS) % save simulation data f1 = [ TEST '/' char(datetime, 'dd-MM-yyyy-HH-mm-ss')]; % windows compatible name - f = ['results-' ROBOT '-costfun/' f1]; + f = ['results-' ROBOT '-costfun2-soltraj/' f1]; mkdir(f) % save workspace dsave([f '/workspace_composite.mat']); @@ -79,16 +83,16 @@ for i = 1:length(TESTS) s1_ = size(worker_index); for n = 1:s1_(2) h = [h, figure('Name', [TEST ' ' num2str(n)] )]; - plot_results(t{n}, q{n}, ref_t{n}, U{n}, U_track{n}, U_corr{n}); + plot_results(t{n}, q{n}, ref_t{n}, U{n}, U_track{n}, U_track{n}); end % plot correction different between 1-step and multistep h = [h, figure('Name', 'difference between 1step and multistep')]; subplot(2,1,1) - plot(t{2}, U_corr{2}(:, 1) - U_corr{3}(:, 1)) + plot(t{2}, U{2}(:, 1) - U{3}(:, 1)) xlabel('t') ylabel(['difference on ' sim_data{1}.input1_name ' between 1-step and multistep']) subplot(2,1,2) - plot(t{2}, U_corr{2}(:, 2) - U_corr{3}(:, 2)) + plot(t{2}, U{2}(:, 2) - U{3}(:, 2)) xlabel('t') ylabel(['difference on ' sim_data{1}.input2_name ' between 1-step and multistep']) % save figures @@ -100,37 +104,31 @@ for i = 1:length(TESTS) end + %% FUNCTION DECLARATIONS % Discrete-time simulation -function [t, q, y, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = simulate_discr(sim_data) +function [t, q, y, ref_t, U, U_track, Q_pred] = simulate_discr(sim_data) tc = sim_data.tc; steps = sim_data.tfin/tc q = sim_data.q0'; t = 0; - Q_pred = zeros(sim_data.PREDICTION_HORIZON,3,sim_data.tfin/sim_data.tc + 1); - U_corr_pred_history=zeros(sim_data.PREDICTION_HORIZON,2,steps); - [u_discr, u_track, u_corr, U_corr_history, q_pred] = control_act(t, q, sim_data); - sim_data.U_corr_history = U_corr_history; + Q_pred = zeros(sim_data.PREDICTION_HORIZON,3, steps + 1); + + [u_discr, u_track, q_pred] = control_act(t(end), q(end, :), sim_data); U = u_discr'; - U_corr = u_corr'; U_track = u_track'; Q_pred(:, :, 1) = q_pred; - y = []; if eq(sim_data.robot, 0) fun = @(t, q, u_discr, sim_data) unicycle(t, q, u_discr, sim_data); elseif eq(sim_data.robot, 1) fun = @(t, q, u_discr, sim_data) diffdrive(t, q, u_discr, sim_data); end - - for n = 1:steps - sim_data.old_u_corr = u_corr; - sim_data.old_u_track = u_track; - sim_data.old_u = u_discr; + for n = 1:steps tspan = [(n-1)*tc n*tc]; z0 = q(end, :); @@ -140,21 +138,16 @@ function [t, q, y, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = sim q = [q; z]; t = [t; v]; - [u_discr, u_track, u_corr, U_corr_history, q_pred] = control_act(t(end), q(end, :), sim_data); - sim_data.U_corr_history = U_corr_history; - U = [U; ones(length(v), 1)*u_discr']; - U_corr = [U_corr; ones(length(v), 1)*u_corr']; + [u_discr, u_track, q_pred] = control_act(t(end), q(end, :), sim_data); + U = [U; ones(length(v), 1)*u_discr']; U_track = [U_track; ones(length(v), 1)*u_track']; Q_pred(:, :, 1+n) = q_pred; - - U_corr_pred_history(:,:,n) = permute(U_corr_history, [3, 1, 2]); - - y1 = q(:, 1) + sim_data.b * cos(q(:,3)); - y2 = q(:, 2) + sim_data.b * sin(q(:,3)); - y = [y; [y1, y2]]; end + y1 = q(:, 1) + sim_data.b * cos(q(:,3)); + y2 = q(:, 2) + sim_data.b * sin(q(:,3)); + y = [y1, y2]; + ref_t = double(subs(sim_data.ref, t'))'; end -%% diff --git a/tests/straightline/chill_errortheta_3deg/common.mat b/tests/straightline/chill_errortheta_3deg/common.mat new file mode 100644 index 0000000000000000000000000000000000000000..890569e9b13aa19c806caa6a896f786cb9225034 GIT binary patch literal 738 zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQgHY2i*PhE(NS}aZCnS7eNHSA+ z)^JJT0M}Dvr9?%AZ(lz1d@T9WS^n*NG2^B`f14Cv&1}%x(O|-`crxo>kY*LQW&>o+ zj)uwtRn0eb9K@KF8N}onc7oK}!PRmgt5r&@oN!AaT-^V;fPek|9|^}NTu^W~Fg|ks zxao3TmQyH<8b@_btcV-HqskPbE6I=DbOq?->WRx@ZsF^du~Ul61^yn9V?eZ