A sparse MPC solver for walking motion generation.
qp_ip.cpp
Go to the documentation of this file.
1 
8 /****************************************
9  * INCLUDES
10  ****************************************/
11 #include "qp_ip.h"
12 #include "state_handling.h"
13 #include "qp.h"
14 
15 
16 #include <cmath> // log
17 
18 /****************************************
19  * FUNCTIONS
20  ****************************************/
21 
22 using namespace IP;
23 
24 //==============================================
25 // qp_ip
26 
39  const int N_,
40  const double gain_position_,
41  const double gain_velocity_,
42  const double gain_acceleration_,
43  const double gain_jerk_,
44  const double tol_,
45  const bool obj_computation_on_,
46  const backtrackingSearchType bs_type_) :
47  problem_parameters (N_, gain_position_, gain_velocity_, gain_acceleration_, gain_jerk_),
48  chol (N_)
49 {
50  dX = new double[SMPC_NUM_VAR*N]();
51  g = new double[2*N];
52  i2hess = new double[2*N];
53  i2hess_grad = new double[N*SMPC_NUM_VAR];
54  grad = new double[2*N];
55 
56  tol = tol_;
57 
58  obj_computation_on = obj_computation_on_;
59  bs_type = bs_type_;
60 
61  gain_position = gain_position_;
62 
63  Q[0] = gain_position_/2;
64  Q[1] = gain_velocity_/2;
65  Q[2] = gain_acceleration_/2;
66  P = gain_jerk_/2;
67 }
68 
69 
72 {
73  if (g != NULL)
74  delete g;
75  if (i2hess != NULL)
76  delete i2hess;
77  if (i2hess_grad != NULL)
78  delete i2hess_grad;
79  if (grad != NULL)
80  delete grad;
81  if (dX != NULL)
82  delete dX;
83 }
84 
85 
98  const double* T,
99  const double* h,
100  const double h_initial_,
101  const double* angle,
102  const double* zref_x_,
103  const double* zref_y_,
104  const double* lb_,
105  const double* ub_)
106 {
107  set_state_parameters (T, h, h_initial_, angle);
108 
109  lb = lb_;
110  ub = ub_;
111 
112  zref_x = zref_x_;
113  zref_y = zref_y_;
114 
115  form_g (zref_x, zref_y);
116 }
117 
118 
119 
126 void qp_ip::form_g (const double *zref_x_, const double *zref_y_)
127 {
128  double p0, p1;
129  double cosA, sinA;
130 
131  for (int i = 0; i < N; i++)
132  {
133  cosA = spar[i].cos;
134  sinA = spar[i].sin;
135 
136  // zref
137  p0 = zref_x_[i];
138  p1 = zref_y_[i];
139 
140  // inv (2*H) * R' * Cp' * zref
141  g[i*2] = -(cosA*p0 + sinA*p1)*gain_position;
142  g[i*2 + 1] = -(-sinA*p0 + cosA*p1)*gain_position;
143  }
144 }
145 
146 
147 
156 double qp_ip::form_grad_i2hess_logbar (const double kappa)
157 {
158  double phi_X_logbar = 1.0;
159 
160  // grad = H*X + g + kappa * b;
161  // initialize inverted hessian
162  // initialize logarithmic barrier in the function
163  for (int i = 0; i < 2*N; i++)
164  {
165  const int j = 3*i;
166  double lb_diff = -lb[i] + X[j];
167  double ub_diff = ub[i] - X[j];
168 
169  // logarithmic barrier
170  if (bs_type == SMPC_IP_BS_LOGBAR)
171  {
172  phi_X_logbar += log(lb_diff * ub_diff);
173  }
174 
175  lb_diff = 1/lb_diff;
176  ub_diff = 1/ub_diff;
177 
178  // grad = H*X + g + kappa * (ub_diff - lb_diff)
179  const double grad_el = X[j]*gain_position + g[i] + kappa * (ub_diff - lb_diff);
180  grad[i] = grad_el;
181 
182  // only elements 1:3:N*SMPC_NUM_STATE_VAR on the diagonal of hessian
183  // can change
184  // hess = 2H + kappa * (ub_diff^2 + lb_diff^2)
185  const double i2hess_el = 1/(gain_position + kappa * (ub_diff*ub_diff + lb_diff*lb_diff));
186  i2hess[i] = i2hess_el;
187 
188  i2hess_grad[j] = -grad_el * i2hess_el;
189  i2hess_grad[j+1] = - X[j+1]; //grad[j+1] * i2Q[1];
190  i2hess_grad[j+2] = - X[j+2]; //grad[j+2] * i2Q[2];
191  }
192 
193  for (int i = N*SMPC_NUM_STATE_VAR; i < N*SMPC_NUM_VAR; i+= SMPC_NUM_CONTROL_VAR)
194  {
195  i2hess_grad[i] = - X[i]; //grad[i] * i2P;
196  i2hess_grad[i+1] = - X[i+1]; //grad[i+1] * i2P;
197  }
198 
199  return (-kappa * phi_X_logbar);
200 }
201 
202 
203 
209 {
210  int i,j;
211  double phi_X_pos = 0;
212  double phi_X_vel = 0;
213  double phi_X_acc = 0;
214  double phi_X_jerk = 0;
215  double phi_X_gX = 0;
216 
217  // phi_X = X'*H*X + g'*X
218  for(i = 0, j = 0;
219  i < N*SMPC_NUM_STATE_VAR;
220  i += SMPC_NUM_STATE_VAR, j += 2)
221  {
222  const double X_copy[6] = {X[i], X[i+1], X[i+2], X[i+3], X[i+4], X[i+5]};
223 
224  // X'*H*X
225  phi_X_pos += X_copy[0]*X_copy[0] + X_copy[3]*X_copy[3];
226  phi_X_vel += X_copy[1]*X_copy[1] + X_copy[4]*X_copy[4];
227  phi_X_acc += X_copy[2]*X_copy[2] + X_copy[5]*X_copy[5];
228 
229  // g'*X
230  phi_X_gX += g[j]*X_copy[0] + g[j+1]*X_copy[3];
231  }
232  for (; i < N*SMPC_NUM_VAR; i += SMPC_NUM_CONTROL_VAR)
233  {
234  // X'*H*X
235  phi_X_jerk += X[i] * X[i] + X[i+1] * X[i+1];
236  }
237 
238  return (Q[0]*phi_X_pos + Q[1]*phi_X_vel + Q[2]*phi_X_acc + P*phi_X_jerk + phi_X_gX);
239 }
240 
241 
248 {
249  double min_alpha = 1.0;
250  double alpha = 1.0;
251 
252  for (int i = 0; i < 2*N; i++)
253  {
254  // lower bound may be violated
255  if (dX[i*3] < 0)
256  {
257  const double tmp_alpha = (lb[i]-X[i*3])/dX[i*3];
258  if (tmp_alpha < min_alpha)
259  {
260  min_alpha = tmp_alpha;
261  }
262  }
263  // upper bound may be violated
264  else if (dX[i*3] > 0)
265  {
266  const double tmp_alpha = (ub[i]-X[i*3])/dX[i*3];
267  if (tmp_alpha < min_alpha)
268  {
269  min_alpha = tmp_alpha;
270  }
271  }
272  }
273 
274  if (min_alpha > tol)
275  {
276  while (alpha > min_alpha)
277  {
278  alpha *= bs_beta;
279  }
280  }
281  else
282  {
283  alpha = 0;
284  }
285 
286  return (alpha);
287 }
288 
289 
290 
297 {
298  double res_pos = 0;
299  double res_vel = 0;
300  double res_acc = 0;
301  double res_jerk = 0;
302 
303  for (int i = 0, j = 0; i < N*SMPC_NUM_STATE_VAR; i += SMPC_NUM_STATE_VAR, j += 2)
304  {
306  {
307  res_pos += (X[i]*gain_position + g[j]) * dX[i]
308  + (X[i+3]*gain_position + g[j+1]) * dX[i+3];
309  }
310  else
311  {
312  res_pos += grad[j] * dX[i]
313  + grad[j+1] * dX[i+3];
314  }
315 
316  res_vel += X[i+1] * dX[i+1] //grad[i+1] * dX[i+1]
317  + X[i+4] * dX[i+4]; //grad[i+4] * dX[i+4]
318 
319  res_acc += X[i+2] * dX[i+2] //grad[i+2] * dX[i+2]
320  + X[i+5] * dX[i+5]; //grad[i+5] * dX[i+5];
321  }
322  for (int i = N*SMPC_NUM_STATE_VAR; i < N*SMPC_NUM_VAR; i += SMPC_NUM_CONTROL_VAR)
323  {
324  res_jerk += X[i] * dX[i] + X[i+1] * dX[i+1];
325  // grad[i+6] * dX[i+6]
326  // grad[i+7] * dX[i+7];
327  }
328 
329  return ((res_pos + res_vel/i2Q[1] + res_acc/i2Q[2] + res_jerk/i2P)*bs_alpha);
330 }
331 
332 
341 double qp_ip::form_phi_X_tmp (const double kappa, const double alpha)
342 {
343  int i,j;
344 
345 
346  // phi_X += X'*H*X
347  double res_gX = 0;
348  double res_pos = 0;
349  double res_vel = 0;
350  double res_acc = 0;
351  double res_jerk = 0;
352  double res_logbar = 1.0;
353  for (i = 0,j = 0; i < 2*N; i+=2, j += SMPC_NUM_STATE_VAR)
354  {
355  double X_tmp[6] = {
356  X[j] + alpha * dX[j],
357  X[j+1] + alpha * dX[j+1],
358  X[j+2] + alpha * dX[j+2],
359  X[j+3] + alpha * dX[j+3],
360  X[j+4] + alpha * dX[j+4],
361  X[j+5] + alpha * dX[j+5],
362  };
363 
364  // logarithmic barrier
365  res_logbar += log((-lb[i] + X_tmp[0]) * (ub[i] - X_tmp[0])
366  * (-lb[i+1] + X_tmp[3]) * (ub[i+1] - X_tmp[3]));
367 
368  // phi_X += g'*X
369  res_gX += g[i] * X_tmp[0] + g[i+1] * X_tmp[3];
370 
371  // phi_X += X'*H*X // states
372  res_pos += X_tmp[0]*X_tmp[0] + X_tmp[3]*X_tmp[3];
373  res_vel += X_tmp[1]*X_tmp[1] + X_tmp[4]*X_tmp[4];
374  res_acc += X_tmp[2]*X_tmp[2] + X_tmp[5]*X_tmp[5];
375  }
376  // phi_X += X'*H*X // controls
378  {
379  double X_tmp[2] = {
380  X[i] + alpha * dX[i],
381  X[i+1] + alpha * dX[i+1]
382  };
383 
384  res_jerk += X_tmp[0] * X_tmp[0] + X_tmp[1] * X_tmp[1];
385  }
386 
387  return (res_gX + Q[0]*res_pos + Q[1]*res_vel + Q[2]*res_acc + P*res_jerk - kappa * res_logbar);
388 }
389 
390 
391 
403  const double t_,
404  const double mu_,
405  const double bs_alpha_,
406  const double bs_beta_,
407  const unsigned int max_iter_,
408  const double tol_out_)
409 {
410  t = t_;
411  mu = mu_;
412  bs_alpha = bs_alpha_;
413  bs_beta = bs_beta_;
414  max_iter = max_iter_;
415  tol_out = tol_out_;
416 }
417 
418 
419 
427 void qp_ip::solve(vector<double> &obj_log)
428 {
429  if (obj_computation_on)
430  {
431  obj_log.clear();
432  obj_log.push_back(compute_obj(true));
433  }
434 
435  double kappa = 1/t;
436  double duality_gap = 2*N*kappa;
437 
438  int_loop_counter = 0;
439  ext_loop_counter = 0;
440  bs_counter = 0;
441 
442  for (;;)
443  {
445  while ((max_iter == 0) || (int_loop_counter < max_iter))
446  {
448  if(!solve_onestep(kappa, obj_log))
449  {
450  break;
451  }
452  }
453  if ((max_iter == 0) && (int_loop_counter == max_iter))
454  {
455  break;
456  }
457 
458  kappa /= mu;
459  duality_gap = 2*N*kappa;
460  if (duality_gap < tol_out)
461  {
462  break;
463  }
464  }
465 }
466 
467 
469 {
470  double decrement_pos = 0;
471  double decrement_vel = 0;
472  double decrement_acc = 0;
473  double decrement_jerk = 0;
474  for (int i = 0, j = 0; i < N*2; i += 2, j += SMPC_NUM_STATE_VAR)
475  {
476  decrement_pos += dX[j] * dX[j] / i2hess[i]
477  + dX[j+3] * dX[j+3] / i2hess[i+1];
478 
479  decrement_vel += dX[j+1] * dX[j+1]
480  + dX[j+4] * dX[j+4];
481 
482  decrement_acc += dX[j+2] * dX[j+2]
483  + dX[j+5] * dX[j+5];
484  }
485  for (int i = N*SMPC_NUM_STATE_VAR; i < N*SMPC_NUM_VAR; i += SMPC_NUM_CONTROL_VAR)
486  {
487  decrement_jerk += dX[i] * dX[i]
488  + dX[i+1] * dX[i+1];
489  }
490  return (decrement_pos + decrement_vel/i2Q[1] + decrement_acc/i2Q[2] + decrement_jerk/i2P);
491 }
492 
493 
494 
503 bool qp_ip::solve_onestep (const double kappa, vector<double> &obj_log)
504 {
506  double phi_X = 0.0;
507 
508  if (bs_type == SMPC_IP_BS_LOGBAR)
509  {
510  phi_X = form_grad_i2hess_logbar (kappa);
511  phi_X += form_phi_X ();
512  }
513  else
514  {
515  form_grad_i2hess_logbar (kappa);
517  {
518  phi_X = compute_obj(false);
519  }
520  }
521 
522 
523  chol.solve (*this, i2hess_grad, i2hess, X, dX);
524 
525 
526  // stopping criterion (decrement)
527  if (form_decrement () < tol)
528  {
529  return (false);
530  }
531 
532 
533  // A number from 0 to 1, which controls depth of descent #X = #X + #alpha*#dX.
534  double alpha = init_alpha ();
535  // stopping criterion (step size)
536  if (alpha < tol)
537  {
538  return (false); // done
539  }
540 
541 
542  // backtracking search
543  if (bs_type != SMPC_IP_BS_NONE)
544  {
545  double bs_kappa = kappa;
547  {
548  bs_kappa = 0.0; // eliminates logarithmic barrier
549  }
550  const double bs_alpha_grad_dX = form_bs_alpha_obj_dX ();
551  for (;;)
552  {
553  ++bs_counter;
554  if (form_phi_X_tmp (bs_kappa, alpha) <= phi_X + alpha * bs_alpha_grad_dX)
555  {
556  break;
557  }
558 
559  alpha = bs_beta * alpha;
560 
561  // stopping criterion (step size)
562  if (alpha < tol)
563  {
564  return (false); // done
565  }
566  }
567  }
568 
569 
570  // Move in the feasible descent direction
571  for (int i = 0; i < N*SMPC_NUM_VAR; i += SMPC_NUM_VAR)
572  {
573  X[i] += alpha * dX[i];
574  X[i+1] += alpha * dX[i+1];
575  X[i+2] += alpha * dX[i+2];
576  X[i+3] += alpha * dX[i+3];
577  X[i+4] += alpha * dX[i+4];
578  X[i+5] += alpha * dX[i+5];
579  X[i+6] += alpha * dX[i+6];
580  X[i+7] += alpha * dX[i+7];
581  }
582  if (obj_computation_on)
583  {
584  obj_log.push_back(compute_obj(true));
585  }
586 
587  return (true);
588 }
589 
590 
591 
604  const double *x_coord,
605  const double *y_coord,
606  const double *init_state,
607  const bool tilde_state,
608  double* X_)
609 {
610  X = X_;
611  form_init_fp_tilde<problem_parameters>(*this, x_coord, y_coord, init_state, tilde_state, X);
612 
613  // go back to bar states
614  double *cur_state = X;
615  for (int i=0; i<N; i++)
616  {
617  state_handling::tilde_to_bar (spar[i].sin, spar[i].cos, cur_state);
618  cur_state = &cur_state[SMPC_NUM_STATE_VAR];
619  }
620 }
621 
622 
631 double qp_ip::compute_obj(const bool add_constant_term)
632 {
633  int i,j;
634  double obj_pos = 0.0;
635  double obj_vel = 0.0;
636  double obj_acc = 0.0;
637  double obj_jerk = 0.0;
638  double obj_gX = 0.0;
639  double obj_ref = 0.0;
640 
641  // phi_X = X'*H*X + g'*X
642  for(i = 0, j = 0;
643  i < N*SMPC_NUM_STATE_VAR;
644  i += SMPC_NUM_STATE_VAR, j += 2)
645  {
646  const double X_copy[6] = {X[i], X[i+1], X[i+2], X[i+3], X[i+4], X[i+5]};
647 
648  // X'*H*X
649  obj_pos += X_copy[0]*X_copy[0] + X_copy[3]*X_copy[3];
650  obj_vel += X_copy[1]*X_copy[1] + X_copy[4]*X_copy[4];
651  obj_acc += X_copy[2]*X_copy[2] + X_copy[5]*X_copy[5];
652 
653  // g'*X
654  obj_gX += g[j]*X_copy[0] + g[j+1]*X_copy[3];
655 
656  if (add_constant_term)
657  {
658  const int k = i / SMPC_NUM_STATE_VAR;
659  obj_ref += zref_x[k]*zref_x[k] + zref_y[k]*zref_y[k];
660  }
661  }
662  for (; i < N*SMPC_NUM_VAR; i += SMPC_NUM_CONTROL_VAR)
663  {
664  // X'*H*X
665  obj_jerk += X[i] * X[i] + X[i+1] * X[i+1];
666  }
667 
668  return (Q[0]*obj_pos
669  + Q[1]*obj_vel
670  + Q[2]*obj_acc
671  + P*obj_jerk
672  + obj_gX
673  + Q[0]*obj_ref);
674 }
unsigned int ext_loop_counter
Definition: qp_ip.h:88
#define SMPC_NUM_STATE_VAR
Number of state variables.
Definition: smpc_solver.h:24
double Q[3]
Diagonal elements of H.
Definition: qp_ip.h:126
double * dX
Definition: qp_ip.h:106
double * i2hess
Inverted hessian: non-repeating diagonal elements 1:3:N*SMPC_NUM_STATE_VAR, 2*N in total.
Definition: qp_ip.h:113
bool solve_onestep(const double, vector< double > &)
One step of interior point method.
Definition: qp_ip.cpp:503
double tol
tolerance
Definition: qp_ip.h:97
void set_state_parameters(const double *, const double *, const double, const double *)
Initializes quadratic problem.
const double * ub
lower and upper bounds
Definition: qp_ip.h:138
A set of problem parameters.
double form_grad_i2hess_logbar(const double)
Compute gradient of phi (partially), varying elements of i2hess, logarithmic barrier part of phi,...
Definition: qp_ip.cpp:156
unsigned int int_loop_counter
Definition: qp_ip.h:87
Use objective function, which includes logarithmic barrier term.
Definition: smpc_solver.h:425
#define SMPC_NUM_VAR
Total number of variables.
Definition: smpc_solver.h:28
double init_alpha()
tolerance of the outer loop
Definition: qp_ip.cpp:247
void set_ip_parameters(const double, const double, const double, const double, const unsigned int, const double)
Set parameters of interior-point method.
Definition: qp_ip.cpp:402
unsigned int max_iter
backtracking search parameter beta
Definition: qp_ip.h:150
void set_parameters(const double *, const double *, const double, const double *, const double *, const double *, const double *, const double *)
Initializes quadratic problem.
Definition: qp_ip.cpp:97
Use original objective function (no logarithmic barrier term).
Definition: smpc_solver.h:427
const double * zref_x
Definition: qp_ip.h:141
bool obj_computation_on
Definition: qp_ip.h:99
~qp_ip()
Definition: qp_ip.cpp:71
double * i2hess_grad
Inverted hessian * gradient (N*SMPC_NUM_VAR vector)
Definition: qp_ip.h:116
void tilde_to_bar(const double sinA, const double cosA, double *state)
Converts state from X_tilde to X_bar.
double t
Definition: qp_ip.h:146
void solve(vector< double > &)
Solve QP using interior-point method.
Definition: qp_ip.cpp:427
IP::chol_solve chol
An instance of IP::chol_solve class.
Definition: qp_ip.h:132
backtrackingSearchType
Type of the backtracking search.
Definition: smpc_solver.h:420
state_parameters * spar
#define SMPC_NUM_CONTROL_VAR
Number of control variables.
Definition: smpc_solver.h:26
double tol_out
maximum number of internal loop iterations (in total)
Definition: qp_ip.h:151
double mu
logarithmic barrier parameter
Definition: qp_ip.h:147
double form_bs_alpha_obj_dX()
Forms bs_alpha * (objective') * dX.
Definition: qp_ip.cpp:296
double * X
Definition: qp_ip.h:84
double bs_beta
backtracking search parameter alpha
Definition: qp_ip.h:149
double * grad
2*N gradient vector, only the elements that correspond to the ZMP positions are computed,...
Definition: qp_ip.h:121
backtrackingSearchType bs_type
Definition: qp_ip.h:100
unsigned int bs_counter
Definition: qp_ip.h:89
const double * zref_y
Definition: qp_ip.h:142
void form_g(const double *, const double *)
Forms vector g.
Definition: qp_ip.cpp:126
double compute_obj(const bool)
Computes value of the objective function.
Definition: qp_ip.cpp:631
double form_phi_X_tmp(const double, const double)
Forms phi(X+alpha*dX)
Definition: qp_ip.cpp:341
double form_decrement()
Definition: qp_ip.cpp:468
double * g
2*N non-zero elements of vector g.
Definition: qp_ip.h:109
double form_phi_X()
Compute phi_X for initial point, phi_X must already store logarithmic barrier term.
Definition: qp_ip.cpp:208
double P
Diagonal elements of H.
Definition: qp_ip.h:127
const double * lb
lower and upper bounds
Definition: qp_ip.h:137
void solve(const problem_parameters &, const double *, const double *, const double *, double *)
Determines feasible descent direction.
void form_init_fp(const double *, const double *, const double *, const bool, double *)
Generates an initial feasible point. First we perform a change of variable to X_tilde generate a feas...
Definition: qp_ip.cpp:603
double gain_position
Definition: qp_ip.h:94
double bs_alpha
multiplier of t, >1.
Definition: qp_ip.h:148
qp_ip(const int N_, const double, const double, const double, const double, const double, const bool, const backtrackingSearchType)
Constructor: initialization of the constant parameters.
Definition: qp_ip.cpp:38
Disable backtracking search.
Definition: smpc_solver.h:423