abbandon correction, linearize around trajectory
try three different approaches, with questionable resultsmaster-costfun2
parent
eee95b0059
commit
5e77c3beed
262
control_act.m
262
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)
|
||||
|
|
|
|||
37
plot_all.m
37
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'])
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
51
tesi.m
51
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
|
||||
|
||||
%%
|
||||
|
|
|
|||
Binary file not shown.
Loading…
Reference in New Issue