Inheritance diagram for quadNLP:

Collaboration diagram for quadNLP:

[legend]

Public Member Functions
	quadNLP (SystemID default_system, int N, double dt, double mu, double panic_weights, double constraint_panic_weights, double Q_temporal_factor, double R_temporal_factor, const Eigen::VectorXi &fixed_complexity_schedule, const NLPConfig &config)

	quadNLP (const quadNLP &nlp)
	Custom deep copy constructor.

virtual	~quadNLP ()

Overloaded from TNLP
virtual bool	get_nlp_info (Index &n, Index &m, Index &nnz_jac_g, Index &nnz_h_lag, IndexStyleEnum &index_style)

bool	get_bounds_info_single_complex_fe (int i, Eigen::VectorXd &x_lb, Eigen::VectorXd &x_ub, Eigen::VectorXd &u_lb, Eigen::VectorXd &u_ub, Eigen::VectorXd &g_l, Eigen::VectorXd &g_u)

virtual bool	get_bounds_info (Index n, Number x_l, Number x_u, Index m, Number g_l, Number g_u)

virtual bool	get_starting_point (Index n, bool init_x, Number x, bool init_z, Number z_L, Number z_U, Index m, bool init_lambda, Number lambda)

virtual bool	eval_f (Index n, const Number *x, bool new_x, Number &obj_value)

virtual bool	eval_grad_f (Index n, const Number x, bool new_x, Number grad_f)

Eigen::VectorXd	eval_g_single_complex_fe (int i, const Eigen::VectorXd &x0, const Eigen::VectorXd &u, const Eigen::VectorXd &x1)

virtual bool	eval_g (Index n, const Number x, bool new_x, Index m, Number g)

virtual bool	eval_jac_g (Index n, const Number x, bool new_x, Index m, Index nele_jac, Index iRow, Index jCol, Number values)

virtual void	compute_nnz_jac_g ()

virtual bool	eval_h (Index n, const Number x, bool new_x, Number obj_factor, Index m, const Number lambda, bool new_lambda, Index nele_hess, Index iRow, Index jCol, Number *values)

virtual void	compute_nnz_h ()

virtual void	finalize_solution (SolverReturn status, Index n, const Number x, const Number z_L, const Number z_U, Index m, const Number g, const Number lambda, Number obj_value, const IpoptData ip_data, IpoptCalculatedQuantities *ip_cq)

virtual void	update_initial_guess (const quadNLP &nlp_prev, int shift_idx)

virtual void	update_solver (const Eigen::VectorXd &initial_state, const Eigen::MatrixXd &ref_traj, const Eigen::MatrixXd &foot_positions, const std::vector< std::vector< bool > > &contact_schedule, const Eigen::VectorXi &adaptive_complexity_schedule, const Eigen::VectorXd &ground_height, const double &first_element_duration_, int plan_index_diff, const bool &init)

void	update_structure ()

void	get_lifted_trajectory (Eigen::MatrixXd &state_traj_lifted, Eigen::MatrixXd &control_traj_lifted)

void	get_heuristic_trajectory (Eigen::MatrixXd &state_traj_heuristic, Eigen::MatrixXd &control_traj_heuristic)

template<typename T >
Eigen::Block< T >	get_primal_state_var (T &decision_var, const int &idx) const

template<typename T >
Eigen::Block< T >	get_primal_body_state_var (T &decision_var, const int &idx) const

template<typename T >
Eigen::Block< T >	get_primal_foot_state_var (T &decision_var, const int &idx) const

template<typename T >
Eigen::Block< T >	get_primal_joint_state_var (T &decision_var, const int &idx) const

template<typename T >
Eigen::Block< T >	get_primal_control_var (T &decision_var, const int &idx) const

template<typename T >
Eigen::Block< T >	get_primal_body_control_var (T &decision_var, const int &idx) const

template<typename T >
Eigen::Block< T >	get_primal_foot_control_var (T &decision_var, const int &idx) const

template<typename T >
Eigen::Block< T >	get_slack_state_var (T &decision_var, const int &idx) const

template<typename T >
Eigen::Block< T >	get_slack_constraint_var (T &decision_var, const int &idx) const

template<typename T >
Eigen::Block< T >	get_primal_constraint_vals (T &constraint_vals, const int &idx) const

template<typename T >
Eigen::Block< T >	get_relaxed_primal_constraint_vals (T &constraint_vals, const int &idx) const

