collocation package, functions, point costs wip

+ocl/+collocation/coefficients.m

@@ -0,0 +1,15 @@
+function coeff = coefficients(tau_root)
+d = length(tau_root)-1; % order
+coeff = zeros(d, d+1);
+for j=1:d+1
+  % Construct Lagrange polynomials to get the polynomial 
+  % basis at the collocation point
+  basis = 1;
+  for r=1:d+1
+    if r ~= j
+      basis = conv(basis, [1, -tau_root(r)]);
+      basis = basis / (tau_root(j)-tau_root(r));
+    end
+  end
+  coeff(j,:) = basis;
+end

+ocl/+collocation/collocationPoints.m

@@ -0,0 +1,12 @@
+function [ points ] = collocationPoints( d )
+if d == 2
+  points = [0 0.33333333333333333333333333333333, 1.0];
+elseif d == 3
+  points = [0 0.15505102572168222296866701981344, 0.64494897427831787695140519645065, 1.0];
+elseif d == 4
+  points = [0 0.088587959512704206321842548277345, 0.4094668644407346569380479195388, 0.7876594617608470016989485884551, 1.0];
+elseif d == 5
+  points = [0 0.057104196114518224192124762339517, 0.27684301363812369167760607524542, 0.5835904323689168338162858162832, 0.86024013565621926247217743366491, 1.0];
+else
+  error('Only collocation order between 2 and 5 is supported.');
+end

+ocl/+collocation/evalCoefficients.m

@@ -0,0 +1,8 @@
+function r = evalCoefficients(coeff, d, point)
+
+oclAssert(point<=1 && point >=0);
+
+r = zeros(d+1, 1);
+for j=1:d+1
+  r(j) = polyval(coeff(j,:), point);
+end

+ocl/+collocation/evalCoefficientsDerivative.m

@@ -0,0 +1,8 @@
+function r = evalCoefficientsDerivative(coeff, tau_root, d)
+r = zeros(d+1, d+1);
+for k=1:d+1
+  pder = polyder(coeff(k,:));
+  for j=1:d+1
+    r(k,j) = polyval(pder, tau_root(j));
+  end
+end

+ocl/+collocation/evalCoefficientsIntegral.m

@@ -0,0 +1,11 @@
+function r = evalCoefficientsIntegral(coeff, d, point)
+
+oclAssert(point<=1 && point >=0);
+
+r = zeros(d+1, 1);
+for k=1:d+1
+  % Evaluate the integral of the polynomial to get 
+  % the coefficients of the quadrature function
+  p_int = polyint(coeff(k,:));
+  r(k) = polyval(p_int, point);
+end

+ocl/+collocation/getStateAtPoint.m

@@ -0,0 +1,16 @@
+function x = getStateAtPoint(colloc, x0, vars, point)
+
+d = colloc.order;
+nx = colloc.num_x;
+nz = colloc.num_z;
+coeff = colloc.coefficients;
+
+coeff_eval_point = ocl.collocation.evalCoefficients(coeff, d, point);
+
+x = coeff_eval_point(1)*x0;
+
+for j=1:length(d)
+  j_vars = (j-1)*(nx+nz);
+  j_x = j_vars+1:j_vars+nx;
+  x = x + coeff_eval_point(j+1)*vars(j_x);
+end

+ocl/+simultaneous/equations.m

@@ -5,7 +5,7 @@
 H_norm = stage.H_norm;
 T = stage.T;
 nx = stage.nx;
-ni = stage.integrator.ni;
+ni = stage.integrator.num_i;
 nu = stage.nu;
 np = stage.np;
 gridcost_fun = @stage.gridcostfun;
@@ -30,6 +30,19 @@
   gridcost = gridcost + gridcost_fun(k, N+1, X(:,k), P(:,k));
 end
 
+% point costs
+grid = ocl.simultaneous.normalizedStateTimes(stage);
+grid_N = grid(1:end-1,:);
+grid_collocation = ocl.simultaneous.normalizedIntegratorTimes(stage);
+
+grid_merged = [grid_N,grid_collocation];
+
+for k=1:length(stage.pointcostsarray)
+  point = stage.pointcostsarray{k}.point;
+  fh = stage.pointcostsarray{k}.fh;
+
+end
+
 gridcon0 = gridcon{1};
 gridcon0_lb = gridcon_lb{1};
 gridcon0_ub = gridcon_ub{1};

+ocl/+simultaneous/getBounds.m

@@ -1,6 +1,6 @@
 function [lb_stage,ub_stage] = getBounds(stage)
 
