Skip to content
This repository has been archived by the owner on Jun 1, 2023. It is now read-only.

Commit

Permalink
grid costs, grid constraints
Browse files Browse the repository at this point in the history
  • Loading branch information
@jkoendev
jkoendev committed Jun 28, 2019
1 parent e30fac6 commit cf40761
Show file tree
Hide file tree
Showing 7 changed files with 68 additions and 68 deletions.
4 changes: 2 additions & 2 deletions +ocl/+examples/cartpole.m
View file
Expand Up @@ -4,7 +4,7 @@
%
function [sol,times,solver] = cartpole

solver = ocl.Solver([], 'vars', @varsfun, 'dae', @daefun, 'intervalcosts', @intervalcosts, 'N', 40, 'd', 3);
solver = ocl.Solver([], 'vars', @varsfun, 'dae', @daefun, 'gridcosts', @gridcosts, 'N', 40, 'd', 3);

p0 = 0; v0 = 0;
theta0 = 180*pi/180; omega0 = 0;
Expand Down Expand Up @@ -80,7 +80,7 @@ function daefun(sh,x,~,u,~)

end

function intervalcosts(self,k,K,x,~)
function gridcosts(self,k,K,x,~)
if k == K
self.add( x.time );
end
Expand Down
4 changes: 2 additions & 2 deletions +ocl/+examples/pendulum.m
View file
Expand Up @@ -8,7 +8,7 @@
solver = ocl.Solver([], ...
@ocl.examples.pendulum.varsfun, ...
@ocl.examples.pendulum.daefun, ...
@pathcosts, @intervalcosts, ...
@pathcosts, @gridcosts, ...
'callback_setup', @ocl.examples.pendulum.simcallbacksetup, ...
'callback', @ocl.examples.pendulum.simcallback, ...
'N', 40);
Expand Down Expand Up @@ -42,7 +42,7 @@
snapnow;
end

function intervalcosts(ch,k,K,x,~)
function gridcosts(ch,k,K,x,~)
if k==K
ch.add(1e-6*x.time);
end
Expand Down
6 changes: 3 additions & 3 deletions +ocl/+examples/racecar.m
View file
Expand Up @@ -10,7 +10,7 @@
MAX_TIME = 20;

solver = ocl.Solver([], @varsfun, @daefun, ...
'intervalcosts', @intervalcosts, 'intervalconstraints', @intervalconstraints, 'N', 50);
'gridcosts', @gridcosts, 'gridconstraints', @gridconstraints, 'N', 50);

% parameters
m = 1; % mass [kg]
Expand Down Expand Up @@ -134,13 +134,13 @@ function daefun(sh,x,~,u,p)
sh.setODE('time', 1);
end

function intervalcosts(ch,k,K,x,~)
function gridcosts(ch,k,K,x,~)
if k==K
ch.add(x.time);
end
end

function intervalconstraints(ch,~,~,x,p)
function gridconstraints(ch,~,~,x,p)
% speed constraint
ch.add(x.vx^2+x.vy^2, '<=', p.Vmax^2);

Expand Down
58 changes: 29 additions & 29 deletions +ocl/+simultaneous/equations.m
View file
Expand Up @@ -8,8 +8,8 @@
ni = stage.integrator.ni;
nu = stage.nu;
np = stage.np;
intervalcost_fun = @stage.intervalcostfun;
intervalcon_fun = @stage.intervalconstraintfun;
gridcost_fun = @stage.gridcostfun;
gridcon_fun = @stage.gridconstraintfun;

[~,N] = ocl.simultaneous.nvars(H_norm, nx, ni, nu, np);
[X_indizes, I_indizes, U_indizes, P_indizes, H_indizes] = ocl.simultaneous.getStageIndizes(stage);
Expand All @@ -20,33 +20,33 @@
P = reshape(stage_vars(P_indizes), np, N+1);
H = reshape(stage_vars(H_indizes), 1 , N);

% interval constraints, interval costs
intervalcon = cell(1,N+1);
intervalcon_lb = cell(1,N+1);
intervalcon_ub = cell(1,N+1);
intervalcost = 0;
% grid constraints, grid costs
gridcon = cell(1,N+1);
gridcon_lb = cell(1,N+1);
gridcon_ub = cell(1,N+1);
gridcost = 0;
for k=1:N+1
[intervalcon{k}, intervalcon_lb{k}, intervalcon_ub{k}] = intervalcon_fun(k, N+1, X(:,k), P(:,k));
intervalcost = intervalcost + intervalcost_fun(k, N+1, X(:,k), P(:,k));
[gridcon{k}, gridcon_lb{k}, gridcon_ub{k}] = gridcon_fun(k, N+1, X(:,k), P(:,k));
gridcost = gridcost + gridcost_fun(k, N+1, X(:,k), P(:,k));
end