template<typename T >
Eigen::Block< T >	get_slack_constraint_vals (T &constraint_vals, const int &idx) const

template<typename T >
Eigen::Block< T >	get_dynamic_var (T &decision_var, const int &idx) const

template<typename T >
Eigen::Block< T >	get_dynamic_jac_var (T &jacobian_var, const int &idx) const

template<typename T >
Eigen::Block< T >	get_relaxed_dynamic_jac_var (T &jacobian_var, const int &idx) const

template<typename T >
Eigen::Block< T >	get_slack_jac_var (T &jacobian_var, const int &idx) const

template<typename T >
Eigen::Block< T >	get_dynamic_hess_var (T &hessian_var, const int &idx) const

template<typename T >
Eigen::Block< T >	get_state_cost_hess_var (T &hessian_var, const int &idx) const

template<typename T >
Eigen::Block< T >	get_control_cost_hess_var (T &hessian_var, const int &idx) const

int	get_primal_constraint_idx (int idx) const
	Return the first index of the constraint vector corresponding to the given finite element.

int	get_slack_var_constraint_idx (int idx) const
	Return the first index of the slack constraint vector corresponding to the given finite element.

int	get_relaxed_constraint_idx (int idx) const
	Return the first index of the slack constraint vector corresponding to the given finite element.

int	get_primal_idx (int idx) const
	Return the first index of the decision variable vector corresponding to the given finite element.

int	get_primal_state_idx (int idx) const
	Return the first index of the decision variable vector corresponding to the given finite element's state.

int	get_primal_control_idx (int idx) const
	Return the first index of the decision variable vector corresponding to the given finite element's control input.

int	get_slack_state_idx (int idx) const
	Return the first index of the decision variable vector corresponding to the slack variable for the given finite element's state.

int	get_slack_constraint_var_idx (int idx) const
	Return the first index of the decision variable vector corresponding to the slack variable for the given finite element's state.

void	loadCasadiFuncs ()
	Load the casadi function pointers into map member vars.

void	loadConstraintNames ()
	Load the constraint names for debugging.

Public Attributes
NLPDiagnostics	diagnostics_
	Diagnostics struct for gathering metadata.

NLPConfig	config_
	Config struct for storing meta parameters.

SystemID	default_system_
	Default system used for NLP (if not mixed complexity)

std::shared_ptr< quad_utils::QuadKD >	quadKD_

int	N_

int	g_relaxed_

const int	n_body_ = 12

const int	n_foot_ = 24

const int	n_joints_ = 24

const int	n_tail_ = 4

const int	m_body_ = 12

const int	m_foot_ = 24

const int	m_tail_ = 2

Eigen::VectorXi	n_vec_
	Vectors of state and constraint dimension for each finite element.

Eigen::VectorXi	n_slack_vec_

Eigen::VectorXi	m_vec_

Eigen::VectorXi	g_vec_

Eigen::VectorXi	g_slack_vec_

Eigen::VectorXi	n_cost_vec_

Eigen::VectorXi	m_cost_vec_

const bool	apply_slack_to_complex_constr_ = true
	Boolean for whether to apply panic variables for complex constraints.

const bool	always_constrain_feet_ = false
	Boolean for whether to allow modifications of foot trajectory.

const bool	use_terrain_constraint_ = false
	Boolean for whether to include the terrain in the foot height constraint.

const bool	remember_complex_elements_ = true
	Boolean for whether to include the terrain in the foot height constraint.

const grid_map::InterpolationMethods	interp_type_

std::vector< std::vector< std::string > >	constr_names_
	Map for constraint names.

int	n_vars_
	Number of variables , primal variables, slack variables, and constraints.

int	n_vars_primal_

int	n_vars_slack_

int	n_constraints_

int	n_null_
	Number of state and control variables added in complex model.

int	m_null_

Eigen::VectorXd	x_null_nom_
	Nominal null state variables.

double	Q_temporal_factor_

double	R_temporal_factor_

Eigen::MatrixXd	foot_pos_body_

Eigen::MatrixXd	foot_pos_world_

Eigen::MatrixXd	foot_vel_world_

double	dt_

double	mu_

const double	mass_ = 13.3
	Mass of the platform (set to zero to ignore nominal ff)

const double	foot_mass_ = 0.01
	Mass of the feet (as modeled in casadi)

const double	grav_ = 9.81
	Gravity constant.

const int	num_feet_ = 4
	Number of feet.

