FMPC solver. More...

#include <FmpcSolver.h>

Collaboration diagram for nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >:

[legend]

Classes
struct	Coefficient
	Coefficients of linearized KKT condition. More...

struct	ComputationDuration
	Data of computation duration. More...

struct	Configuration
	Configuration. More...

struct	TraceData
	Data to trace optimization loop. More...

struct	Variable
	Optimization variables. More...

Public Types
enum	Status { Status::Uninitialized = 0, Status::Succeeded = 1, Status::ErrorInForward = 2, Status::ErrorInBackward = 3, Status::ErrorInUpdate = 4, Status::MaxIterationReached = 5, Status::IterationContinued = 6 }
	Result status. More...

using	StateDimVector = typename FmpcProblem< StateDim, InputDim, IneqDim >::StateDimVector
	Type of vector of state dimension. More...

using	InputDimVector = typename FmpcProblem< StateDim, InputDim, IneqDim >::InputDimVector
	Type of vector of input dimension. More...

using	IneqDimVector = typename FmpcProblem< StateDim, InputDim, IneqDim >::IneqDimVector
	Type of vector of inequality dimension. More...

using	StateStateDimMatrix = typename FmpcProblem< StateDim, InputDim, IneqDim >::StateStateDimMatrix
	Type of matrix of state x state dimension. More...

using	InputInputDimMatrix = typename FmpcProblem< StateDim, InputDim, IneqDim >::InputInputDimMatrix
	Type of matrix of input x input dimension. More...

using	StateInputDimMatrix = typename FmpcProblem< StateDim, InputDim, IneqDim >::StateInputDimMatrix
	Type of matrix of state x input dimension. More...

using	InputStateDimMatrix = typename FmpcProblem< StateDim, InputDim, IneqDim >::InputStateDimMatrix
	Type of matrix of input x state dimension. More...

using	IneqStateDimMatrix = typename FmpcProblem< StateDim, InputDim, IneqDim >::IneqStateDimMatrix
	Type of matrix of inequality x state dimension. More...

using	IneqInputDimMatrix = typename FmpcProblem< StateDim, InputDim, IneqDim >::IneqInputDimMatrix
	Type of matrix of inequality x input dimension. More...

using	IneqIneqDimMatrix = Eigen::Matrix< double, IneqDim, IneqDim >
	Type of matrix of inequality x inequality dimension. More...

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	FmpcSolver (const std::shared_ptr< FmpcProblem< StateDim, InputDim, IneqDim >> &problem)
	Constructor. More...

Configuration &	config ()
	Accessor to configuration. More...

const Configuration &	config () const
	Const accessor to configuration. More...

Status	solve (double current_t, const StateDimVector &current_x, const Variable &initial_variable)
	Solve optimization. More...

const Variable &	variable () const
	Const accessor to optimization variables. More...

const std::vector< Coefficient > &	coeffList () const
	Const accessor to sequence of coefficients of linearized KKT condition. More...

const std::vector< TraceData > &	traceDataList () const
	Const accessor to trace data list. More...

const ComputationDuration &	computationDuration () const
	Const accessor to computation duration. More...

void	dumpTraceDataList (const std::string &file_path) const
	Dump trace data list. More...

Protected Member Functions
void	checkVariable () const
	Check optimization variables. More...

Status	procOnce (int iter)
	Process one iteration. More...

double	calcKktError (double barrier_eps) const
	Calculate KKT condition error. More...

bool	backwardPass ()
	Process backward pass a.k.a backward Riccati recursion. More...

bool	forwardPass ()
	Process forward pass a.k.a forward Riccati recursion. More...

bool	updateVariables ()
	Update optimization variables given Newton-step direction. More...

void	setupMeritFunc ()
	Setup the merit function and its directional derivative. More...

double	calcMeritFunc (const Variable &variable) const
	Calculate merit function. More...

Protected Attributes
Configuration	config_
	Configuration. More...

std::shared_ptr< FmpcProblem< StateDim, InputDim, IneqDim > >	problem_
	FMPC problem. More...

Variable	variable_
	Optimization variables. More...

Variable	delta_variable_
	Update amount of optimization variables. More...

std::vector< Coefficient >	coeff_list_
	Sequence of coefficients of linearized KKT condition. More...

std::vector< TraceData >	trace_data_list_
	Sequence of trace data. More...

ComputationDuration	computation_duration_
	Computation duration data. More...

double	current_t_ = 0
	Current time [sec]. More...

StateDimVector	current_x_ = StateDimVector::Zero()
	Current state. More...

