A sparse MPC solver for walking motion generation (old version).
|
Initializes lower diagonal matrix L and performs backward and forward substitutions using this matrix. More...
#include <matrix_ecL_as.h>
Public Member Functions | |
matrix_ecL_as (const int) | |
~matrix_ecL_as () | |
void | form (const problem_parameters &) |
Builds matrix L. | |
void | solve_backward (const int, double *) |
Solve system ecL' * x = b using backward substitution. | |
void | solve_forward (const int, double *) |
Solve system ecL * x = b using forward substitution. | |
Public Attributes | |
double * | ecL |
double ** | ecL_diag |
double ** | ecL_ndiag |
Private Member Functions | |
void | chol_dec (double *) |
Performs Cholesky decomposition of 3x3 matrix. | |
void | form_iQBiPB (const double *, const double *, const double, double *) |
Forms matrix iQBiPB = 0.5 * inv(Q) + 0.5 * B * inv(P) * B. | |
void | form_iQAT (const double, const double, const double *) |
Forms matrix iQAT = 0.5 * inv (Q) * A'. | |
void | form_AiQATiQBiPB (const problem_parameters &, const state_parameters &, double *) |
Forms matrix AiQATiQBiPB = A * inv(Q) * A' + 0.5 * inv(Q) + 0.5 * B * inv(P) * B. | |
void | form_L_non_diag (const double *, double *) |
Forms a 3x3 matrix L(k+1, k), which lies below the diagonal of L. | |
void | form_L_diag (const double *, double *) |
Forms a 3x3 matrix L(k+1, k+1), which lies on the main diagonal of L. | |
Private Attributes | |
double * | iQAT |
Initializes lower diagonal matrix L and performs backward and forward substitutions using this matrix.
Definition at line 41 of file matrix_ecL_as.h.
matrix_ecL_as::matrix_ecL_as | ( | const int | N | ) |
Definition at line 24 of file matrix_ecL_as.cpp.
Definition at line 42 of file matrix_ecL_as.cpp.
void matrix_ecL_as::chol_dec | ( | double * | mx9 | ) | [private] |
Performs Cholesky decomposition of 3x3 matrix.
[in,out] | mx9 | a pointer to matrix, the result is stored in the same place. |
Definition at line 68 of file matrix_ecL_as.cpp.
void matrix_ecL_as::form | ( | const problem_parameters & | ppar | ) |
void matrix_ecL_as::form_AiQATiQBiPB | ( | const problem_parameters & | ppar, |
const state_parameters & | stp, | ||
double * | result | ||
) | [private] |
Forms matrix AiQATiQBiPB = A * inv(Q) * A' + 0.5 * inv(Q) + 0.5 * B * inv(P) * B.
[in] | ppar | problem parameters. |
[in] | stp | state parameters. |
[out] | result | result |
Definition at line 143 of file matrix_ecL_as.cpp.
void matrix_ecL_as::form_iQAT | ( | const double | A3, |
const double | A6, | ||
const double * | i2Q | ||
) | [private] |
Forms matrix iQAT = 0.5 * inv (Q) * A'.
[in] | A3 | 4th and 7th elements of A. |
[in] | A6 | 6th element of A. |
[in] | i2Q | a vector of 3 elements, which contains diagonal elements of 0.5*inv(Q). |
Definition at line 122 of file matrix_ecL_as.cpp.
void matrix_ecL_as::form_iQBiPB | ( | const double * | B, |
const double * | i2Q, | ||
const double | i2P, | ||
double * | result | ||
) | [private] |
Forms matrix iQBiPB = 0.5 * inv(Q) + 0.5 * B * inv(P) * B.
[in] | B | a vector of 3 elements. |
[in] | i2Q | a vector of 3 elements, which contains diagonal elements of 0.5 * inv(Q). |
[in] | i2P | 0.5 * inv(P) (only one number) |
[out] | result | the result |
Definition at line 99 of file matrix_ecL_as.cpp.
void matrix_ecL_as::form_L_diag | ( | const double * | ecLp, |
double * | ecLc | ||
) | [private] |
Forms a 3x3 matrix L(k+1, k+1), which lies on the main diagonal of L.
[in] | ecLp | upper triangular matrix matrix lying to the left from ecLc on the same level of L |
[in] | ecLc | the result is stored here |
Definition at line 209 of file matrix_ecL_as.cpp.
void matrix_ecL_as::form_L_non_diag | ( | const double * | ecLp, |
double * | ecLc | ||
) | [private] |
Forms a 3x3 matrix L(k+1, k), which lies below the diagonal of L.
[in] | ecLp | previous matrix lying on the diagonal of L |
[in] | ecLc | the result is stored here |
Definition at line 173 of file matrix_ecL_as.cpp.
void matrix_ecL_as::solve_backward | ( | const int | N, |
double * | x | ||
) |
Solve system ecL' * x = b using backward substitution.
[in] | N | number of states in the preview window |
[in,out] | x | vector "b" as input, vector "x" as output. |
Definition at line 329 of file matrix_ecL_as.cpp.
void matrix_ecL_as::solve_forward | ( | const int | N, |
double * | x | ||
) |
Solve system ecL * x = b using forward substitution.
[in] | N | number of states in the preview window |
[in,out] | x | vector "b" as input, vector "x" as output (N * SMPC_NUM_STATE_VAR) |
Definition at line 268 of file matrix_ecL_as.cpp.
double* matrix_ecL_as::ecL |
Definition at line 53 of file matrix_ecL_as.h.
double** matrix_ecL_as::ecL_diag |
Definition at line 54 of file matrix_ecL_as.h.
double** matrix_ecL_as::ecL_ndiag |
Definition at line 55 of file matrix_ecL_as.h.
double* matrix_ecL_as::iQAT [private] |
Definition at line 69 of file matrix_ecL_as.h.