A sparse MPC solver for walking motion generation.
Public Member Functions | Public Attributes | Private Member Functions | Private Attributes | List of all members
qp_ip Class Reference

Solve a quadratic program with a specific structure. qp_ip = Quadratic Programming / Interior-point method. More...

#include <qp_ip.h>

Inheritance diagram for qp_ip:
IP::problem_parameters

Public Member Functions

 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. More...
 
 ~qp_ip ()
 
void set_parameters (const double *, const double *, const double, const double *, const double *, const double *, const double *, const double *)
 Initializes quadratic problem. More...
 
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 feasible point, and then we go back to X_bar. More...
 
void set_ip_parameters (const double, const double, const double, const double, const unsigned int, const double)
 Set parameters of interior-point method. More...
 
void solve (vector< double > &)
 Solve QP using interior-point method. More...
 
- Public Member Functions inherited from IP::problem_parameters
 problem_parameters (const int, const double, const double, const double, const double)
 
 ~problem_parameters ()
 
void set_state_parameters (const double *, const double *, const double, const double *)
 Initializes quadratic problem. More...
 

Public Attributes

double * X
 
unsigned int int_loop_counter
 
unsigned int ext_loop_counter
 
unsigned int bs_counter
 
- Public Attributes inherited from IP::problem_parameters
int N
 
double h_initial
 Height of the CoM at initial state divided by the gravity, this initial state precede the first state in the preview window. More...
 
state_parametersspar
 
double i2Q [3]
 
double i2P
 

Private Member Functions

double init_alpha ()
 tolerance of the outer loop More...
 