double	barrier_eps_ = 1e-4
	Barrier parameter. More...

double	merit_const_scale_ = 0.0
	Scale of constraint errors in the merit function. More...

double	merit_func_ = 0.0
	Merit function. More...

double	merit_deriv_ = 0.0
	Directional derivative of merit function. More...

Detailed Description

template<int StateDim, int InputDim, int IneqDim>
class nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >

FMPC solver.

Template Parameters

StateDim	state dimension
InputDim	input dimension
IneqDim	inequality dimension

See the following for a detailed algorithm.

S Katayama. Fast model predictive control of robotic systems with rigid contacts. Ph.D. thesis (section 2.2), Kyoto University, 2022.

Definition at line 23 of file FmpcSolver.h.

Member Typedef Documentation

◆ IneqDimVector

template<int StateDim, int InputDim, int IneqDim>

using nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::IneqDimVector = typename FmpcProblem<StateDim, InputDim, IneqDim>::IneqDimVector

Type of vector of inequality dimension.

Definition at line 33 of file FmpcSolver.h.

◆ IneqIneqDimMatrix

template<int StateDim, int InputDim, int IneqDim>

using nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::IneqIneqDimMatrix = Eigen::Matrix<double, IneqDim, IneqDim>

Type of matrix of inequality x inequality dimension.

Definition at line 54 of file FmpcSolver.h.

◆ IneqInputDimMatrix

template<int StateDim, int InputDim, int IneqDim>

using nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::IneqInputDimMatrix = typename FmpcProblem<StateDim, InputDim, IneqDim>::IneqInputDimMatrix

Type of matrix of inequality x input dimension.

Definition at line 51 of file FmpcSolver.h.

◆ IneqStateDimMatrix

template<int StateDim, int InputDim, int IneqDim>

using nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::IneqStateDimMatrix = typename FmpcProblem<StateDim, InputDim, IneqDim>::IneqStateDimMatrix

Type of matrix of inequality x state dimension.

Definition at line 48 of file FmpcSolver.h.

◆ InputDimVector

template<int StateDim, int InputDim, int IneqDim>

using nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::InputDimVector = typename FmpcProblem<StateDim, InputDim, IneqDim>::InputDimVector

Type of vector of input dimension.

Definition at line 30 of file FmpcSolver.h.

◆ InputInputDimMatrix

template<int StateDim, int InputDim, int IneqDim>

using nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::InputInputDimMatrix = typename FmpcProblem<StateDim, InputDim, IneqDim>::InputInputDimMatrix

Type of matrix of input x input dimension.

Definition at line 39 of file FmpcSolver.h.

◆ InputStateDimMatrix

template<int StateDim, int InputDim, int IneqDim>

using nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::InputStateDimMatrix = typename FmpcProblem<StateDim, InputDim, IneqDim>::InputStateDimMatrix

Type of matrix of input x state dimension.

Definition at line 45 of file FmpcSolver.h.

◆ StateDimVector

template<int StateDim, int InputDim, int IneqDim>

using nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::StateDimVector = typename FmpcProblem<StateDim, InputDim, IneqDim>::StateDimVector

Type of vector of state dimension.

Definition at line 27 of file FmpcSolver.h.

◆ StateInputDimMatrix

template<int StateDim, int InputDim, int IneqDim>

using nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::StateInputDimMatrix = typename FmpcProblem<StateDim, InputDim, IneqDim>::StateInputDimMatrix

Type of matrix of state x input dimension.

Definition at line 42 of file FmpcSolver.h.

◆ StateStateDimMatrix

template<int StateDim, int InputDim, int IneqDim>

using nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::StateStateDimMatrix = typename FmpcProblem<StateDim, InputDim, IneqDim>::StateStateDimMatrix

Type of matrix of state x state dimension.

Definition at line 36 of file FmpcSolver.h.

Member Enumeration Documentation

◆ Status

template<int StateDim, int InputDim, int IneqDim>

enum nmpc_fmpc::FmpcSolver::Status

strong

Result status.

Enumerator
Uninitialized	Uninitialized.
Succeeded	Succeeded.
ErrorInForward	Error in forward.
ErrorInBackward	Error in backward.
ErrorInUpdate	Error in update.
MaxIterationReached	Maximum iteration reached.
IterationContinued	Iteration continued (used internally only)

Definition at line 92 of file FmpcSolver.h.

Constructor & Destructor Documentation

◆ FmpcSolver()

template<int StateDim, int InputDim, int IneqDim>

