Computing optimal weights

for the preview control.

First you need to include the header file:

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

int main() {
This class computes the gains for preview control for a given discrete system. The discrete system is...
Definition: OptimalControllerSolver.hh:144

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;
doublereal * b
Definition: qld.cpp:386
doublereal * c
Definition: qld.cpp:386

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:

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.

static const unsigned int MODE_WITHOUT_INITIALPOS
Definition: OptimalControllerSolver.hh:146

To display the weights in the standard output

anOCS->DisplayWeights();
void DisplayWeights()
Definition: OptimalControllerSolver.cpp:314

It is possible to retrieve the weights in a vector:

anOCS->GetF(lF);
void GetF(Eigen::MatrixXd &LF)
Definition: OptimalControllerSolver.cpp:319

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);
static const unsigned int MODE_WITH_INITIALPOS
Definition: OptimalControllerSolver.hh:147
void ComputeWeights(unsigned int Mode)
Definition: OptimalControllerSolver.cpp:170