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_ip.h>
Public Member Functions | |
matrix_ecL_ip (const int) | |
~matrix_ecL_ip () | |
void | form (const problem_parameters &, const double *) |
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 |
Private Member Functions | |
void | chol_dec (double *) |
Performs Cholesky decomposition of a matrix. | |
void | form_M (const double, const double, const double *, const double *) |
Forms M = R*inv(hess_phi)*R'. | |
void | form_MBiPB (const double *, const double, double *) |
Forms matrix MBiPB = M + B * inv(2*P) * B. | |
void | form_MAT (const double, const double) |
Forms matrix MAT = M * A'. | |
void | form_AMATMBiPB (const double, const double, double *) |
Forms matrix AMATMBiPB = A * M * A' + 0.5 * M + 0.5 * B * inv(P) * B. | |
void | form_L_non_diag (const double *, double *) |
Forms a 6x6 matrix L(k+1, k), which lies below the diagonal of L. | |
void | form_L_diag (double *) |
Forms a 6x6 matrix L(k+1, k+1), which lies below the diagonal of L. | |
void | form_L_diag (const double *, double *) |
Forms a 6x6 matrix L(k+1, k+1), which lies on the main diagonal of L. | |
Private Attributes | |
double * | M |
double * | MAT |
R * inv(Q) * R'. |
Initializes lower diagonal matrix L and performs backward and forward substitutions using this matrix.
Definition at line 42 of file matrix_ecL_ip.h.
matrix_ecL_ip::matrix_ecL_ip | ( | const int | N | ) |
Definition at line 25 of file matrix_ecL_ip.cpp.
Definition at line 34 of file matrix_ecL_ip.cpp.
void matrix_ecL_ip::chol_dec | ( | double * | mx | ) | [private] |
Performs Cholesky decomposition of a matrix.
[in,out] | mx | a pointer to a 6x6 matrix, the result is stored in the same place. |
Definition at line 97 of file matrix_ecL_ip.cpp.
void matrix_ecL_ip::form | ( | const problem_parameters & | ppar, |
const double * | i2hess | ||
) |
Builds matrix L.
[in] | ppar | parameters. |
[in] | i2hess | 2*N diagonal elements of inverted hessian. |
Definition at line 365 of file matrix_ecL_ip.cpp.
void matrix_ecL_ip::form_AMATMBiPB | ( | const double | A3, |
const double | A6, | ||
double * | result | ||
) | [private] |
Forms matrix AMATMBiPB = A * M * A' + 0.5 * M + 0.5 * B * inv(P) * B.
[in] | A3 | 4th and 7th elements of A (A is represented by two identical 3x3 matrices). |
[in] | A6 | 6th element of A (A is represented by two identical 3x3 matrices). |
[in,out] | result | MBiPB as input / result |
Definition at line 284 of file matrix_ecL_ip.cpp.
void matrix_ecL_ip::form_L_diag | ( | double * | ecLc | ) | [private] |
Forms a 6x6 matrix L(k+1, k+1), which lies below the diagonal of L.
[in,out] | ecLc | MBiPB as input / result |
Definition at line 254 of file matrix_ecL_ip.cpp.
void matrix_ecL_ip::form_L_diag | ( | const double * | ecLp, |
double * | ecLc | ||
) | [private] |
Forms a 6x6 matrix L(k+1, k+1), which lies on the main diagonal of L.
[in] | ecLp | a 6x6 matrix lying to the left from ecLc on the same level of L |
[in,out] | ecLc | AMATMBiPB as input / the result is stored here |
Definition at line 330 of file matrix_ecL_ip.cpp.
void matrix_ecL_ip::form_L_non_diag | ( | const double * | ecLp, |
double * | ecLc | ||
) | [private] |
Forms a 6x6 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 197 of file matrix_ecL_ip.cpp.
void matrix_ecL_ip::form_M | ( | const double | sinA, |
const double | cosA, | ||
const double * | i2Q, | ||
const double * | i2hess | ||
) | [private] |
Forms M = R*inv(hess_phi)*R'.
[in] | sinA | sin of rotation angle. |
[in] | cosA | cos of rotation angle. |
[in] | i2Q | a vector of three repeating diagonal elements of inv(Q) |
[in] | i2hess | a 2*N vector of diagonal elements of hess_phi (indicies of these elements are 1:3:N*SMPC_NUM_STATE_VAR) |
Definition at line 61 of file matrix_ecL_ip.cpp.
void matrix_ecL_ip::form_MAT | ( | const double | A3, |
const double | A6 | ||
) | [private] |
Forms matrix MAT = M * A'.
[in] | A3 | 4th and 7th elements of A. |
[in] | A6 | 6th element of A. |
Definition at line 160 of file matrix_ecL_ip.cpp.
void matrix_ecL_ip::form_MBiPB | ( | const double * | B, |
const double | i2P, | ||
double * | result | ||
) | [private] |
Forms matrix MBiPB = M + B * inv(2*P) * B.
[in] | B | a vector of 3 elements. |
[in] | i2P | 0.5 * inv(P) (only one number) |
[out] | result | result. |
Definition at line 137 of file matrix_ecL_ip.cpp.
void matrix_ecL_ip::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 474 of file matrix_ecL_ip.cpp.
void matrix_ecL_ip::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 414 of file matrix_ecL_ip.cpp.
double* matrix_ecL_ip::ecL |
Definition at line 54 of file matrix_ecL_ip.h.
double* matrix_ecL_ip::M [private] |
Definition at line 71 of file matrix_ecL_ip.h.
double* matrix_ecL_ip::MAT [private] |
R * inv(Q) * R'.
Definition at line 72 of file matrix_ecL_ip.h.