Skip to content

Commit 5c7101f

Browse files
kubitzkubitz
committed
added position constraints
1 parent b10626f commit 5c7101f

File tree

6 files changed

+53
-32
lines changed

6 files changed

+53
-32
lines changed

‎FunctionTemplate.asv

Lines changed: 16 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -26,9 +26,9 @@ param.useShrinkingHorizon=true;
2626
param.N=ceil(param.Tf/param.Ts)
2727

2828
% Declare penalty matrices:
29-
Qt = [100 1 100 1 100 10 100 10];
29+
Qt = [0 0 0 0 0 0 0 0];
3030
Pt = [100 10 100 10 100 10 100 10];
31-
Rt = [1 1];
31+
Rt = [0.1 0.1];
3232

3333
param.Q = 1*diag(Qt);
3434
param.P = 100*diag(Pt);
@@ -57,6 +57,10 @@ uh=[1; 1];
5757
param.numConstraints=size(Dt,1);
5858
%% Compute trajectory constraints matrices and vector
5959
[param.DD,param.EE,param.bb]=genTrajectoryConstraints(Dt,Et,bt,param.N);
60+
sDD=size(param.DD,2);
61+
param.DD=[param.DD;zeros(8,sDD-8),eye(8);zeros(8,sDD-8),-eye(8)];
62+
param.EE=[param.EE;zeros(16,size(param.EE,2))];
63+
param.bb=[param.bb;param.targetMod+param.eps_r;-param.targetMod+param.eps_r];
6064

6165
%% Compute QP constraint matrices
6266
[param.Gamma,param.Phi] = genPrediction(param.A,param.B,param.N);
@@ -127,11 +131,11 @@ if isempty(it)
127131
G=param.G;
128132
else
129133
% Shave off matrices as the horizon shrinks
130-
DD=reduceMatrix(DD,param.numConstraints,param.numStates);
131-
EE=reduceMatrix(EE,param.numConstraints,param.numInputs);
132-
bb=reduceMatrix(bb,param.numConstraints,0);
133-
Gamma=reduceMatrix(Gamma,param.numStates,param.numInputs);
134-
Phi=reduceMatrix(Phi,param.numStates,0);
134+
DD=reduceMatrixTop(DD,param.numConstraints,param.numStates);
135+
EE=reduceMatrixTop(EE,param.numConstraints,param.numInputs);
136+
bb=reduceMatrixTop(bb,param.numConstraints,0);
137+
Gamma=reduceMatrixBottom(Gamma,param.numStates,param.numInputs);
138+
Phi=reduceMatrixBottom(Phi,param.numStates,0);
135139
end
136140

137141
if (param.useShrinkingHorizon==true) && (it<param.N)
@@ -338,10 +342,13 @@ chRect=[max(c1,c2);max(c3,c4)];
338342

339343
end
340344

341-
function reducedMatrix = reduceMatrix(A,nRows,nCols)
342-
reducedMatrix=A(1:size(A,1)-nRows,1:size(A,2)-nCols);
345+
function reducedMatrix = reduceMatrixTop(A,nRows,nCols)
346+
reducedMatrix=A(nRows+1:end,nCols+1:end);
343347
end
344348

349+
function reducedMatrix = reduceMatrixBottom(A,nRows,nCols)
350+
reducedMatrix=A(1:end-nRows,1:end-nCols);
351+
end
345352

346353
function [A,B,C] = getLineParamsStd(pointA,pointB)
347354
% Get line parameters in standard form Ax+By=C

‎FunctionTemplate.m

Lines changed: 9 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -26,9 +26,10 @@
2626
param.N=ceil(param.Tf/param.Ts)
2727

2828
% Declare penalty matrices:
29-
Qt = [100 1 100 1 100 10 100 10];
29+
Qt = [0 2 0 2 5 0 5 0];
3030
Pt = [100 10 100 10 100 10 100 10];
31-
Rt = [1 1];
31+
Pt = [0 0 0 0 0 0 0 0];
32+
Rt = [0.1 0.1];
3233

3334
param.Q = 1*diag(Qt);
3435
param.P = 100*diag(Pt);
@@ -57,6 +58,10 @@
5758
param.numConstraints=size(Dt,1);
5859
%% Compute trajectory constraints matrices and vector
5960
[param.DD,param.EE,param.bb]=genTrajectoryConstraints(Dt,Et,bt,param.N);
61+
sDD=size(param.DD,2);
62+
param.DD=[param.DD;zeros(8,sDD-8),eye(8);zeros(8,sDD-8),-eye(8)];
63+
param.EE=[param.EE;zeros(16,size(param.EE,2))];
64+
param.bb=[param.bb;param.targetMod+0.5*param.eps_r;-param.targetMod+0.5*param.eps_r];
6065

6166
%% Compute QP constraint matrices
6267
[param.Gamma,param.Phi] = genPrediction(param.A,param.B,param.N);
@@ -339,11 +344,11 @@
339344
end
340345

