Computing optimal weights

for the preview control. More...

for the preview control.

First you need to include the header file:

#include "PreviewControl/OptimalControllerSolver.hh"

In the following we will use a pointer towards an instance of class OptimalControllerSolver.

int main() {
  PatternGeneratorJRL::OptimalControllerSolver *anOCS;

You have then to build the linear discrete system of the linear inverted pendulum such that:

\begin{eqnarray*} {\bf x}_{k+1} &=& {\bf A}{\bf x}_k + {\bf B} u_{k} \\ p_k &=& {\bf C} {\bf x}_{k} \\ \end{eqnarray*}

For this we need to declare the associated matrices:

  /* Declare the linear system */
  Eigen::MatrixXd A(3, 3);
  Eigen::MatrixXd b(3, 1);
  Eigen::MatrixXd c(1, 3);
  Eigen::MatrixXd lF;

and the weights of the function to be minimized:

  double Q, R;
  int Nl;
  double T = 0.005;

Then we have to initialize the discretized linear system with

\begin{eqnarray*} {\bf A} & \equiv & \left[ \begin{matrix} 1 & T & T^2/2 \\ 0 & 1 & T \\ 0 & 0 & 1 \end{matrix} \right] \\ {\bf B} & \equiv & \left[ \begin{matrix} T^3/6 \\ T^2/2 \\ T \end{matrix} \right] \\ {\bf C} & \equiv & \left[ 1 \; 0 \; \frac{-z_c}{g} \right] \end{eqnarray*}

  /* Build the initial discrete system
     regarding the CoM and the ZMP. */
  A(0, 0) = 1.0;
  A(0, 1) = T;
  A(0, 2) = T * T / 2.0;
  A(1, 0) = 0.0;
  A(1, 1) = 1.0;
  A(1, 2) = T;
  A(2, 0) = 0.0;
  A(2, 1) = 0.0;
  A(2, 2) = 1;
 
  b(0, 0) = T * T * T / 6.0;
  b(1, 0) = T * T / 2.0;
  b(2, 0) = T;
 
  c(0, 0) = 1.0;
  c(0, 1) = 0.0;
  c(0, 2) = -0.814 / 9.8;

We then have to initialize the weights of the index function:

\[ J = \sum^{NL}_{j=1} \{ Q(p^{ref}_j -p_j)^2 + Ru_j^2 \} \]

  Q = 1.0;
  R = 1e-6;
 
  Nl = (int)(1.6 / T);

To suppress the problem of the initial CoM position, we can reformulate the discrete problem by posing the following:

\begin{eqnarray*} {\bf x}^*_{k+1} = \widetilde{\bf A} {\bf x}^*_{k} + \widetilde{\bf b} \Delta u_k p_k = \widetilde{\bf c}{\bf x}^*_{k} \end{eqnarray*}

with \f{eqnarray* \Delta u_k \equiv u_k - u_{k-1} & \Delta {\bf x}_k \equiv {\bf x}_k - {\bf x}_{k-1} {\bf x}_k \equiv \left[ \begin{matrix} p_k\ \Delta {\bf x}_k \end{matrix} \right] \f}

\begin{eqnarray*} \widetilde{\bf A} &\equiv & \left[ \begin{matrix} 1 & {\bf cA} \\ {\bf 0} & {\bf A} \\ \end{matrix} \right] \\ \tilde{\bf b} & \equiv & \left[ \begin{matrix} {\bf cb} \\ {\bf c} \end{matrix} \right] \\ \tilde{\bf c} & \equiv & [ 1 \; 0 \; 0 \; 0] \\ \end{eqnarray*}

Then the subsequent code performs this operation and displays the associated matrices:

  // Build the derivated system
  Eigen::MatrixXd Ax(4, 4);
  Eigen::MatrixXd tmpA;
  Eigen::MatrixXd bx(4, 1);
  Eigen::MatrixXd tmpb;
  Eigen::MatrixXd cx(1, 4);
 
  tmpA = c * A;
  cout << "tmpA :" << tmpA << endl;
 
  Ax(0, 0) = 1.0;
  for (int i = 0; i < 3; i++) {
    Ax(0, i + 1) = tmpA(0, i);
    Ax(i + 1, 0) = 0.;
    for (int j = 0; j < 3; j++) Ax(i + 1, j + 1) = A(i, j);
  }
  cout << "Ax: " << endl << Ax << endl;
 
  tmpb = c * b;
  bx(0, 0) = tmpb(0, 0);
  for (int i = 0; i < 3; i++) {
    bx(i + 1, 0) = b(i, 0);
  }
 
  cout << "bx: " << endl << bx << endl;
 
  cx(0, 0) = 1.0;
  cx(0, 1) = 0.0;
  cx(0, 2) = 0.0;
  cx(0, 3) = 0.0;
  cout << "cx: " << endl << cx << endl;

To create the instance of the object solving the Riccati Equation:

      new PatternGeneratorJRL::OptimalControllerSolver(Ax, bx, cx, Q, R, Nl);

The computation of the weights is done by calling ComputeWeights(). There is only one parameter to specify, but it is important as the weights are computed differently according to this parameter. If you use the mode without initial position please uses MODE_WITH_INITIALPOS.

      PatternGeneratorJRL::OptimalControllerSolver::MODE_WITHOUT_INITIALPOS);

To display the weights in the standard output

  anOCS->DisplayWeights();

It is possible to retrieve the weights in a vector:

  anOCS->GetF(lF);

To compute the same weight with a specific initial position and the original linear system you can use :

  anOCS = new PatternGeneratorJRL::OptimalControllerSolver(A, b, c, Q, R, Nl);
 
  anOCS->ComputeWeights(
      PatternGeneratorJRL::OptimalControllerSolver::MODE_WITH_INITIALPOS);