grid_map::GridMap	terrain_
	Terrain map.

Eigen::VectorXd	g_min_complex_soft_

Eigen::VectorXd	g_max_complex_soft_

Eigen::MatrixXd	ground_height_

Eigen::VectorXd	w0_

Eigen::VectorXd	z_L0_

Eigen::VectorXd	z_U0_

Eigen::VectorXd	lambda0_

Eigen::VectorXd	g0_

double	mu0_
	Friction coefficient.

bool	warm_start_
	Whether warm start.

Eigen::MatrixXd	x_reference_

Eigen::VectorXd	x_current_

Eigen::MatrixXi	contact_sequence_

int	nnz_jac_g_

std::vector< int >	first_step_idx_jac_g_

Eigen::VectorXi	iRow_jac_g_

Eigen::VectorXi	jCol_jac_g_

int	nnz_h_

int	nnz_compact_h_

Eigen::VectorXi	nnz_step_jac_g_

Eigen::VectorXi	nnz_step_hess_

std::vector< int >	first_step_idx_hess_g_

Eigen::VectorXi	iRow_h_

Eigen::VectorXi	jCol_h_

Eigen::VectorXi	iRow_compact_h_

Eigen::VectorXi	jCol_compact_h_

double	panic_weights_

double	constraint_panic_weights_

double	first_element_duration_

Eigen::VectorXi	adaptive_complexity_schedule_
	Vector of ids for adaptive model complexity schedule.

Eigen::VectorXi	fixed_complexity_schedule_
	Vector of ids for fixed model complexity schedule.

Eigen::VectorXi	fe_idxs_
	Vector of indices for relevant quantities.

Eigen::VectorXi	u_idxs_

Eigen::VectorXi	x_idxs_

Eigen::VectorXi	slack_state_var_idxs_

Eigen::VectorXi	slack_constraint_var_idxs_

Eigen::VectorXi	primal_constraint_idxs_

Eigen::VectorXi	slack_var_constraint_idxs_

Eigen::VectorXi	dynamic_jac_var_idxs_

Eigen::VectorXi	relaxed_dynamic_jac_var_idxs_

Eigen::VectorXi	panic_jac_var_idxs_

Eigen::VectorXi	dynamic_hess_var_idxs_

Eigen::VectorXi	cost_idxs_

Eigen::VectorXi	relaxed_constraint_idxs_

Eigen::ArrayXi	relaxed_primal_constraint_idxs_in_element_
	Indices of relaxed constraints.

Eigen::ArrayXi	constraint_panic_weights_vec_

Eigen::VectorXi	sys_id_schedule_
	Vector of system ids.

Eigen::MatrixXi	nnz_mat_
	Matrix of function info.

Eigen::MatrixXi	nrow_mat_

Eigen::MatrixXi	ncol_mat_

Eigen::MatrixXi	first_step_idx_mat_

Eigen::MatrixXi	relaxed_nnz_mat_

std::vector< std::vector< Eigen::VectorXi > >	iRow_mat_
	Matrix of function sparsity data.

std::vector< std::vector< Eigen::VectorXi > >	jCol_mat_

std::vector< std::vector< Eigen::VectorXi > >	iRow_mat_relaxed_

std::vector< std::vector< Eigen::VectorXi > >	jCol_mat_relaxed_

std::vector< std::vector< Eigen::VectorXi > >	relaxed_idx_in_full_sparse_

std::vector< casadi_int >	nnz_jac_g_vec_
	Vector of nonzero entries for constraint Jacobian and Hessian.

std::vector< casadi_int >	nnz_h_vec_

std::vector< std::vector< decltype(eval_g_spirit) * > >	eval_vec_

std::vector< std::vector< decltype(eval_g_spirit_work) * > >	eval_work_vec_

std::vector< std::vector< decltype(eval_g_spirit_incref) * > >	eval_incref_vec_

std::vector< std::vector< decltype(eval_g_spirit_decref) * > >	eval_decref_vec_

std::vector< std::vector< decltype(eval_g_spirit_checkout) * > >	eval_checkout_vec_

std::vector< std::vector< decltype(eval_g_spirit_release) * > >	eval_release_vec_

std::vector< std::vector< decltype(eval_g_spirit_sparsity_out) * > >	eval_sparsity_vec_

Static Public Attributes
static const int	num_sys_id_ = 6

static const int	num_func_id_ = 3