EIGEN_MAKE_ALIGNED_OPERATOR_NEW nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::FmpcSolver ( const std::shared_ptr< FmpcProblem< StateDim, InputDim, IneqDim >> & problem )

inline

Constructor.

Parameters

problem FMPC problem

Definition at line 296 of file FmpcSolver.h.

Member Function Documentation

◆ backwardPass()

template<int StateDim, int InputDim, int IneqDim>

bool nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::backwardPass

protected

Process backward pass a.k.a backward Riccati recursion.

Returns: whether the process is finished successfully

Definition at line 524 of file FmpcSolver.hpp.

◆ calcKktError()

template<int StateDim, int InputDim, int IneqDim>

double nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::calcKktError ( double barrier_eps ) const

protected

Calculate KKT condition error.

Parameters

barrier_eps barrier parameter

Definition at line 496 of file FmpcSolver.hpp.

◆ calcMeritFunc()

template<int StateDim, int InputDim, int IneqDim>

double nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::calcMeritFunc ( const Variable & variable ) const

protected

Calculate merit function.

Parameters

variable variable

Definition at line 936 of file FmpcSolver.hpp.

◆ checkVariable()

template<int StateDim, int InputDim, int IneqDim>

void nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::checkVariable

protected

Check optimization variables.

Definition at line 285 of file FmpcSolver.hpp.

◆ coeffList()

template<int StateDim, int InputDim, int IneqDim>

const std::vector<Coefficient>& nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::coeffList ( ) const

inline

Const accessor to sequence of coefficients of linearized KKT condition.

Definition at line 325 of file FmpcSolver.h.

◆ computationDuration()

template<int StateDim, int InputDim, int IneqDim>

const ComputationDuration& nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::computationDuration ( ) const

inline

Const accessor to computation duration.

Definition at line 337 of file FmpcSolver.h.

◆ config() [1/2]

template<int StateDim, int InputDim, int IneqDim>

Configuration& nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::config ( )

inline

Accessor to configuration.

Definition at line 299 of file FmpcSolver.h.

◆ config() [2/2]

template<int StateDim, int InputDim, int IneqDim>

const Configuration& nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::config ( ) const

inline

Const accessor to configuration.

Definition at line 305 of file FmpcSolver.h.

◆ dumpTraceDataList()

template<int StateDim, int InputDim, int IneqDim>

void nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::dumpTraceDataList ( const std::string & file_path ) const

Dump trace data list.

Parameters

file_path path to output file

Definition at line 260 of file FmpcSolver.hpp.

◆ forwardPass()

template<int StateDim, int InputDim, int IneqDim>

bool nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::forwardPass

protected

Process forward pass a.k.a forward Riccati recursion.

Returns: whether the process is finished successfully

Definition at line 668 of file FmpcSolver.hpp.

◆ procOnce()

template<int StateDim, int InputDim, int IneqDim>

FmpcSolver< StateDim, InputDim, IneqDim >::Status nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::procOnce ( int iter )

protected

Process one iteration.

Parameters

iter	current iteration

Returns: result status

Definition at line 365 of file FmpcSolver.hpp.

◆ setupMeritFunc()

template<int StateDim, int InputDim, int IneqDim>

void nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::setupMeritFunc

protected

Setup the merit function and its directional derivative.

Definition at line 837 of file FmpcSolver.hpp.

◆ solve()

template<int StateDim, int InputDim, int IneqDim>

FmpcSolver< StateDim, InputDim, IneqDim >::Status nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::solve	(	double	current_t,
		const StateDimVector &	current_x,
		const Variable &	initial_variable
	)

Solve optimization.

Parameters

current_t	current time [sec]
current_x	current state
initial_variable	initial guess of optimization variables

Returns: result status

Definition at line 158 of file FmpcSolver.hpp.

◆ traceDataList()

template<int StateDim, int InputDim, int IneqDim>

const std::vector<TraceData>& nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::traceDataList ( ) const

inline

Const accessor to trace data list.

Definition at line 331 of file FmpcSolver.h.

◆ updateVariables()

template<int StateDim, int InputDim, int IneqDim>

bool nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::updateVariables

protected

Update optimization variables given Newton-step direction.

Returns: whether the process is finished successfully

Definition at line 711 of file FmpcSolver.hpp.

◆ variable()

template<int StateDim, int InputDim, int IneqDim>

const Variable& nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::variable ( ) const

inline

Const accessor to optimization variables.

Definition at line 319 of file FmpcSolver.h.

Member Data Documentation

◆ barrier_eps_