double form_bs_alpha_obj_dX ()
 Forms bs_alpha * (objective') * dX. More...
 
double form_phi_X_tmp (const double, const double)
 Forms phi(X+alpha*dX) More...
 
bool solve_onestep (const double, vector< double > &)
 One step of interior point method. More...
 
void form_g (const double *, const double *)
 Forms vector g. More...
 
double form_grad_i2hess_logbar (const double)
 Compute gradient of phi (partially), varying elements of i2hess, logarithmic barrier part of phi, i2hess_grad = -i2hess*grad. More...
 
double form_phi_X ()
 Compute phi_X for initial point, phi_X must already store logarithmic barrier term. More...
 
double form_decrement ()
 
double compute_obj (const bool)
 Computes value of the objective function. More...
 

Private Attributes

double gain_position
 
double tol
 tolerance More...
 
bool obj_computation_on
 
backtrackingSearchType bs_type
 
double * dX
 
double * g
 2*N non-zero elements of vector g. More...
 
double * i2hess
 Inverted hessian: non-repeating diagonal elements 1:3:N*SMPC_NUM_STATE_VAR, 2*N in total. More...
 
double * i2hess_grad
 Inverted hessian * gradient (N*SMPC_NUM_VAR vector) More...
 
double * grad
 2*N gradient vector, only the elements that correspond to the ZMP positions are computed, it is faster to compute the others on the fly. Hint: the computed terms are affected by the logarithmic barrier. More...
 
IP::chol_solve chol
 An instance of IP::chol_solve class. More...
 
const double * zref_x
 
const double * zref_y
 
double t
 
double mu
 logarithmic barrier parameter More...
 
double bs_alpha
 multiplier of t, >1. More...
 
double bs_beta
 backtracking search parameter alpha More...
 
unsigned int max_iter
 backtracking search parameter beta More...
 
double tol_out
 maximum number of internal loop iterations (in total) More...
 
double Q [3]
 Diagonal elements of H. More...
 
double P
 Diagonal elements of H. More...
 
const double * lb
 lower and upper bounds More...
 
const double * ub
 lower and upper bounds More...
 

Detailed Description

Solve a quadratic program with a specific structure. qp_ip = Quadratic Programming / Interior-point method.

Definition at line 38 of file qp_ip.h.

Constructor & Destructor Documentation

◆ qp_ip()

qp_ip::qp_ip ( const int  N_,
const double  gain_position_,
const double  gain_velocity_,
const double  gain_acceleration_,
const double  gain_jerk_,
const double  tol_,
const bool  obj_computation_on_,
const backtrackingSearchType  bs_type_ 
)

Constructor: initialization of the constant parameters.

Parameters
[in]N_Number of sampling times in a preview window
[in]gain_position_(Alpha) Position gain
[in]gain_velocity_(Beta) Velocity gain
[in]gain_acceleration_(Gamma) Acceleration gain
[in]gain_jerk_(Eta) Jerk gain
[in]tol_tolerance
[in]obj_computation_on_enable computation of the objective function
[in]bs_type_type of backtracking search

Definition at line 38 of file qp_ip.cpp.

◆ ~qp_ip()

qp_ip::~qp_ip ( )

Destructor

Definition at line 71 of file qp_ip.cpp.

Member Function Documentation

◆ compute_obj()

double qp_ip::compute_obj ( const bool  add_constant_term)
private

Computes value of the objective function.

Parameters
[in]add_constant_termthe objective function contains a constant term, that can be dropped in some cases.
Returns
value of the objective function.

Definition at line 631 of file qp_ip.cpp.

◆ form_bs_alpha_obj_dX()

double qp_ip::form_bs_alpha_obj_dX ( )
private

Forms bs_alpha * (objective') * dX.

Returns
result of multiplication.

Definition at line 296 of file qp_ip.cpp.

◆ form_decrement()

double qp_ip::form_decrement ( )
private

Definition at line 468 of file qp_ip.cpp.

◆ form_g()

void qp_ip::form_g ( const double *  zref_x_,
const double *  zref_y_ 
)
private

Forms vector g.

Parameters
[in]zref_x_x coordinates of reference ZMP positions
[in]zref_y_y coordinates of reference ZMP positions

Definition at line 126 of file qp_ip.cpp.

◆ form_grad_i2hess_logbar()

double qp_ip::form_grad_i2hess_logbar ( const double  kappa)
private

Compute gradient of phi (partially), varying elements of i2hess, logarithmic barrier part of phi, i2hess_grad = -i2hess*grad.

Parameters
[in]kappa1/t, a logarithmic barrier multiplicator.
Returns
logarithmic barrier part of phi.

Definition at line 156 of file qp_ip.cpp.

◆ form_init_fp()

void qp_ip::form_init_fp ( const double *  x_coord,
const double *  y_coord,
const double *  init_state,
const bool  tilde_state,
double *  X_ 
)

Generates an initial feasible point. First we perform a change of variable to X_tilde generate a feasible point, and then we go back to X_bar.

Parameters
[in]x_coordx coordinates of points satisfying constraints
[in]y_coordy coordinates of points satisfying constraints
[in]init_statecurrent state
[in]tilde_stateif true the state is assumed to be in X_tilde form
[in,out]X_initial guess / solution of optimization problem

Definition at line 603 of file qp_ip.cpp.

◆ form_phi_X()

double qp_ip::form_phi_X ( )
private

Compute phi_X for initial point, phi_X must already store logarithmic barrier term.

Definition at line 208 of file qp_ip.cpp.

◆ form_phi_X_tmp()

double qp_ip::form_phi_X_tmp ( const double  kappa,
const double  alpha 
)
private

Forms phi(X+alpha*dX)

Parameters
[in]kappalogarithmic barrier multiplicator.
[in]alphastep length
Returns
a value of phi.

Definition at line 341 of file qp_ip.cpp.

◆ init_alpha()

double qp_ip::init_alpha ( )
private

tolerance of the outer loop

Find initial value of alpha.

Note
sets alpha to 0, if it is too small.

Definition at line 247 of file qp_ip.cpp.

◆ set_ip_parameters()

void qp_ip::set_ip_parameters ( const double  t_,
const double  mu_,
const double  bs_alpha_,
const double  bs_beta_,
const unsigned int  max_iter_,
const double  tol_out_ 
)

Set parameters of interior-point method.

Parameters
[in]t_logarithmic barrier parameter
[in]mu_multiplier of t, >1.
[in]bs_alpha_backtracking search parameter alpha
[in]bs_beta_backtracking search parameter beta
[in]max_iter_maximum number of internal loop iterations
[in]tol_out_tolerance of the outer loop

Definition at line 402 of file qp_ip.cpp.

◆ set_parameters()

void qp_ip::set_parameters ( const double *  T,
const double *  h,
const double  h_initial_,
const double *  angle,
const double *  zref_x_,
const double *  zref_y_,
const double *  lb_,
const double *  ub_ 
)

Initializes quadratic problem.

Parameters
[in]TSampling time (for the moment it is assumed to be constant) [sec.]
[in]hHeight of the Center of Mass divided by gravity
[in]h_initial_current h
[in]angleRotation angle for each state in the preview window
[in]zref_x_reference values of z_x
[in]zref_y_reference values of z_y
[in]lb_array of lower bounds for z_x and z_y
[in]ub_array of upper bounds for z_x and z_y

Definition at line 97 of file qp_ip.cpp.

◆ solve()

void qp_ip::solve ( vector< double > &  obj_log)

Solve QP using interior-point method.

Parameters
[in,out]obj_loga vector of objective function values
Returns
0 if ok, negative number otherwise.

Definition at line 427 of file qp_ip.cpp.

◆ solve_onestep()

bool qp_ip::solve_onestep ( const double  kappa,
vector< double > &  obj_log 
)
private

One step of interior point method.

Parameters
[in]kappalogarithmic barrier multiplier
[in,out]obj_loga vector of objective function values
Returns
true if a step was made, false if alpha or dX are too small.

Value of phi(X), where phi is the cost function + log barrier.

Definition at line 503 of file qp_ip.cpp.

Member Data Documentation

◆ bs_alpha

double qp_ip::bs_alpha
private

multiplier of t, >1.

Definition at line 148 of file qp_ip.h.

◆ bs_beta

double qp_ip::bs_beta
private

backtracking search parameter alpha

Definition at line 149 of file qp_ip.h.

◆ bs_counter

unsigned int qp_ip::bs_counter

Definition at line 89 of file qp_ip.h.

◆ bs_type

backtrackingSearchType qp_ip::bs_type
private

Definition at line 100 of file qp_ip.h.

◆ chol

IP::chol_solve qp_ip::chol
private

An instance of IP::chol_solve class.

Definition at line 132 of file qp_ip.h.

◆ dX

double* qp_ip::dX
private

Feasible descent direction (to be used for updating X).

Definition at line 106 of file qp_ip.h.

◆ ext_loop_counter

unsigned int qp_ip::ext_loop_counter

Definition at line 88 of file qp_ip.h.

◆ g

double* qp_ip::g
private

2*N non-zero elements of vector g.

Definition at line 109 of file qp_ip.h.

◆ gain_position

double qp_ip::gain_position
private

Definition at line 94 of file qp_ip.h.

◆ grad

double* qp_ip::grad
private

2*N gradient vector, only the elements that correspond to the ZMP positions are computed, it is faster to compute the others on the fly. Hint: the computed terms are affected by the logarithmic barrier.

Definition at line 121 of file qp_ip.h.

◆ i2hess

double* qp_ip::i2hess
private

Inverted hessian: non-repeating diagonal elements 1:3:N*SMPC_NUM_STATE_VAR, 2*N in total.

Definition at line 113 of file qp_ip.h.

◆ i2hess_grad

double* qp_ip::i2hess_grad
private

Inverted hessian * gradient (N*SMPC_NUM_VAR vector)

Definition at line 116 of file qp_ip.h.

◆ int_loop_counter

unsigned int qp_ip::int_loop_counter

Definition at line 87 of file qp_ip.h.

◆ lb

const double* qp_ip::lb
private

lower and upper bounds

Definition at line 137 of file qp_ip.h.

◆ max_iter

unsigned int qp_ip::max_iter
private

backtracking search parameter beta

Definition at line 150 of file qp_ip.h.

◆ mu

double qp_ip::mu
private

logarithmic barrier parameter

Definition at line 147 of file qp_ip.h.

◆ obj_computation_on

bool qp_ip::obj_computation_on
private

Definition at line 99 of file qp_ip.h.

◆ P

double qp_ip::P
private

Diagonal elements of H.

Definition at line 127 of file qp_ip.h.

◆ Q

double qp_ip::Q[3]
private

Diagonal elements of H.

Definition at line 126 of file qp_ip.h.

◆ t

double qp_ip::t
private

Definition at line 146 of file qp_ip.h.

◆ tol

double qp_ip::tol
private

tolerance

Definition at line 97 of file qp_ip.h.

◆ tol_out

double qp_ip::tol_out
private

maximum number of internal loop iterations (in total)

Definition at line 151 of file qp_ip.h.

◆ ub

const double* qp_ip::ub
private

lower and upper bounds

Definition at line 138 of file qp_ip.h.

◆ X

double* qp_ip::X

Variables for the QP (contain the states + control variables). Initial feasible point with respect to the equality and inequality constraints.

Definition at line 84 of file qp_ip.h.

◆ zref_x

const double* qp_ip::zref_x
private

Definition at line 141 of file qp_ip.h.

◆ zref_y

const double* qp_ip::zref_y
private

Definition at line 142 of file qp_ip.h.


The documentation for this class was generated from the following files: