Control.Mpc.MultipleShooting.solveTick

Parameters, inputs, outputs, defaults and equations from the source file.

function solveTick
    "Warm-shift the horizon, then condense and take one Gauss-Newton step"
    input OcpSpec ocp;
    input Real currentState[:];
    input Real reference[size(currentState, 1)];
    input Real stateWeight[size(currentState, 1)];
    input Real terminalWeight[size(currentState, 1)];
    input Real inputWeight[:];
    input Real controlsPrev[:, size(inputWeight, 1)];
    input Real nodesPrev[size(controlsPrev, 1), size(currentState, 1)];
    input Real stateJacobians[
      size(controlsPrev, 1) * size(currentState, 1), size(currentState, 1)];
    input Real inputJacobians[
      size(controlsPrev, 1) * size(currentState, 1), size(inputWeight, 1)];
    input Real nominalNodes[size(controlsPrev, 1), size(currentState, 1)];
    output Real controlsNext[size(controlsPrev, 1), size(controlsPrev, 2)];
    output Real nodesNext[size(controlsPrev, 1), size(currentState, 1)];
    output Real commandOut[size(controlsPrev, 2)];
    output Boolean solveAccepted;
    output Real cost;
  protected
    Real jacobianMatrix[
      size(controlsPrev, 1) * (size(currentState, 1) + size(controlsPrev, 2)),
      size(controlsPrev, 1) * size(controlsPrev, 2)];
    Real residualVector[
      size(controlsPrev, 1) * (size(currentState, 1) + size(controlsPrev, 2))];
    Real sensitivity[size(currentState, 1),
      size(controlsPrev, 1) * size(controlsPrev, 2)]
      "Running condensed sensitivity dx_k/du of the whole control stack";
    Real sensitivityNew[size(currentState, 1),
      size(controlsPrev, 1) * size(controlsPrev, 2)];
    Real sensitivityAll[size(controlsPrev, 1) * size(currentState, 1),
      size(controlsPrev, 1) * size(controlsPrev, 2)];
    Real carry[size(currentState, 1)]
      "Running condensed defect contribution to dx_k at du = 0";
    Real carryNew[size(currentState, 1)];
    Real carryAll[size(controlsPrev, 1) * size(currentState, 1)];
    Real stateJacobianBlock[size(currentState, 1), size(currentState, 1)];
    Real inputJacobianBlock[size(currentState, 1), size(controlsPrev, 2)];
    Real defect[size(currentState, 1)];
    Real weightRow[size(currentState, 1)];
    Real step[size(controlsPrev, 1) * size(controlsPrev, 2)];
    Real weightSqrt;
    Real accumulator;
    Real stepScale;
    Boolean stepAccepted;
    Integer nx;
    Integer nu;
    Integer horizon;
  algorithm
    nx := size(currentState, 1);
    horizon := size(controlsPrev, 1);
    nu := size(controlsPrev, 2);

    // Warm start: shift the previous solution one interval and repeat the
    // final node. This must stay consistent with the linearization query
    // points published in the equation section, because the provided
    // Jacobians and nominal values are evaluated exactly there.
    controlsNext := controlsPrev;
    nodesNext := nodesPrev;
    for k in 1:(horizon - 1) loop
      controlsNext[k, :] := controlsPrev[k + 1, :];
      nodesNext[k, :] := nodesPrev[k + 1, :];
    end for;

    // Define every working variable before the loops; the checked DAE
    // constructor requires an initial value for everything a loop reads.
    weightSqrt := 0.0;
    accumulator := 0.0;
    stepScale := 0.0;
    stepAccepted := true;
    carry := zeros(size(currentState, 1));
    carryNew := zeros(size(currentState, 1));
    defect := zeros(size(currentState, 1));
    weightRow := zeros(size(currentState, 1));
    stateJacobianBlock := zeros(size(currentState, 1), size(currentState, 1));
    inputJacobianBlock := zeros(size(currentState, 1), size(controlsPrev, 2));
    sensitivity := zeros(size(currentState, 1),
      size(controlsPrev, 1) * size(controlsPrev, 2));
    sensitivityNew := zeros(size(currentState, 1),
      size(controlsPrev, 1) * size(controlsPrev, 2));
    sensitivityAll := zeros(size(controlsPrev, 1) * size(currentState, 1),
      size(controlsPrev, 1) * size(controlsPrev, 2));
    carryAll := zeros(size(controlsPrev, 1) * size(currentState, 1));
    step := zeros(size(controlsPrev, 1) * size(controlsPrev, 2));
    residualVector := zeros(
      size(controlsPrev, 1) * (size(currentState, 1) + size(controlsPrev, 2)));
    jacobianMatrix := zeros(
      size(controlsPrev, 1) * (size(currentState, 1) + size(controlsPrev, 2)),
      size(controlsPrev, 1) * size(controlsPrev, 2));

    // Forward pass over the shooting intervals: condensation of the
    // linearized continuity dx_(k+1) = A_k dx_k + B_k du_k + d_k with
    // dx_0 = 0 into sensitivity and carry, and assembly of the weighted
    // tracking residual and its Jacobian. The residual system is built by
    // accumulation into zeroed arrays and whole-array matrix products;
    // scalar-accumulation loops at this nesting depth are miscompiled by
    // the current compiler, and the matrix form is clearer anyway.
    for k in 1:horizon loop
      // Accumulation into freshly zeroed blocks; a plain elementwise copy
      // loop here is miscompiled by the current compiler.
      stateJacobianBlock := zeros(size(currentState, 1), size(currentState, 1));
      inputJacobianBlock := zeros(size(currentState, 1), size(controlsPrev, 2));
      for i in 1:nx loop
        stateJacobianBlock[i, :] := stateJacobianBlock[i, :]
          + stateJacobians[(k - 1) * nx + i, :];
        inputJacobianBlock[i, :] := inputJacobianBlock[i, :]
          + inputJacobians[(k - 1) * nx + i, :];
      end for;
      defect := nominalNodes[k, :] - nodesNext[k, :];

      carryNew := stateJacobianBlock * carry + defect;
      sensitivityNew := stateJacobianBlock * sensitivity;
      carry := carryNew;
      sensitivity := sensitivityNew;
      for i in 1:nx loop
        for j in 1:nu loop
          sensitivity[i, (k - 1) * nu + j] :=
            sensitivity[i, (k - 1) * nu + j] + inputJacobianBlock[i, j];
        end for;
      end for;

      weightRow := if k == horizon then terminalWeight else stateWeight;
      for i in 1:nx loop
        weightSqrt := sqrt(max(weightRow[i], 0.0));
        carryAll[(k - 1) * nx + i] :=
          carryAll[(k - 1) * nx + i] + carry[i];
        residualVector[(k - 1) * nx + i] :=
          residualVector[(k - 1) * nx + i] + weightSqrt
          * (nodesNext[k, i] + carry[i] - reference[i]);
        for col in 1:(horizon * nu) loop
          sensitivityAll[(k - 1) * nx + i, col] :=
            sensitivityAll[(k - 1) * nx + i, col] + sensitivity[i, col];
          jacobianMatrix[(k - 1) * nx + i, col] :=
            jacobianMatrix[(k - 1) * nx + i, col]
            + weightSqrt * sensitivity[i, col];
        end for;
      end for;
      for j in 1:nu loop
        weightSqrt := sqrt(max(inputWeight[j], 0.0));
        jacobianMatrix[horizon * nx + (k - 1) * nu + j, (k - 1) * nu + j] :=
          jacobianMatrix[horizon * nx + (k - 1) * nu + j, (k - 1) * nu + j]
          + weightSqrt;
        residualVector[horizon * nx + (k - 1) * nu + j] :=
          residualVector[horizon * nx + (k - 1) * nu + j]
          + weightSqrt * controlsNext[k, j];
      end for;
    end for;

    cost := 0.0;
    for row in 1:(horizon * (nx + nu)) loop
      cost := cost + 0.5 * residualVector[row] * residualVector[row];
    end for;

    (step, stepAccepted) := NlpSolver(jacobianMatrix, residualVector,
      ocp.regularization);
    // A rejected step is applied with scale zero instead of branching,
    // because the compiler does not yet accept loops inside conditional
    // branches of a function; the iterate is left unchanged either way.
    stepScale := if stepAccepted then 1.0 else 0.0;
    solveAccepted := stepAccepted;
    for k in 1:horizon loop
      for j in 1:nu loop
        controlsNext[k, j] := controlsNext[k, j]
          + stepScale * step[(k - 1) * nu + j];
      end for;
      for i in 1:nx loop
        accumulator := carryAll[(k - 1) * nx + i];
        for col in 1:(horizon * nu) loop
          accumulator := accumulator
            + sensitivityAll[(k - 1) * nx + i, col] * step[col];
        end for;
        nodesNext[k, i] := nodesNext[k, i] + stepScale * accumulator;
      end for;
    end for;

    commandOut := controlsNext[1, :];
  end solveTick;