341346
function reducedMatrix = reduceMatrixTop(A,nRows,nCols)
342-
reducedMatrix=A(nRows+1:size(A,1),nCols+1:size(A,2));
347+
reducedMatrix=A(nRows+1:end,nCols+1:end);
343348
end
344349

345350
function reducedMatrix = reduceMatrixBottom(A,nRows,nCols)
346-
reducedMatrix=A(1:size(A,1)-nRows,1:size(A,2)-nCols);
351+
reducedMatrix=A(1:end-nRows,1:end-nCols);
347352
end
348353

349354
function [A,B,C] = getLineParamsStd(pointA,pointB)

‎defaultCourse.m

Lines changed: 19 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -3,38 +3,42 @@
33

44
if coursenum == 1
55
% The shape for the first part
6-
constraints.rect = [0.00, 0.05;
7-
0.45, 0.50;
8-
0.50, 0.45;
9-
0.05, 0.00];
6+
% constraints.rect = [0.00, 0.05;
7+
% 0.45, 0.50;
8+
% 0.50, 0.45;
9+
% 0.05, 0.00];
1010

1111
constraints.ellipses = {};
1212

1313
penalties = [-1, -2, -1, -1];
1414

15-
start = [0.05, 0.05];
16-
target = [0.4, 0.4];
17-
15+
% start = [0.05, 0.05];
16+
% target = [0.4, 0.4];
17+
constraints.rect=[-0.0819,-0.493;0.0917,0.492;0.0819,0.493;-0.0917,-0.492];
18+
start= [0.0917481275985262, 0.49153563561776936];
19+
target= [-0.0819,-0.493];
1820
Wmax = 1;
1921
Tf = 5;
2022
elseif coursenum == 2
2123
% The shape for the second part
22-
constraints.rect = [0.00, 0.05;
23-
0.25, 0.30;
24-
0.50, 0.05;
25-
0.25, -0.20];
26-
24+
% constraints.rect = [0.00, 0.05;
25+
% 0.25, 0.30;
26+
% 0.50, 0.05;
27+
% 0.25, -0.20];
28+
constraints.rect=[-0.0819,-0.493;0.0917,0.492;0.0819,0.493;-0.0917,-0.492];
29+
start= [0.0917481275985262, 0.49153563561776936];
30+
target= [0.08190005006840413, 0.49327211739443866];
2731
ellipse.a = 0.3;
2832
ellipse.b = 0.3;
2933
ellipse.xc = 0.25;
3034
ellipse.yc = -0.20;
3135

32-
constraints.ellipses{1} = ellipse;
36+
constraints.ellipses = {};
3337

3438
penalties = [-1, -1, -1, -1, -1];
3539

36-
start = [0.05, 0.05];
37-
target = [0.45, 0.05];
40+
% start = [0.05, 0.05];
41+
% target = [0.45, 0.05];
3842

3943
Wmax = 1;
4044
Tf = 5;

‎mySetup.m

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -26,9 +26,10 @@
2626
param.N=ceil(param.Tf/param.Ts)
2727

2828
% Declare penalty matrices:
29-
Qt = [100 1 100 1 100 10 100 10];
29+
Qt = [0 2 0 2 5 0 5 0];
3030
Pt = [100 10 100 10 100 10 100 10];
31-
Rt = [1 1];
31+
Pt = [0 0 0 0 0 0 0 0];
32+
Rt = [0.1 0.1];
3233

3334
param.Q = 1*diag(Qt);
3435
param.P = 100*diag(Pt);
@@ -57,6 +58,10 @@
5758
param.numConstraints=size(Dt,1);
5859
%% Compute trajectory constraints matrices and vector
5960
[param.DD,param.EE,param.bb]=genTrajectoryConstraints(Dt,Et,bt,param.N);
61+
sDD=size(param.DD,2);
62+
param.DD=[param.DD;zeros(8,sDD-8),eye(8);zeros(8,sDD-8),-eye(8)];
63+
param.EE=[param.EE;zeros(16,size(param.EE,2))];
64+
param.bb=[param.bb;param.targetMod+0.5*param.eps_r;-param.targetMod+0.5*param.eps_r];
6065

6166
%% Compute QP constraint matrices
6267
[param.Gamma,param.Phi] = genPrediction(param.A,param.B,param.N);

‎reduceMatrixBottom.m

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
11
function reducedMatrix = reduceMatrixBottom(A,nRows,nCols)
2-
reducedMatrix=A(1:size(A,1)-nRows,1:size(A,2)-nCols);
2+
reducedMatrix=A(1:end-nRows,1:end-nCols);
33
end
44

‎reduceMatrixTop.m

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
11
function reducedMatrix = reduceMatrixTop(A,nRows,nCols)
2-
reducedMatrix=A(nRows+1:size(A,1),nCols+1:size(A,2));
2+
reducedMatrix=A(nRows+1:end,nCols+1:end);
33
end
44

0 commit comments

Comments
 (0)