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:

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_parameters *	spar

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:NSMPC_NUM_STATE_VAR, 2N 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_term the 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] kappa 1/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_coord	x coordinates of points satisfying constraints
[in]	y_coord	y coordinates of points satisfying constraints
[in]	init_state	current state
[in]	tilde_state	if 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]	kappa	logarithmic barrier multiplicator.
[in]	alpha	step 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]	T	Sampling time (for the moment it is assumed to be constant) [sec.]
[in]	h	Height of the Center of Mass divided by gravity
[in]	h_initial_	current h
[in]	angle	Rotation 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_log a 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]	kappa	logarithmic barrier multiplier
[in,out]	obj_log	a 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:

solver/qp_ip.h
solver/qp_ip.cpp

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ qp_ip()

◆ ~qp_ip()

Member Function Documentation

◆ compute_obj()

◆ form_bs_alpha_obj_dX()

◆ form_decrement()

◆ form_g()

◆ form_grad_i2hess_logbar()

◆ form_init_fp()

◆ form_phi_X()

◆ form_phi_X_tmp()

◆ init_alpha()

◆ set_ip_parameters()

◆ set_parameters()

◆ solve()

◆ solve_onestep()

Member Data Documentation

◆ bs_alpha

◆ bs_beta

◆ bs_counter

◆ bs_type

◆ chol

◆ dX

◆ ext_loop_counter

◆ g

◆ gain_position

◆ grad

◆ i2hess

◆ i2hess_grad

◆ int_loop_counter

◆ lb

◆ max_iter

◆ mu

◆ obj_computation_on

◆ P

◆ Q

◆ t

◆ tol

◆ tol_out

◆ ub

◆ X

◆ zref_x

◆ zref_y