intervalcon0 = intervalcon{1};
intervalcon0_lb = intervalcon_lb{1};
intervalcon0_ub = intervalcon_ub{1};
gridcon0 = gridcon{1};
gridcon0_lb = gridcon_lb{1};
gridcon0_ub = gridcon_ub{1};

intervalconF = intervalcon{end};
intervalconF_lb = intervalcon_lb{end};
intervalconF_ub = intervalcon_ub{end};
gridconF = gridcon{end};
gridconF_lb = gridcon_lb{end};
gridconF_ub = gridcon_ub{end};

intervalcon = horzcat(intervalcon{2:end-1});
intervalcon_lb = horzcat(intervalcon_lb{2:end-1});
intervalcon_ub = horzcat(intervalcon_ub{2:end-1});
gridcon = horzcat(gridcon{2:end-1});
gridcon_lb = horzcat(gridcon_lb{2:end-1});
gridcon_ub = horzcat(gridcon_ub{2:end-1});

% fix dimensions of empty path constraints
if isempty(intervalcon)
intervalcon = double.empty(0,N-1);
intervalcon_lb = double.empty(0,N-1);
intervalcon_ub = double.empty(0,N-1);
if isempty(gridcon)
gridcon = double.empty(0,N-1);
gridcon_lb = double.empty(0,N-1);
gridcon_ub = double.empty(0,N-1);
end

[xend_arr, cost_arr, int_eq_arr, int_times] = stage.integratormap(X(:,1:end-1), I, U, H, P(:,1:end-1));
Expand Down Expand Up @@ -75,18 +75,18 @@

% merge integrator equations, continuity, and path constraints,
% timesteps constraints
shooting_eq = [int_eq_arr(:,1:N-1); continuity(:,1:N-1); intervalcon; h_eq; p_eq(:,1:N-1)];
shooting_eq_lb = [zeros(ni,N-1); zeros(nx,N-1); intervalcon_lb; h_eq_lb; p_eq_lb(:,1:N-1)];
shooting_eq_ub = [zeros(ni,N-1); zeros(nx,N-1); intervalcon_ub; h_eq_ub; p_eq_ub(:,1:N-1)];
shooting_eq = [int_eq_arr(:,1:N-1); continuity(:,1:N-1); gridcon; h_eq; p_eq(:,1:N-1)];
shooting_eq_lb = [zeros(ni,N-1); zeros(nx,N-1); gridcon_lb; h_eq_lb; p_eq_lb(:,1:N-1)];
shooting_eq_ub = [zeros(ni,N-1); zeros(nx,N-1); gridcon_ub; h_eq_ub; p_eq_ub(:,1:N-1)];

% reshape shooting equations to column vector, append lastintegrator and
% continuity equations
constraints = [intervalcon0; shooting_eq(:); int_eq_arr(:,N); continuity(:,N); intervalconF; p_eq(:,N) ];
constraints_lb = [intervalcon0_lb; shooting_eq_lb(:); zeros(ni,1); zeros(nx,1); intervalconF_lb; p_eq_lb(:,N) ];
constraints_ub = [intervalcon0_ub; shooting_eq_ub(:); zeros(ni,1); zeros(nx,1); intervalconF_ub; p_eq_ub(:,N) ];
constraints = [gridcon0; shooting_eq(:); int_eq_arr(:,N); continuity(:,N); gridconF; p_eq(:,N) ];
constraints_lb = [gridcon0_lb; shooting_eq_lb(:); zeros(ni,1); zeros(nx,1); gridconF_lb; p_eq_lb(:,N) ];
constraints_ub = [gridcon0_ub; shooting_eq_ub(:); zeros(ni,1); zeros(nx,1); gridconF_ub; p_eq_ub(:,N) ];

% sum all costs
costs = sum(cost_arr) + intervalcost;
costs = sum(cost_arr) + gridcost;

% regularization on U
if controls_regularization && numel(U)>0
Expand Down
16 changes: 8 additions & 8 deletions +ocl/+test/testOclStage.m
View file
Expand Up @@ -3,25 +3,25 @@
% stage empty test
stage = OclStage(1, @emptyVars, @emptyDae);
assertEqual(stage.pathcostfun([],[],[],[]),0);
assertEqual(stage.intervalcostfun(1,10,[],[]),0);
assertEqual(stage.gridcostfun(1,10,[],[]),0);

[val,lb,ub] = stage.intervalconstraintfun(1,10,[],[]);
[val,lb,ub] = stage.gridconstraintfun(1,10,[],[]);
assertEqual(val,[]);
assertEqual(lb,[]);
assertEqual(ub,[]);

% stage valid test
stage = OclStage(1, @validVars, @validDae, ...
@validPathCosts, @validintervalCosts, @validintervalConstraints);
@validPathCosts, @validgridCosts, @validgridConstraints);

c = stage.pathcostfun(ones(stage.nx,1),ones(stage.nz,1),ones(stage.nu,1),ones(stage.np,1));
assertEqual(c,26+1e-3*12);

c = stage.intervalcostfun(5,5,ones(stage.nx,1),ones(stage.np,1));
c = stage.gridcostfun(5,5,ones(stage.nx,1),ones(stage.np,1));
assertEqual(c, -1);

% path constraints in the form of : -inf <= val <= 0 or 0 <= val <= 0
[val,lb,ub] = stage.intervalconstraintfun(2,5,ones(stage.nx,1),ones(stage.np,1));
[val,lb,ub] = stage.gridconstraintfun(2,5,ones(stage.nx,1),ones(stage.np,1));
% ub all zero
assertEqual(ub,zeros(36,1));
% lb either zero for eq or -inf for ineq
Expand All @@ -30,7 +30,7 @@
assertEqual(val,[0,0,0,0,0,-1,2,2,2,2,2,0,-3*ones(1,12),zeros(1,12)].');

% bc
[val,lb,ub] = stage.intervalconstraintfun(1,5,2*ones(stage.nx,1),ones(stage.np,1));
[val,lb,ub] = stage.gridconstraintfun(1,5,2*ones(stage.nx,1),ones(stage.np,1));
assertEqual(ub,zeros(3,1));
assertEqual(lb,[0,-inf,-inf].');
assertEqual(val,[-1,1,-4].');
Expand Down Expand Up @@ -106,14 +106,14 @@ function validPathCosts(ch,x,z,u,p)
ch.add(-1); % -1
end

function validintervalCosts(ch,k,N,x,p)
function validgridCosts(ch,k,N,x,p)
ch.add(x.d);
ch.add(0);
ch.add(-1);
ch.add(-1*p.v*1);
end

function validintervalConstraints(ch,k,N,x,p)
function validgridConstraints(ch,k,N,x,p)
if k == 1
ch.add(x.a,'==',3);
ch.add(x.a,'>=',3*1);
Expand Down
10 changes: 5 additions & 5 deletions Core/OclSolver.m
View file
Expand Up @@ -19,8 +19,8 @@
function self = OclSolver(varargin)
% OclSolver(T, 'vars', @varsfun, 'dae', @daefun,
% 'pathcosts', @pathcostfun,
% 'intervalcosts', @intervalcostfun,
% 'intervalconstraints', @intervalconstraintsfun, casadi_options)
% 'gridcosts', @gridcostfun,
% 'gridconstraints', @gridconstraintsfun, casadi_options)
% OclSolver(stages, transitions, casadi_options)

if isnumeric(varargin{1}) && isa(varargin{2}, 'OclSystem')
Expand All @@ -41,8 +41,8 @@
p.addKeyword('vars', emptyfh, @oclIsFunHandle);
p.addKeyword('dae', emptyfh, @oclIsFunHandle);
p.addKeyword('pathcosts', zerofh, @oclIsFunHandle);
p.addKeyword('intervalcosts', zerofh, @oclIsFunHandle);
p.addKeyword('intervalconstraints', emptyfh, @oclIsFunHandle);
p.addKeyword('gridcosts', zerofh, @oclIsFunHandle);
p.addKeyword('gridconstraints', emptyfh, @oclIsFunHandle);

p.addKeyword('callback', emptyfh, @oclIsFunHandle);
p.addKeyword('callback_setup', emptyfh, @oclIsFunHandle);
Expand All @@ -57,7 +57,7 @@

r = p.parse(varargin{:});

stageList = {OclStage(r.T, r.vars, r.dae, r.pathcosts, r.intervalcosts, r.intervalconstraints, ...
stageList = {OclStage(r.T, r.vars, r.dae, r.pathcosts, r.gridcosts, r.gridconstraints, ...
r.callback_setup, r.callback, 'N', r.N, 'd', r.d)};
transitionList = {};

Expand Down
38 changes: 19 additions & 19 deletions Core/OclStage.m
View file
Expand Up @@ -6,8 +6,8 @@
integrator

pathcostfun
intervalcostsfh
intervalconstraintsfh
gridcostsfh
gridconstraintsfh

callbacksetupfh
callbackfh
Expand Down Expand Up @@ -48,8 +48,8 @@
p.addKeyword('vars', emptyfh, @oclIsFunHandle);
p.addKeyword('dae', emptyfh, @oclIsFunHandle);
p.addKeyword('pathcosts', emptyfh, @oclIsFunHandle);
p.addKeyword('intervalcosts', emptyfh, @oclIsFunHandle);
p.addKeyword('intervalconstraints', emptyfh, @oclIsFunHandle);
p.addKeyword('gridcosts', emptyfh, @oclIsFunHandle);
p.addKeyword('gridconstraints', emptyfh, @oclIsFunHandle);

p.addKeyword('callbacksetup', emptyfh, @oclIsFunHandle);
p.addKeyword('callback', emptyfh, @oclIsFunHandle);
Expand All @@ -62,8 +62,8 @@
varsfhInput = r.vars;
daefhInput = r.dae;
pathcostsfhInput = r.pathcosts;
intervalcostsfhInput = r.intervalcosts;
intervalconstraintsfhInput = r.intervalconstraints;
gridcostsfhInput = r.gridcosts;
gridconstraintsfhInput = r.gridconstraints;

callbacksetupfh = r.callbacksetup;
callbackfh = r.callback;
Expand All @@ -83,7 +83,7 @@
H_normInput = H_normInput/sum(H_normInput);
oclWarning(['Timesteps given in pararmeter N are not normalized! ', ...
'N either be a scalar value or a normalized vector with the length ', ...
'of the number of control interval. Check the documentation of N. ', ...
'of the number of control grid. Check the documentation of N. ', ...
'Make sure the timesteps sum up to 1, and contain the relative ', ...
'length of the timesteps. OpenOCL normalizes the timesteps and proceeds.']);
end
Expand All @@ -96,8 +96,8 @@
self.H_norm = H_normInput;
self.integrator = colocation;
self.pathcostfun = @colocation.pathcostfun;
self.intervalcostsfh = intervalcostsfhInput;
self.intervalconstraintsfh = intervalconstraintsfhInput;
self.gridcostsfh = gridcostsfhInput;
self.gridconstraintsfh = gridconstraintsfhInput;

self.callbacksetupfh = callbacksetupfh;
self.callbackfh = callbackfh;
Expand Down Expand Up @@ -214,27 +214,27 @@ function setParameterBounds(self,id,varargin)
self.parameterBounds.upper = p_ub.value;
end

function r = intervalcostfun(self,k,N,x,p)
intervalCostHandler = OclCost();
function r = gridcostfun(self,k,N,x,p)
gridCostHandler = OclCost();

x = Variable.create(self.states,x);
p = Variable.create(self.parameters,p);

self.intervalcostsfh(intervalCostHandler,k,N,x,p);
self.gridcostsfh(gridCostHandler,k,N,x,p);

r = intervalCostHandler.value;
r = gridCostHandler.value;
end

function [val,lb,ub] = intervalconstraintfun(self,k,N,x,p)
intervalConHandler = OclConstraint();
function [val,lb,ub] = gridconstraintfun(self,k,N,x,p)
gridConHandler = OclConstraint();
x = Variable.create(self.states,x);
p = Variable.create(self.parameters,p);

self.intervalconstraintsfh(intervalConHandler,k,N,x,p);
self.gridconstraintsfh(gridConHandler,k,N,x,p);

val = intervalConHandler.values;
lb = intervalConHandler.lowerBounds;
ub = intervalConHandler.upperBounds;
val = gridConHandler.values;
lb = gridConHandler.lowerBounds;
ub = gridConHandler.upperBounds;
end

function callbacksetupfun(self)
Expand Down

0 comments on commit cf40761

Please sign in to comment.