-[nv_stage,~] = ocl.simultaneous.nvars(stage.H_norm, stage.nx, stage.integrator.ni, stage.nu, stage.np);
+[nv_stage,~] = ocl.simultaneous.nvars(stage.H_norm, stage.nx, stage.integrator.num_i, stage.nu, stage.np);
 
 lb_stage = -inf * ones(nv_stage,1);
 ub_stage = inf * ones(nv_stage,1);

+ocl/+simultaneous/getInitialGuess.m

@@ -2,7 +2,7 @@
 % creates an initial guess from the information that we have about
 % bounds in the stage
 
-[nv_stage,N] = ocl.simultaneous.nvars(stage.H_norm, stage.nx, stage.integrator.ni, stage.nu, stage.np);
+[nv_stage,N] = ocl.simultaneous.nvars(stage.H_norm, stage.nx, stage.integrator.num_i, stage.nu, stage.np);
 
 [X_indizes, I_indizes, U_indizes, P_indizes, H_indizes] = ocl.simultaneous.getStageIndizes(stage);
 

+ocl/+simultaneous/getInitialGuessWithUserData.m

@@ -19,6 +19,8 @@
   ocl.types.variable.setFromNdMatrix(ig_stage.states.get(id), ytarget);
 
   xtarget = ocl.simultaneous.normalizedIntegratorTimes(stage);
+  xtarget = xtarget(:);
+
   ytarget = interp1(xdata,ydata,xtarget,'linear','extrap');
 
   ocl.types.variable.setFromNdMatrix(ig_stage.integrator.states.get(id), ytarget);

+ocl/+simultaneous/getStageIndizes.m

@@ -2,7 +2,7 @@
 
 N = length(stage.H_norm);
 nx = stage.nx;
-ni = stage.integrator.ni;
+ni = stage.integrator.num_i;
 nu = stage.nu;
 np = stage.np;
 

+ocl/+simultaneous/normalizedIntegratorTimes.m

@@ -2,11 +2,10 @@
 H_norm = stage.H_norm;
 integrator = stage.integrator;
 
-r = zeros(length(H_norm), integrator.nt);
+r = zeros(length(H_norm), integrator.num_t);
 time = 0;
 for k=1:length(H_norm)
   h = H_norm(k);
   r(k,:) = time + h * stage.integrator.normalized_times();
   time = time + H_norm(k);
 end
-r = reshape(r, length(H_norm) * integrator.nt, 1);

+ocl/+simultaneous/times.m

@@ -1,5 +1,5 @@
 function stage_time_struct = times(stage)
 stage_time_struct = OclStructure();
 stage_time_struct.addRepeated({'states', 'integrator', 'controls'}, ...
-  {OclMatrix([1,1]), OclMatrix([stage.integrator.nt,1]), OclMatrix([1,1])}, length(stage.H_norm));
+  {OclMatrix([1,1]), OclMatrix([stage.integrator.num_t,1]), OclMatrix([1,1])}, length(stage.H_norm));
 stage_time_struct.add('states', OclMatrix([1,1]));

+ocl/Pointcost.m

@@ -0,0 +1,17 @@
+classdef Pointcost < handle
+
+  properties
+    point
+    fh
+  end
+
+  methods
+
+    function self = Pointcost(point, fh)
+      self.point = point;
+      self.fh = fh;
+    end
+
+  end
+
+end

CasadiLibrary/CasadiSolver.m

@@ -50,7 +50,7 @@
         stage = stageList{k};
 
         x = expr(['x','_s',mat2str(k)], stage.nx);
-        vi = expr(['vi','_s',mat2str(k)], stage.integrator.ni);
+        vi = expr(['vi','_s',mat2str(k)], stage.integrator.num_i);
         u = expr(['u','_s',mat2str(k)], stage.nu);
         h = expr(['h','_s',mat2str(k)]);
         p = expr(['p','_s',mat2str(k)], stage.np);
@@ -60,7 +60,7 @@
 
         stage.integratormap = integrator_fun.map(stage.N,'serial');
 
-        nv_stage = ocl.simultaneous.nvars(stage.H_norm, stage.nx, stage.integrator.ni, stage.nu, stage.np);
+        nv_stage = ocl.simultaneous.nvars(stage.H_norm, stage.nx, stage.integrator.num_i, stage.nu, stage.np);
         v_last_stage = v_stage;
         v_stage = expr(['v','_s',mat2str(k)], nv_stage);
 
@@ -174,7 +174,7 @@
       i = 1;
       for k=1:length(pl)
         stage = pl{k};
-        nv_stage = ocl.simultaneous.nvars(stage.H_norm, stage.nx, stage.integrator.ni, stage.nu, stage.np);
+        nv_stage = ocl.simultaneous.nvars(stage.H_norm, stage.nx, stage.integrator.num_i, stage.nu, stage.np);
         sol{k} = sol_values(i:i+nv_stage-1);
         i = i + nv_stage;
       end