template<int StateDim, int InputDim, int IneqDim>

double nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::barrier_eps_ = 1e-4

protected

Barrier parameter.

Definition at line 414 of file FmpcSolver.h.

◆ coeff_list_

template<int StateDim, int InputDim, int IneqDim>

std::vector<Coefficient> nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::coeff_list_

protected

Sequence of coefficients of linearized KKT condition.

Definition at line 399 of file FmpcSolver.h.

◆ computation_duration_

template<int StateDim, int InputDim, int IneqDim>

ComputationDuration nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::computation_duration_

protected

Computation duration data.

Definition at line 405 of file FmpcSolver.h.

◆ config_

template<int StateDim, int InputDim, int IneqDim>

Configuration nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::config_

protected

Configuration.

Definition at line 387 of file FmpcSolver.h.

◆ current_t_

template<int StateDim, int InputDim, int IneqDim>

double nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::current_t_ = 0

protected

Current time [sec].

Definition at line 408 of file FmpcSolver.h.

◆ current_x_

template<int StateDim, int InputDim, int IneqDim>

StateDimVector nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::current_x_ = StateDimVector::Zero()

protected

Current state.

Definition at line 411 of file FmpcSolver.h.

◆ delta_variable_

template<int StateDim, int InputDim, int IneqDim>

Variable nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::delta_variable_

protected

Update amount of optimization variables.

Definition at line 396 of file FmpcSolver.h.

◆ merit_const_scale_

template<int StateDim, int InputDim, int IneqDim>

double nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::merit_const_scale_ = 0.0

protected

Scale of constraint errors in the merit function.

Definition at line 417 of file FmpcSolver.h.

◆ merit_deriv_

template<int StateDim, int InputDim, int IneqDim>

double nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::merit_deriv_ = 0.0

protected

Directional derivative of merit function.

Definition at line 423 of file FmpcSolver.h.

◆ merit_func_

template<int StateDim, int InputDim, int IneqDim>

double nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::merit_func_ = 0.0

protected

Merit function.

Definition at line 420 of file FmpcSolver.h.

◆ problem_

template<int StateDim, int InputDim, int IneqDim>

std::shared_ptr<FmpcProblem<StateDim, InputDim, IneqDim> > nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::problem_

protected

FMPC problem.

Definition at line 390 of file FmpcSolver.h.

◆ trace_data_list_

template<int StateDim, int InputDim, int IneqDim>

std::vector<TraceData> nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::trace_data_list_

protected

Sequence of trace data.

Definition at line 402 of file FmpcSolver.h.

◆ variable_

template<int StateDim, int InputDim, int IneqDim>

Variable nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >::variable_

protected

Optimization variables.

Definition at line 393 of file FmpcSolver.h.

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

/home/runner/work/NMPC/NMPC/catkin_ws/src/NMPC/nmpc_fmpc/include/nmpc_fmpc/FmpcSolver.h
/home/runner/work/NMPC/NMPC/catkin_ws/src/NMPC/nmpc_fmpc/include/nmpc_fmpc/FmpcSolver.hpp

Classes

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

template<int StateDim, int InputDim, int IneqDim> class nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >

Member Typedef Documentation

◆ IneqDimVector

◆ IneqIneqDimMatrix

◆ IneqInputDimMatrix

◆ IneqStateDimMatrix

◆ InputDimVector

◆ InputInputDimMatrix

◆ InputStateDimMatrix

◆ StateDimVector

◆ StateInputDimMatrix

◆ StateStateDimMatrix

Member Enumeration Documentation

◆ Status

Constructor & Destructor Documentation

◆ FmpcSolver()

Member Function Documentation

◆ backwardPass()

◆ calcKktError()

◆ calcMeritFunc()

◆ checkVariable()

◆ coeffList()

◆ computationDuration()

◆ config() [1/2]

◆ config() [2/2]

◆ dumpTraceDataList()

◆ forwardPass()

◆ procOnce()

◆ setupMeritFunc()

◆ solve()

◆ traceDataList()

◆ updateVariables()

◆ variable()

Member Data Documentation

◆ barrier_eps_

◆ coeff_list_

◆ computation_duration_

◆ config_

◆ current_t_

◆ current_x_

◆ delta_variable_

◆ merit_const_scale_

◆ merit_deriv_

◆ merit_func_

◆ problem_

◆ trace_data_list_

◆ variable_

template<int StateDim, int InputDim, int IneqDim>
class nmpc_fmpc::FmpcSolver< StateDim, InputDim, IneqDim >