Private Member Functions
Methods to block default compiler methods.
The compiler automatically generates the following three methods. Since the default compiler implementation is generally not what you want (for all but the most simple classes), we usually put the declarations of these methods in the private section and never implement them. This prevents the compiler from implementing an incorrect "default" behavior without us knowing. (See Scott Meyers book, "Effective C++")
quadNLP &	operator= (const quadNLP &)

Constructor & Destructor Documentation

◆ quadNLP() [1/2]

quadNLP::quadNLP	(	SystemID	default_system,
		int	N,
		double	dt,
		double	mu,
		double	panic_weights,
		double	constraint_panic_weights,
		double	Q_temporal_factor,
		double	R_temporal_factor,
		const Eigen::VectorXi &	fixed_complexity_schedule,
		const NLPConfig &	config
	)

Default constructor

◆ quadNLP() [2/2]

quadNLP::quadNLP ( const quadNLP & nlp )

Custom deep copy constructor.

Parameters

[in] nlp Object to be deep copied

◆ ~quadNLP()

quadNLP::~quadNLP ( )

virtual

Default destructor

Member Function Documentation

◆ eval_f()

bool quadNLP::eval_f	(	Index	n,
		const Number *	x,
		bool	new_x,
		Number &	obj_value
	)

virtual

Method to return the objective value

◆ eval_g()

bool quadNLP::eval_g	(	Index	n,
		const Number *	x,
		bool	new_x,
		Index	m,
		Number *	g
	)

virtual

Method to return the constraint residuals

◆ eval_g_single_complex_fe()

Eigen::VectorXd quadNLP::eval_g_single_complex_fe	(	int	i,
		const Eigen::VectorXd &	x0,
		const Eigen::VectorXd &	u,
		const Eigen::VectorXd &	x1
	)

Method to return the constraint residual for requested data

◆ eval_grad_f()

bool quadNLP::eval_grad_f	(	Index	n,
		const Number *	x,
		bool	new_x,
		Number *	grad_f
	)

virtual

Method to return the gradient of the objective

◆ eval_h()

bool quadNLP::eval_h	(	Index	n,
		const Number *	x,
		bool	new_x,
		Number	obj_factor,
		Index	m,
		const Number *	lambda,
		bool	new_lambda,
		Index	nele_hess,
		Index *	iRow,
		Index *	jCol,
		Number *	values
	)

virtual

Method to return: 1) The structure of the hessian of the lagrangian (if "values" is NULL) 2) The values of the hessian of the lagrangian (if "values" is not NULL)

◆ eval_jac_g()

bool quadNLP::eval_jac_g	(	Index	n,
		const Number *	x,
		bool	new_x,
		Index	m,
		Index	nele_jac,
		Index *	iRow,
		Index *	jCol,
		Number *	values
	)

virtual

Method to return: 1) The structure of the jacobian (if "values" is NULL) 2) The values of the jacobian (if "values" is not NULL)

◆ finalize_solution()

void quadNLP::finalize_solution	(	SolverReturn	status,
		Index	n,
		const Number *	x,
		const Number *	z_L,
		const Number *	z_U,
		Index	m,
		const Number *	g,
		const Number *	lambda,
		Number	obj_value,
		const IpoptData *	ip_data,
		IpoptCalculatedQuantities *	ip_cq
	)

virtual

This method is called when the algorithm is complete so the TNLP can store/write the solution

◆ get_bounds_info()

bool quadNLP::get_bounds_info	(	Index	n,
		Number *	x_l,
		Number *	x_u,
		Index	m,
		Number *	g_l,
		Number *	g_u
	)

virtual

Method to return the bounds for my problem

◆ get_bounds_info_single_complex_fe()

bool quadNLP::get_bounds_info_single_complex_fe	(	int	i,
		Eigen::VectorXd &	x_lb,
		Eigen::VectorXd &	x_ub,
		Eigen::VectorXd &	u_lb,
		Eigen::VectorXd &	u_ub,
		Eigen::VectorXd &	g_l,
		Eigen::VectorXd &	g_u
	)

Method to return the bounds for my problem

◆ get_nlp_info()

bool quadNLP::get_nlp_info	(	Index &	n,
		Index &	m,
		Index &	nnz_jac_g,
		Index &	nnz_h_lag,
		IndexStyleEnum &	index_style
	)

virtual

Method to return some info about the NLP

◆ get_primal_constraint_idx()

