changed truncation direction

Author: kubitz

FunctionTemplate.asv

@@ -8,27 +8,34 @@ param.K = [2, 0, 0, 0, 0, 0, 0, 0;
 % This is a sample way to send reference points
 param.xTar = shape.target(1);
 param.yTar = shape.target(2);
+param.target=shape.target;r = zeros(8,1);
 
-param.target=shape.target;
+% Make the crane go to (xTar, yTar)
+r=zeros(8,1); r(1,1) = param.xTar; r(3,1) = param.yTar;
+param.targetMod=r;
 param.start=shape.start;
 param.constraints=shape.constraints;
 param.eps_r=shape.eps_r;
 param.eps_t=shape.eps_t;
 param.Wmax=shape.Wmax;
 param.Tf=shape.Tf;
 param.angleConstraint=3*pi/180;
-param.Ts = 0.1;
+param.Ts = 0.05;
 param.Tf=shape.Tf;
 param.useShrinkingHorizon=true;
 param.N=ceil(param.Tf/param.Ts)
+
 % Declare penalty matrices:
-Qt = [100 10 100 10 100 10 100 10];
+Qt = [100 1 100 1 100 10 100 10];
 Pt = [100 10 100 10 100 10 100 10];
 Rt = [1 1];
 
 param.Q = 1*diag(Qt);
 param.P = 100*diag(Pt);
 param.R = 1*diag(Rt);
+param.numStates=size(param.Q,1);
+param.numInputs=size(param.R,1);
+
 
 load('Crane_NominalParameters.mat');
 [param.A,param.B,param.C,~] = genCraneODE(m,M,MR,r,9.81,Tx,Ty,Vx,Vy,param.Ts);
@@ -103,7 +110,7 @@ end % End of myStateEstimator
 %% Modify the following function for your controller
 function u = myMPController(r, x_hat, param)
 %% Do not delete this line
-u=zeros(2,1);
+u=zeros(param.numInputs,1);
 % Create the output array of the appropriate size
 persistent it EE DD Gamma Phi bb
 if isempty(it)
@@ -119,28 +126,25 @@ if isempty(it)
     H=param.H;
     G=param.G;
 else
-    % Shave off top of matrices as the horizon recedes
-    DD=reduceMatrix(DD,param.numConstraints,8);
-    EE=reduceMatrix(EE,param.numConstraints,2);
+    % Shave off matrices as the horizon shrinks
+    DD=reduceMatrix(DD,param.numConstraints,param.numStates);
+    EE=reduceMatrix(EE,param.numConstraints,param.numInputs);
     bb=reduceMatrix(bb,param.numConstraints,0);
-    Gamma=reduceMatrix(Gamma,8,2);
-    Phi=reduceMatrix(Phi,8,0);
+    Gamma=reduceMatrix(Gamma,param.numStates,param.numInputs);
+    Phi=reduceMatrix(Phi,param.numStates,0);
 end 
 
-if (param.useShrinkingHorizon==true)
+if (param.useShrinkingHorizon==true) && (it<param.N)
     N=param.N-it;
     [F,J,L]=genConstraintMatrices(DD,EE,Gamma,Phi,N);
     [H,G] = genCostMatrices(Gamma,Phi,param.Q,param.R,param.P,N);
-    [Lchol , p] = chol(H,'lower');
-    Linv = linsolve(Lchol,eye(size(Lchol)),struct('LT',true)); 
     u = genMPController(H,G,F,bb,J,L,x_hat,r,2);
     it=it+1;
 end 
 end % End of myMPController
 
 function [F,J,L] = genConstraintMatrices(DD,EE,Gamma,Phi,N)
 sx = size(Gamma,1) / N;
-sx=8;
 shift = [zeros(sx, sx*(N-1));eye(sx*(N-1))]*[eye(sx*(N-1)), zeros(sx*(N-1),sx)];
 F = DD*shift*Gamma + EE;
 J = -DD*(shift*Phi + [eye(sx);zeros(sx*(N-1), sx)]);
@@ -263,7 +267,7 @@ function [u,status,iA1] = genMPController(H,G,F,bb,J,L,x,xTarget,m)
     opt.IntegrityChecks = false;%% for code generation
     opt.ConstraintTolerance = 1e-3;
     opt.DataType = 'double';
-    opt.UseHessianAsInput = false;
+    opt.UseHessianAsInput = true;
 
 
     % Compute the linear term of the cost function

FunctionTemplate.m

@@ -8,8 +8,11 @@
 % This is a sample way to send reference points
 param.xTar = shape.target(1);
 param.yTar = shape.target(2);
+param.target=shape.target;r = zeros(8,1);
 
-param.target=shape.target;
+% Make the crane go to (xTar, yTar)
+r=zeros(8,1); r(1,1) = param.xTar; r(3,1) = param.yTar;
+param.targetMod=r;
 param.start=shape.start;
 param.constraints=shape.constraints;
 param.eps_r=shape.eps_r;
@@ -124,11 +127,11 @@
     G=param.G;
 else
     % Shave off matrices as the horizon shrinks
-    DD=reduceMatrix(DD,param.numConstraints,param.numStates);
-    EE=reduceMatrix(EE,param.numConstraints,param.numInputs);
-    bb=reduceMatrix(bb,param.numConstraints,0);
-    Gamma=reduceMatrix(Gamma,param.numStates,param.numInputs);
-    Phi=reduceMatrix(Phi,param.numStates,0);
+    DD=reduceMatrixTop(DD,param.numConstraints,param.numStates);
+    EE=reduceMatrixTop(EE,param.numConstraints,param.numInputs);
+    bb=reduceMatrixTop(bb,param.numConstraints,0);
+    Gamma=reduceMatrixBottom(Gamma,param.numStates,param.numInputs);
+    Phi=reduceMatrixBottom(Phi,param.numStates,0);
 end 
 
 if (param.useShrinkingHorizon==true) && (it<param.N)
@@ -335,10 +338,13 @@
 
 end
 
-function reducedMatrix = reduceMatrix(A,nRows,nCols)
-reducedMatrix=A(1:size(A,1)-nRows,1:size(A,2)-nCols);
+function reducedMatrix = reduceMatrixTop(A,nRows,nCols)
+reducedMatrix=A(nRows+1:size(A,1),nCols+1:size(A,2));
 end
 
+function reducedMatrix = reduceMatrixBottom(A,nRows,nCols)
+reducedMatrix=A(1:size(A,1)-nRows,1:size(A,2)-nCols);
+end
 
 function [A,B,C] = getLineParamsStd(pointA,pointB)
 % Get line parameters in standard form Ax+By=C

basicMPCFunctions/RectConstraints.mlx

@@ -17,11 +17,11 @@
     G=param.G;
 else
     % Shave off matrices as the horizon shrinks
-    DD=reduceMatrix(DD,param.numConstraints,param.numStates);
-    EE=reduceMatrix(EE,param.numConstraints,param.numInputs);
-    bb=reduceMatrix(bb,param.numConstraints,0);
-    Gamma=reduceMatrix(Gamma,param.numStates,param.numInputs);
-    Phi=reduceMatrix(Phi,param.numStates,0);
+    DD=reduceMatrixTop(DD,param.numConstraints,param.numStates);
+    EE=reduceMatrixTop(EE,param.numConstraints,param.numInputs);
+    bb=reduceMatrixTop(bb,param.numConstraints,0);
+    Gamma=reduceMatrixBottom(Gamma,param.numStates,param.numInputs);
+    Phi=reduceMatrixBottom(Phi,param.numStates,0);
 end 
 
 if (param.useShrinkingHorizon==true) && (it<param.N)

myMPController.m

@@ -8,8 +8,11 @@
 % This is a sample way to send reference points
 param.xTar = shape.target(1);
 param.yTar = shape.target(2);
+param.target=shape.target;r = zeros(8,1);
 
-param.target=shape.target;
+% Make the crane go to (xTar, yTar)
+r=zeros(8,1); r(1,1) = param.xTar; r(3,1) = param.yTar;
+param.targetMod=r;
 param.start=shape.start;
 param.constraints=shape.constraints;
 param.eps_r=shape.eps_r;

mySetup.m

@@ -1,5 +1,5 @@
 function reducedMatrix = reduceMatrix(A,nRows,nCols)
-reducedMatrix=A(1:size(A,1)-nRows,1:size(A,2)-nCols);
+reducedMatrix=A(nRows+1:size(A,1),nCols+1:size(A,2));
 end
 
 

reduceMatrix.m

@@ -0,0 +1,4 @@
+function reducedMatrix = reduceMatrixBottom(A,nRows,nCols)
+reducedMatrix=A(1:size(A,1)-nRows,1:size(A,2)-nCols);
+end
+

reduceMatrixBottom.m

@@ -0,0 +1,4 @@
+function reducedMatrix = reduceMatrixTop(A,nRows,nCols)
+reducedMatrix=A(nRows+1:size(A,1),nCols+1:size(A,2));
+end
+