A sparse MPC solver for walking motion generation.
as_matrix_ecL.cpp
Go to the documentation of this file.
1 
8 /****************************************
9  * INCLUDES
10  ****************************************/
11 
12 #include "as_matrix_ecL.h"
13 
14 #include <cmath> // sqrt
15 
16 
17 /****************************************
18  * DEFINES
19  ****************************************/
20 
23 
37 #define MATRIX_SIZE_3x3 9
38 
40 
41 /****************************************
42  * FUNCTIONS
43  ****************************************/
44 
45 namespace AS
46 {
47  //==============================================
48  // constructors / destructors
49 
50  matrix_ecL::matrix_ecL (const int N)
51  {
52  ecL = new double[MATRIX_SIZE_3x3*N + MATRIX_SIZE_3x3*(N-1)]();
53 
54  iQAT = new double[MATRIX_SIZE_3x3];
55  ecL_diag = new double*[N];
56 
89  for (int i = 0; i < N; i++)
90  {
91  ecL_diag[i] = &ecL[i * MATRIX_SIZE_3x3 * 2];
92  }
93  ecL_ndiag = new double*[N-1];
94  for (int i = 0; i < N-1; i++)
95  {
96  ecL_ndiag[i] = &ecL[i * MATRIX_SIZE_3x3 * 2 + MATRIX_SIZE_3x3];
97  }
98  }
99 
100 
101  matrix_ecL::~matrix_ecL()
102  {
103  if (ecL != NULL)
104  delete ecL;
105 
106  if (ecL_diag != NULL)
107  delete ecL_diag;
108 
109  if (ecL_ndiag != NULL)
110  delete ecL_ndiag;
111 
112  if (iQAT != NULL)
113  delete iQAT;
114  }
115  //==============================================
116 
117 
118 
127  void matrix_ecL::chol_dec (double *mx9)
128  {
129  // 1st line
130  mx9[0] = sqrt (mx9[0]);
131 
132  // 2nd line
133  mx9[1] /= mx9[0];
134  mx9[4] = sqrt(mx9[4] - mx9[1]*mx9[1]);
135 
136  // 3rd line
137  mx9[2] /= mx9[0];
138  mx9[5] = (mx9[5] - mx9[2]*mx9[1])/mx9[4];
139  mx9[8] = sqrt(mx9[8] - mx9[5]*mx9[5] - mx9[2]*mx9[2]);
140 
141  // These elements must be 0. (but they are never used)
142  // mx9[3] = mx9[6] = mx9[7] = 0;
143  }
144 
145 
146 
158  void matrix_ecL::form_iQBiPB (const double *B, const double *i2Q, const double i2P, double* result)
159  {
160  // diagonal elements
161  result[0] = i2P * B[0]*B[0] + i2Q[0];
162  result[4] = i2P * B[1]*B[1] + i2Q[1];
163  result[8] = i2P * B[2]*B[2] + i2Q[2];
164 
165  // symmetric elements (no need to initialize all of them)
166  result[1] = /*result[3] =*/ i2P * B[0]*B[1];
167  result[2] = /*result[6] =*/ i2P * B[0]*B[2];
168  result[5] = /*result[7] =*/ i2P * B[1]*B[2];
169  }
170 
171 
172 
181  void matrix_ecL::form_iQAT (const double A3, const double A6, const double *i2Q)
182  {
183  iQAT[0] = i2Q[0];
184  iQAT[1] = A3 * i2Q[1];
185  iQAT[2] = A6 * i2Q[2];
186  iQAT[4] = i2Q[1];
187  iQAT[5] = A3 * i2Q[2];
188  iQAT[8] = i2Q[2];
189  }
190 
191 
202  void matrix_ecL::form_AiQATiQBiPB (const problem_parameters &ppar, const state_parameters& stp, double *result)
203  {
204  form_iQBiPB (stp.B, ppar.i2Q, ppar.i2P, result);
205 
206  // 1st column
207  result[0] += iQAT[0] + stp.A3*iQAT[1] + stp.A6*iQAT[2];
208  result[1] += iQAT[1] + stp.A3*iQAT[2];
209  result[2] += iQAT[2];
210 
211  // 2nd column
212  // symmetric elements are not initialized
213  // result[3] = iQBiPB[3] + A3*iQAT[4] + A6*iQAT[5];
214  result[4] += iQAT[4] + stp.A3*iQAT[5];
215  result[5] += iQAT[5];
216 
217  // 3rd column
218  // result[6] = iQBiPB[6] + A6*iQAT[8];
219  // result[7] = iQBiPB[7] + A3*iQAT[8];
220  result[8] += iQAT[8];
221  }
222 
223 
224 
232  void matrix_ecL::form_L_non_diag(const double *ecLp, double *ecLc)
233  {
234  /* L(k+1,k) * L(k,k)' = - inv(Q) * A'
235  *
236  * xxx xxx xxx
237  * xx * xx = xx
238  * x x x
239  *
240  * all matrices are upper triangular
241  */
242 
243  // main diagonal
244  ecLc[0] = -iQAT[0] / ecLp[0];
245  ecLc[4] = -iQAT[4] / ecLp[4];
246  ecLc[8] = -iQAT[8] / ecLp[8];
247 
248  // sub-diagonal 1
249  ecLc[3] = (-iQAT[1] - ecLc[0]*ecLp[1]) / ecLp[4];
250  ecLc[7] = (-iQAT[5] - ecLc[4]*ecLp[5]) / ecLp[8];
251 
252  // sub-diagonal 2
253  ecLc[6] = (-iQAT[2] - ecLc[0]*ecLp[2] - ecLc[3]*ecLp[5]) / ecLp[8];
254  }
255 
256 
257 
268  void matrix_ecL::form_L_diag(const double *ecLp, double *ecLc)
269  {
270  // L(k+1,k+1) = (- L(k+1,k) * L(k+1,k)') + (A * inv(Q) * A' + inv(Q) + B * inv(P) * B)
271  // diagonal elements
272  ecLc[0] -= ecLp[0]*ecLp[0] + ecLp[3]*ecLp[3] + ecLp[6]*ecLp[6];
273  ecLc[4] -= ecLp[4]*ecLp[4] + ecLp[7]*ecLp[7];
274  ecLc[8] -= ecLp[8]*ecLp[8];
275  // symmetric nondiagonal elements (no need to initialize all of them)
276  ecLc[1] -= /*ecLc[3] =*/ ecLp[3]*ecLp[4] + ecLp[6]*ecLp[7];
277  ecLc[2] -= /*ecLc[6] =*/ ecLp[6]*ecLp[8];
278  ecLc[5] -= /*ecLc[7] =*/ ecLp[7]*ecLp[8];
279 
280  // chol (L(k+1,k+1))
281  chol_dec (ecLc);
282  }
283 
284 
285 
291  void matrix_ecL::form (const problem_parameters& ppar)
292  {
293  int i;
294  state_parameters stp;
295 
296 
297 
298  // the first matrix on diagonal
299  stp = ppar.spar[0];
300  form_iQBiPB (stp.B, ppar.i2Q, ppar.i2P, ecL_diag[0]);
301  chol_dec (ecL_diag[0]);
302 
303 
304  // offsets
305  for (i = 1; i < ppar.N; i++)
306  {
307  stp = ppar.spar[i];
308  form_iQAT (stp.A3, stp.A6, ppar.i2Q);
309 
310  // form (b), (d), (f) ...
311  form_L_non_diag(ecL_diag[i-1], ecL_ndiag[i-1]);
312 
313  // form (c), (e), (g) ...
314  form_AiQATiQBiPB (ppar, stp, ecL_diag[i]);
315  form_L_diag(ecL_ndiag[i-1], ecL_diag[i]);
316  }
317  }
318 
319 
334  void matrix_ecL::solve_forward(const int N, double *x, const int start_ind) const
335  {
336  int i = start_ind, j = start_ind;
337  double *xc = &x[SMPC_NUM_STATE_VAR * start_ind]; // 6 current elements of x
338 
339 
340  // compute the first 6 elements using forward substitution
341  xc[0] /= ecL_diag[i][0];
342  xc[3] /= ecL_diag[i][0];
343 
344  xc[1] -= xc[0] * ecL_diag[i][1];
345  xc[1] /= ecL_diag[i][4];
346 
347  xc[4] -= xc[3] * ecL_diag[i][1];
348  xc[4] /= ecL_diag[i][4];
349 
350  xc[2] -= xc[0] * ecL_diag[i][2] + xc[1] * ecL_diag[i][5];
351  xc[2] /= ecL_diag[i][8];
352 
353  xc[5] -= xc[3] * ecL_diag[i][2] + xc[4] * ecL_diag[i][5];
354  xc[5] /= ecL_diag[i][8];
355 
356  ++i;
357 
358  for (; i < N; ++i, ++j)
359  {
360  // 6 elements of x computed on the previous iteration
361  double *xp = xc;
362  // switch to the next level of L / next 6 elements
363  xc = &xc[SMPC_NUM_STATE_VAR];
364 
365 
366  // update the right part of the equation and compute elements
367  xc[0] -= xp[0] * ecL_ndiag[j][0] + xp[1] * ecL_ndiag[j][3] + xp[2] * ecL_ndiag[j][6];
368  xc[0] /= ecL_diag[i][0];
369 
370  xc[3] -= xp[3] * ecL_ndiag[j][0] + xp[4] * ecL_ndiag[j][3] + xp[5] * ecL_ndiag[j][6];
371  xc[3] /= ecL_diag[i][0];
372 
373 
374  xc[1] -= xp[1] * ecL_ndiag[j][4] + xp[2] * ecL_ndiag[j][7] + xc[0] * ecL_diag[i][1];
375  xc[1] /= ecL_diag[i][4];
376 
377  xc[4] -= xp[4] * ecL_ndiag[j][4] + xp[5] * ecL_ndiag[j][7] + xc[3] * ecL_diag[i][1];
378  xc[4] /= ecL_diag[i][4];
379 
380 
381  xc[2] -= xp[2] * ecL_ndiag[j][8] + xc[0] * ecL_diag[i][2] + xc[1] * ecL_diag[i][5];
382  xc[2] /= ecL_diag[i][8];
383 
384  xc[5] -= xp[5] * ecL_ndiag[j][8] + xc[3] * ecL_diag[i][2] + xc[4] * ecL_diag[i][5];
385  xc[5] /= ecL_diag[i][8];
386  }
387  }
388 
389 
396  void matrix_ecL::solve_backward (const int N, double *x) const
397  {
398  int i;
399  double *xc = & x[(N-1)*SMPC_NUM_STATE_VAR]; // current 6 elements of result
400  double *xp; // 6 elements computed on the previous iteration
401 
402 
403  // compute the last 6 elements using backward substitution
404  xc[2] /= ecL_diag[N-1][8];
405  xc[5] /= ecL_diag[N-1][8];
406 
407  xc[1] -= xc[2] * ecL_diag[N-1][5];
408  xc[1] /= ecL_diag[N-1][4];
409  xc[4] -= xc[5] * ecL_diag[N-1][5];
410  xc[4] /= ecL_diag[N-1][4];
411 
412  xc[0] -= xc[2] * ecL_diag[N-1][2] + xc[1] * ecL_diag[N-1][1];
413  xc[0] /= ecL_diag[N-1][0];
414  xc[3] -= xc[5] * ecL_diag[N-1][2] + xc[4] * ecL_diag[N-1][1];
415  xc[3] /= ecL_diag[N-1][0];
416 
417 
418  for (i = N-2; i >= 0 ; i--)
419  {
420  xp = xc;
421  xc = & x[i*SMPC_NUM_STATE_VAR];
422 
423  // update the right part of the equation and compute elements
424  xc[2] -= xp[0] * ecL_ndiag[i][6] + xp[1] * ecL_ndiag[i][7] + xp[2] * ecL_ndiag[i][8];
425  xc[2] /= ecL_diag[i][8];
426 
427  xc[5] -= xp[3] * ecL_ndiag[i][6] + xp[4] * ecL_ndiag[i][7] + xp[5] * ecL_ndiag[i][8];
428  xc[5] /= ecL_diag[i][8];
429 
430 
431  xc[1] -= xp[0] * ecL_ndiag[i][3] + xp[1] * ecL_ndiag[i][4] + xc[2] * ecL_diag[i][5];
432  xc[1] /= ecL_diag[i][4];
433 
434  xc[4] -= xp[3] * ecL_ndiag[i][3] + xp[4] * ecL_ndiag[i][4] + xc[5] * ecL_diag[i][5];
435  xc[4] /= ecL_diag[i][4];
436 
437 
438  xc[0] -= xp[0] * ecL_ndiag[i][0] + xc[2] * ecL_diag[i][2] + xc[1] * ecL_diag[i][1];
439  xc[0] /= ecL_diag[i][0];
440 
441  xc[3] -= xp[3] * ecL_ndiag[i][0] + xc[5] * ecL_diag[i][2] + xc[4] * ecL_diag[i][1];
442  xc[3] /= ecL_diag[i][0];
443  }
444  }
445 }
SMPC_NUM_STATE_VAR
#define SMPC_NUM_STATE_VAR
Number of state variables.
Definition: smpc_solver.h:24
as_matrix_ecL.h
AS::matrix_ecL::form_L_diag
void form_L_diag(const double *, double *)
Forms a 3x3 matrix L(k+1, k+1), which lies on the main diagonal of L.
Definition: as_matrix_ecL.cpp:268
AS::state_parameters::B
double B[3]
Definition: as_problem_param.h:38
MATRIX_SIZE_3x3
#define MATRIX_SIZE_3x3
Definition: as_matrix_ecL.cpp:37
AS::state_parameters
Definition: as_problem_param.h:26
AS::matrix_ecL::iQAT
double * iQAT
Definition: as_matrix_ecL.h:63
AS::matrix_ecL::matrix_ecL
matrix_ecL(const int)
Definition: as_matrix_ecL.cpp:50
AS::matrix_ecL::form_iQBiPB
void form_iQBiPB(const double *, const double *, const double, double *)
Forms matrix iQBiPB = 0.5 * inv(Q) + 0.5 * B * inv(P) * B.
Definition: as_matrix_ecL.cpp:158
AS::matrix_ecL::ecL
double * ecL
Definition: as_matrix_ecL.h:47
AS::state_parameters::A6
double A6
Definition: as_problem_param.h:36
AS::matrix_ecL::form_AiQATiQBiPB
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.
Definition: as_matrix_ecL.cpp:202
AS::matrix_ecL::~matrix_ecL
~matrix_ecL()
Definition: as_matrix_ecL.cpp:101
AS::problem_parameters::i2Q
double i2Q[3]
Definition: as_problem_param.h:60
AS::problem_parameters
A set of problem parameters.
Definition: as_problem_param.h:45
AS::matrix_ecL::chol_dec
void chol_dec(double *)
Performs Cholesky decomposition of 3x3 matrix.
Definition: as_matrix_ecL.cpp:127
AS::matrix_ecL::form_iQAT
void form_iQAT(const double, const double, const double *)
Forms matrix iQAT = 0.5 * inv (Q) * A'.
Definition: as_matrix_ecL.cpp:181
AS::matrix_ecL::ecL_diag
double ** ecL_diag
Definition: as_matrix_ecL.h:48
AS::matrix_ecL::form
void form(const problem_parameters &)
Builds matrix L.
Definition: as_matrix_ecL.cpp:291
AS::problem_parameters::i2P
double i2P
Definition: as_problem_param.h:65
AS::problem_parameters::spar
state_parameters * spar
Definition: as_problem_param.h:68
AS::problem_parameters::N
int N
Definition: as_problem_param.h:55
AS::state_parameters::A3
double A3
Definition: as_problem_param.h:35
AS::matrix_ecL::solve_forward
void solve_forward(const int, double *, const int start_ind=0) const
Solve system ecL * x = b using forward substitution.
Definition: as_matrix_ecL.cpp:334
AS::matrix_ecL::ecL_ndiag
double ** ecL_ndiag
Definition: as_matrix_ecL.h:49
AS::matrix_ecL::form_L_non_diag
void form_L_non_diag(const double *, double *)
Forms a 3x3 matrix L(k+1, k), which lies below the diagonal of L.
Definition: as_matrix_ecL.cpp:232
AS::matrix_ecL::solve_backward
void solve_backward(const int, double *) const
Solve system ecL' * x = b using backward substitution.
Definition: as_matrix_ecL.cpp:396
Generated by   doxygen 1.8.15