int quadNLP::get_primal_constraint_idx ( int idx ) const

inline

Return the first index of the constraint vector corresponding to the given finite element.

Parameters

[in] idx Index of requested finite element

Returns: Index in constraint vector corresponding to the beginning of the requested fe

◆ get_primal_control_idx()

int quadNLP::get_primal_control_idx ( int idx ) const

inline

Return the first index of the decision variable vector corresponding to the given finite element's control input.

Parameters

[in] idx Index of requested finite element

Returns: Index in decision variable vector corresponding to the beginning of the control input of the requested fe

◆ get_primal_idx()

int quadNLP::get_primal_idx ( int idx ) const

inline

Return the first index of the decision variable vector corresponding to the given finite element.

Parameters

[in] idx Index of requested finite element

Returns: Index in decision variable vector corresponding to the beginning of the requested fe

◆ get_primal_state_idx()

int quadNLP::get_primal_state_idx ( int idx ) const

inline

Return the first index of the decision variable vector corresponding to the given finite element's state.

Parameters

[in] idx Index of requested finite element

Returns: Index in decision variable vector corresponding to the beginning of the state of the requested fe

◆ get_relaxed_constraint_idx()

int quadNLP::get_relaxed_constraint_idx ( int idx ) const

inline

Return the first index of the slack constraint vector corresponding to the given finite element.

Parameters

[in] idx Index of requested finite element

Returns: Index in slack constraint vector corresponding to the beginning of the requested fe

◆ get_slack_constraint_var_idx()

int quadNLP::get_slack_constraint_var_idx ( int idx ) const

inline

Return the first index of the decision variable vector corresponding to the slack variable for the given finite element's state.

Parameters

[in] idx Index of requested finite element

Returns: Index in decision variable vector corresponding to the slack variable of the beginning of the state of the requested fe

◆ get_slack_state_idx()

int quadNLP::get_slack_state_idx ( int idx ) const

inline

Return the first index of the decision variable vector corresponding to the slack variable for the given finite element's state.

Parameters

[in] idx Index of requested finite element

Returns: Index in decision variable vector corresponding to the slack variable of the beginning of the state of the requested fe

◆ get_slack_var_constraint_idx()

int quadNLP::get_slack_var_constraint_idx ( int idx ) const

inline

Return the first index of the slack constraint vector corresponding to the given finite element.

Parameters

[in] idx Index of requested finite element

Returns: Index in slack constraint vector corresponding to the beginning of the requested fe

◆ get_starting_point()

bool quadNLP::get_starting_point	(	Index	n,
		bool	init_x,
		Number *	x,
		bool	init_z,
		Number *	z_L,
		Number *	z_U,
		Index	m,
		bool	init_lambda,
		Number *	lambda
	)

virtual

Method to return the starting point for the algorithm

Member Data Documentation

◆ interp_type_

const grid_map::InterpolationMethods quadNLP::interp_type_

Initial value:

=

grid_map::InterpolationMethods::INTER_LINEAR

◆ num_func_id_

const int quadNLP::num_func_id_ = 3

static

Declare the number of possible function ids (must match size of FunctionID enum)

◆ num_sys_id_

const int quadNLP::num_sys_id_ = 6

static

Declare the number of possible system ids (must match size of SystemID enum)

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

nmpc_controller/include/nmpc_controller/quad_nlp.h
nmpc_controller/src/quad_nlp.cpp
nmpc_controller/src/quad_nlp_utils.cpp

Public Member Functions

Public Attributes

Static Public Attributes

Private Member Functions

Constructor & Destructor Documentation

◆ quadNLP() [1/2]

◆ quadNLP() [2/2]

◆ ~quadNLP()

Member Function Documentation

◆ eval_f()

◆ eval_g()

◆ eval_g_single_complex_fe()

◆ eval_grad_f()

◆ eval_h()

◆ eval_jac_g()

◆ finalize_solution()

◆ get_bounds_info()

◆ get_bounds_info_single_complex_fe()

◆ get_nlp_info()

◆ get_primal_constraint_idx()

◆ get_primal_control_idx()

◆ get_primal_idx()

◆ get_primal_state_idx()

◆ get_relaxed_constraint_idx()

◆ get_slack_constraint_var_idx()

◆ get_slack_state_idx()

◆ get_slack_var_constraint_idx()

◆ get_starting_point()

Member Data Documentation

◆ interp_type_

◆ num_func_id_

◆ num_sys_id_