acados_solver_ros2 main
Acados-based NMPC controllers for ROS2 control
Loading...
Searching...
No Matches
acados_solver.hpp
Go to the documentation of this file.
1// Copyright 2023 ICUBE Laboratory, University of Strasbourg
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14//
15// Author: Thibault Poignonec (tpoignonec@unistra.fr)
16
17#ifndef ACADOS_SOLVER_BASE__ACADOS_SOLVER_HPP_
18#define ACADOS_SOLVER_BASE__ACADOS_SOLVER_HPP_
19
20#include <Eigen/Dense>
21
22#include <iostream>
23#include <vector>
24#include <exception>
25
28
29
30// Acados C interface
31#include "acados_c/ocp_nlp_interface.h"
32
33
34namespace acados
35{
36
42{
43public:
48 {
49public:
50 // Basic dimensions
51
53 unsigned int nx;
54
56 unsigned int nz;
57
59 unsigned int np;
60
62 unsigned int nu;
63
64 // Bounds on x and u
65
67 unsigned int nbx;
68
71 unsigned int nbx_0;
72
74 unsigned int nbx_N;
75
78 unsigned int nbu;
79
80 // Polytope constraints
81
83 unsigned int ng;
84
86 unsigned int ng_N;
87
88 // Non-linear constraints
89
91 unsigned int nh;
92
94 unsigned int nh_N;
95
97 unsigned int nphi;
98
100 unsigned int nphi_N;
101
102 // Slack-related dimensions
103
105 unsigned int ns;
106
108 unsigned int ns_N;
109
111 unsigned int nsbx;
112
114 unsigned int nsbx_N;
115
117 unsigned int nsbu;
118
120 unsigned int nsh;
121
123 unsigned int nsh_N;
124
126 unsigned int nsg;
127
129 unsigned int nsg_N;
130
131
133 unsigned int nsphi;
134
136 unsigned int nsphi_N;
137
138 // Cost ref.
140 unsigned int ny;
141
143 unsigned int ny_0;
144
146 unsigned int ny_N;
147
148 // Misc.
149
152 unsigned int nr;
153 };
154
155public:
156// Acados solver public API
157
167
174 virtual ~AcadosSolver();
175
176// Solver management
177
187 int init(unsigned int N, double Ts);
188
194 int reset();
195
196// Solve and introspection
197
210 int solve();
211
227 int solve_rti(RtiStage rti_phase);
228
229// Simulation
230
240 double dt,
241 ValueVector & x0,
242 ValueVector & u0,
243 ValueVector & p,
244 ValueVector & x_next,
245 ValueVector & z
246 );
247
254 double dt,
255 ValueMap const & x0_map,
256 ValueMap const & u0_map,
257 ValueMap const & p_map,
258 ValueMap & x_next_map,
259 ValueMap & z_map
260 );
261
268 ValueMap const & x0_map,
269 ValueMap const & u0_map,
270 ValueMap const & p_map,
271 ValueMap & x_next_map,
272 ValueMap & z_map
273 );
274
275// Setters
276
284
291 int set_initial_state_values(ValueMap const & x_0_map);
292
303 unsigned int stage,
304 IndexVector & idxbx,
305 ValueVector & lbx,
306 ValueVector & ubx);
307
319 IndexVector & idxbx,
320 ValueVector & lbx,
321 ValueVector & ubx);
322
333 unsigned int stage,
334 IndexVector & idxbu,
335 ValueVector & lbu,
336 ValueVector & ubu);
348 IndexVector & idxbu,
349 ValueVector & lbu,
350 ValueVector & ubu);
351
352// Runtime parameters
353
361 int set_runtime_parameters(unsigned int stage, ValueVector & p_i);
362
372 int set_runtime_parameters(unsigned int stage, ValueMap const & p_i_map);
373
374
384
394 int set_runtime_parameters(ValueMap const & p_i_map);
395
396// Initialization state
397
409 int initialize_state_values(unsigned int stage, ValueVector & x_i);
410
420 int initialize_state_values(unsigned int stage, ValueMap const & x_i_map);
421
429
436 int initialize_state_values(ValueMap const & x_i_map);
437
438// Initialization controls
439
447 int initialize_control_values(unsigned int stage, ValueVector & u_i);
448
456 int initialize_control_values(unsigned int stage, ValueMap const & u_i_map);
457
465
473
474// Getters
481 ValueVector get_state_values(unsigned int stage);
482
490
498
499
507
514 ValueVector get_control_values(unsigned int stage);
515
523
531
539
540// Getters variable mappings
541
547 const IndexMap & x_index_map() const;
548
554 const IndexMap & z_index_map() const;
555
561 const IndexMap & p_index_map() const;
562
568 const IndexMap & u_index_map() const;
569
570// Values map utils
571
581 IndexMap const & index_map,
582 unsigned int expected_total_indexes);
583
593 ValueMap const & values_map,
594 unsigned int expected_total_indexes);
595
606 static bool is_values_map_complete(IndexMap const & index_map, ValueMap const & values_map);
607
624 IndexMap const & index_map,
625 ValueMap const & values_map,
626 unsigned int vector_size,
627 ValueVector & values);
628
638 IndexMap const & index_map,
639 ValueMap const & values_map,
640 unsigned int vector_size);
641
651 IndexMap const & index_map,
652 ValueVector const & values);
653
663 IndexMap const & index_map,
664 ValueVector const & values,
665 ValueMap & value_map);
666
667// Problem dimensions and convenience getters for commonly used attributes
668
674 const Dimensions & dims() const;
675
683 unsigned int nx() const;
684
692 unsigned int nz() const;
693
701 unsigned int np() const;
702
710 unsigned int nu() const;
711
717 unsigned int N() const;
718
726 double Ts() const;
727
728
734 std::vector<double> sampling_intervals() const;
735
736protected:
739
742
745
748
751
752// Solver-specific functions
753
755 virtual int create_index_maps() = 0;
756
757protected:
768
769// Imported Acados solver C-code interface ("internal" --> internal use only!)
770
779 virtual int internal_create_capsule() = 0;
780
791 virtual int internal_create_with_discretization(int n_time_steps, double * new_time_steps) = 0;
792
801 virtual int internal_reset(int reset_qp_solver_mem = 1) = 0;
802
811 virtual int internal_free() = 0;
812
820 virtual int internal_free_capsule() = 0;
821
830 virtual int internal_update_qp_solver_cond_N(int qp_solver_cond_N) = 0;
831
842 virtual int internal_update_params(unsigned int stage, double * value, int np) = 0;
843
856 unsigned int stage,
857 int * idx,
858 double * p,
859 int n_update) = 0;
860
868 virtual int internal_solve() = 0;
869
878 virtual int internal_simulate(
879 double dt /* Time step */,
880 double * x0 /* Differential state initial value */,
881 double * u0 /* Applied controls */,
882 double * p /* Runtime parameters */,
883 double * x_next /* Differential state next value */,
884 double * z_next /* Algebraic state next value */) = 0;
885
891 virtual void internal_print_stats() const = 0;
892
893public:
901 virtual ocp_nlp_in * get_nlp_in() const = 0;
902
910 virtual ocp_nlp_out * get_nlp_out() const = 0;
911
920 virtual ocp_nlp_out * get_sens_out() const = 0;
921
929 virtual ocp_nlp_solver * get_nlp_solver() const = 0;
930
938 virtual ocp_nlp_config * get_nlp_config() const = 0;
939
947 virtual void * get_nlp_opts() const = 0;
948
956 virtual ocp_nlp_dims * get_nlp_dims() const = 0;
957
965 virtual ocp_nlp_plan_t * get_nlp_plan() const = 0;
966
974 virtual unsigned int get_nlp_np() const = 0;
975
976// Imported Acados SIM solver C-code interface (for internal use only!)
977
978protected:
979 // TODO(tpoignonec) : add sim solver private c-interface
980
981public:
982 // TODO(tpoignonec) : add sim solver public c-interface
983
984 // Private own attributes
985
986private:
988 bool _is_initialized = false;
989
991 double _Ts = -1;
992// Runtime data
993
995 int _rti_phase = 0;
996};
997
998} // namespace acados
999
1000#endif // ACADOS_SOLVER_BASE__ACADOS_SOLVER_HPP_
Container for the (fixed) dimensions of the imported Acados OCP.
Definition acados_solver.hpp:48
unsigned int nbx
Number of state stage bounds.
Definition acados_solver.hpp:67
unsigned int ns
Total number of slack variable.
Definition acados_solver.hpp:105
unsigned int nsphi
Number of slack variables used by convex-over-nonlinear constraints.
Definition acados_solver.hpp:133
unsigned int nu
Control vector size.
Definition acados_solver.hpp:62
unsigned int nphi
Number of "convex-over-nonlinear" constraints (see Acados documentation)
Definition acados_solver.hpp:97
unsigned int nsg
Number of slack variables used by polytopic constraints.
Definition acados_solver.hpp:126
unsigned int nbx_0
Number of initial state stage bounds.
Definition acados_solver.hpp:71
unsigned int ng
Number of polytopic constraints.
Definition acados_solver.hpp:83
unsigned int nphi_N
Number of "convex-over-nonlinear" constraints at stage N.
Definition acados_solver.hpp:100
unsigned int nsbx
Number of slack variables used by state bounds.
Definition acados_solver.hpp:111
unsigned int np
Runtime parameters vector size.
Definition acados_solver.hpp:59
unsigned int ny_0
Size of the reference vector at initial stage.
Definition acados_solver.hpp:143
unsigned int ny
Size of the reference vector.
Definition acados_solver.hpp:140
unsigned int nbx_N
Number of terminal state bounds.
Definition acados_solver.hpp:74
unsigned int nsbu
Number of slack variables used by control bounds.
Definition acados_solver.hpp:117
unsigned int nh_N
Number of non-linear constraints at stage N.
Definition acados_solver.hpp:94
unsigned int nsbx_N
Number of slack variables used by terminal state bounds.
Definition acados_solver.hpp:114
unsigned int ny_N
Size of the reference vector at terminal stage.
Definition acados_solver.hpp:146
unsigned int nh
Number of non-linear constraints.
Definition acados_solver.hpp:91
unsigned int nsg_N
Number of slack variables used by (terminal) polytopic constraints.
Definition acados_solver.hpp:129
unsigned int ng_N
Number of polytopic constraints at stage N.
Definition acados_solver.hpp:86
unsigned int nx
Differential state vector size.
Definition acados_solver.hpp:53
unsigned int nr
Dimensions of the image of the inner nonlinear function in positive definite constr....
Definition acados_solver.hpp:152
unsigned int ns_N
Total number of slack variable at stage N.
Definition acados_solver.hpp:108
unsigned int nsphi_N
Number of slack variables used by (terminal) convex-over-nonlinear constraints.
Definition acados_solver.hpp:136
unsigned int nz
Algebraic state vector size.
Definition acados_solver.hpp:56
unsigned int nsh_N
Number of slack variables used by (terminal) non-linear constraints.
Definition acados_solver.hpp:123
unsigned int nsh
Number of slack variables used by non-linear constraints.
Definition acados_solver.hpp:120
unsigned int nbu
Number of control bounds.
Definition acados_solver.hpp:78
Abstract C++ wrapper of generated Acados solver C-code.
Definition acados_solver.hpp:42
int initialize_control_values(ValueMap const &u_i_map)
Initialize the control variable values for ALL stages at once from a key/values map.
ValueVector get_algebraic_state_values(unsigned int stage)
Retrieve the algebraic state variables at a given stage.
IndexMap _z_index_map
Unordered map used to set or retrieve the values of algebraic state variables by name.
Definition acados_solver.hpp:744
int set_runtime_parameters(ValueVector &p_i)
Set the runtime parameters for all stages at once from ordered value vector.
int solve_rti(RtiStage rti_phase)
Solve the non-linear (RTI) optimization problem given the provided initial state, constraints,...
int set_initial_state_values(ValueMap const &x_0_map)
Set the initial state values (i.e., add constraints on initial state) from a map of key/ValueVector p...
int set_control_bounds(IndexVector &idxbu, ValueVector &lbu, ValueVector &ubu)
Set control bounds for all stages.
int simulate(ValueMap const &x0_map, ValueMap const &u0_map, ValueMap const &p_map, ValueMap &x_next_map, ValueMap &z_map)
Simulate the next state given the current state, control inputs and runtime parameters.
ValueMap get_algebraic_state_values_as_map(unsigned int stage)
Retrieve the algebraic state variables at a given stage and package them as a ValueMap.
ValueVector get_control_values(unsigned int stage)
Retrieve the control variables at a given stage.
int solve()
Solve the non-linear optimization problem given the provided initial state, constraints,...
int initialize_state_values(ValueMap const &x_i_map)
Initialize the (differential) state values for ALL stages at once from a key/values map.
int initialize_state_values(ValueVector &x_i)
Initialize the (differential) state values for ALL stages at once from an ordered value vector.
const IndexMap & x_index_map() const
Returns a constant reference to the Key/IndexVector map of differential state variables internally st...
const IndexMap & u_index_map() const
Returns a constant reference to the Key/IndexVector map of control variables internally stored in Aca...
const IndexMap & z_index_map() const
Returns a constant reference to the Key/IndexVector map of algebraic state variables internally store...
virtual void internal_print_stats() const =0
Print the solver stats (e.g., number of SQP iters, number of QP iters, etc.) to the console.
virtual int internal_update_params_sparse(unsigned int stage, int *idx, double *p, int n_update)=0
A sparse implementation of internal_update_params_sparse().
virtual ocp_nlp_plan_t * get_nlp_plan() const =0
Legacy C-function to retrieve the C-interface ocp_nlp_plan property of the solver.
static bool is_values_map_complete(IndexMap const &index_map, ValueMap const &values_map)
Check if for each key-index pair contained in the index map there is a value provided by the value ma...
int set_state_bounds(unsigned int stage, IndexVector &idxbx, ValueVector &lbx, ValueVector &ubx)
Set (differential) state bounds at a given stage.
const IndexMap & p_index_map() const
Returns a constant reference to the Key/IndexVector map of runtime parameters internally stored in Ac...
static ValueVector value_vector_from_map(IndexMap const &index_map, ValueMap const &values_map, unsigned int vector_size)
Returns a vector containing the values provided through the value map.
int free_memory()
Free all the allocated memory.
virtual ocp_nlp_solver * get_nlp_solver() const =0
Legacy C-function to retrieve the C-interface ocp_nlp_solver property of the solver.
IndexMap _p_index_map
Unordered map used to set the values of runtime parameters by name.
Definition acados_solver.hpp:747
const Dimensions & dims() const
Returns the (fixed) dimensions of the OCP problem stored in AcadosSolver::_dims.
int initialize_control_values(unsigned int stage, ValueVector &u_i)
Initialize the control variable values for one stage from an ordered value vector.
int reset()
Resets the Acados solver, including the internal QP solver and the RTI phase cache.
unsigned int np() const
Returns the number of runtime parameters.
int simulate(double dt, ValueVector &x0, ValueVector &u0, ValueVector &p, ValueVector &x_next, ValueVector &z)
Simulate the next state given the current state, control inputs and runtime parameters.
int set_state_bounds(IndexVector &idxbx, ValueVector &lbx, ValueVector &ubx)
Set (differential) state bounds for all stages.
virtual ocp_nlp_in * get_nlp_in() const =0
Legacy C-function to retrieve the C-interface ocp_nlp_in property of the solver.
virtual int internal_update_params(unsigned int stage, double *value, int np)=0
Internal update of the solver runtime parameters.
ValueMap get_control_values_as_map(unsigned int stage)
Retrieve the control variables at a given stage and package them as a ValueMap.
int initialize_control_values(unsigned int stage, ValueMap const &u_i_map)
Initialize the control variable values for one stage from a key/values map.
double Ts() const
Returns the (constant) sampling interval Ts (in seconds).
virtual int internal_create_with_discretization(int n_time_steps, double *new_time_steps)=0
Initialize Acados solver and (optionally) redefine the shooting nodes.
virtual int internal_free_capsule()=0
Free the memory allocated for the capsule.
int simulate(double dt, ValueMap const &x0_map, ValueMap const &u0_map, ValueMap const &p_map, ValueMap &x_next_map, ValueMap &z_map)
Simulate the next state given the current state, control inputs and runtime parameters.
virtual ocp_nlp_dims * get_nlp_dims() const =0
Legacy C-function to retrieve the C-interface ocp_nlp_dims property of the solver that contains the d...
static bool is_map_size_consistent(IndexMap const &index_map, unsigned int expected_total_indexes)
Check if the total number of indexes contained in the provided index map is equal to an expected numb...
int set_runtime_parameters(ValueMap const &p_i_map)
Set the runtime parameters for all stages at once from a key/values map.
virtual int internal_simulate(double dt, double *x0, double *u0, double *p, double *x_next, double *z_next)=0
Simulate the next state given the current state, control inputs and runtime parameters.
static void fill_vector_from_map(IndexMap const &index_map, ValueMap const &values_map, unsigned int vector_size, ValueVector &values)
Fill (and resize if necessary) a vector with the values contained in the value map.
int initialize_state_values(unsigned int stage, ValueVector &x_i)
Initialize the (differential) state values for a stage from an ordered value vector.
virtual ocp_nlp_out * get_sens_out() const =0
Legacy C-function to retrieve the C-interface ocp_nlp_out property of the solver.
virtual int internal_reset(int reset_qp_solver_mem=1)=0
Reset the solver memory and (opt.) the internal QP solver.
int initialize_state_values(unsigned int stage, ValueMap const &x_i_map)
Initialize the (differential) state values for a stage from a key/values map.
virtual int internal_free()=0
Free the memory allocated for the solver.
virtual ocp_nlp_out * get_nlp_out() const =0
Legacy C-function to retrieve the C-interface ocp_nlp_out property of the solver.
unsigned int nz() const
Returns the dimensions of the algebraic state vector.
IndexMap _x_index_map
Unordered map used to set or retrieve the values of diff. state variables by name.
Definition acados_solver.hpp:741
int init(unsigned int N, double Ts)
Initialize the solver and set the (constant) sampling intervals.
virtual int internal_update_qp_solver_cond_N(int qp_solver_cond_N)=0
Update the QP solver qp_solver_cond_N setting.
static ValueMap create_map_from_values(IndexMap const &index_map, ValueVector const &values)
Returns a vector ValueMap object containing the values of the value vector.
Dimensions _dims
Fixed dimensions of the imported Acados OCP.
Definition acados_solver.hpp:738
virtual int create_index_maps()=0
Reinitialize the maps used to set or retrieve value using string variable names.
static void fill_map_from_values(IndexMap const &index_map, ValueVector const &values, ValueMap &value_map)
Fill a ValueMap object with the values of the value vector.
ValueVector get_parameter_values(unsigned int stage)
Retrieve the parameters at a given stage.
int set_control_bounds(unsigned int stage, IndexVector &idxbu, ValueVector &lbu, ValueVector &ubu)
Set control bounds at a given stage.
int set_initial_state_values(ValueVector &x_0)
Set the initial state values (i.e., add constraints on initial state)
IndexMap _u_index_map
Unordered map used to set or retrieve the values of control variables by name.
Definition acados_solver.hpp:750
unsigned int N() const
Returns the number of shooting nodes.
virtual int internal_solve()=0
Solve the non-linear optimization`.
int set_runtime_parameters(unsigned int stage, ValueVector &p_i)
Set the runtime parameters for one stage from ordered value vector.
ValueMap get_parameter_values_as_map(unsigned int stage)
Retrieve the parameters at a given stage and package them as a ValueMap.
AcadosSolver()
Constructor of the AcadosSolver object.
ValueVector get_state_values(unsigned int stage)
Retrieve the differential state variables at a given stage.
virtual ~AcadosSolver()
Destructor of the AcadosSolver object.
ValueMap get_state_values_as_map(unsigned int stage)
Retrieve the differential state variables at a given stage and package them as a ValueMap.
virtual int internal_create_capsule()=0
Create the Acados solver's capsule used by the Acados C-interface.
virtual ocp_nlp_config * get_nlp_config() const =0
Legacy C-function to retrieve the C-interface ocp_nlp_config property of the solver.
virtual unsigned int get_nlp_np() const =0
Legacy C-function to retrieve the number of runtime parameters from the Acados capsule.
static bool is_map_size_consistent(ValueMap const &values_map, unsigned int expected_total_indexes)
Check if the total number of values contained in the provided value map is equal to an expected numbe...
unsigned int nx() const
Returns the dimensions of the differential state vector.
virtual void * get_nlp_opts() const =0
Legacy C-function to retrieve the C-interface get_nlp_opts property of the solver.
int set_runtime_parameters(unsigned int stage, ValueMap const &p_i_map)
Set the runtime parameters for one stage from a key/values map.
int initialize_control_values(ValueVector &u_i)
Initialize the control variable values for ALL stages at once from an ordered value vector.
unsigned int nu() const
Returns the number of control variables.
std::vector< double > sampling_intervals() const
Returns the vector of sampling interval (in seconds).
Definition acados_solver.hpp:35
std::unordered_map< std::string, IndexVector > IndexMap
Mapping between keys (std::string) and indexes (acados::IndexVector).
Definition acados_types.hpp:36
std::vector< unsigned int > IndexVector
List of indexes.
Definition acados_types.hpp:30
RtiStage
Definition acados_types.hpp:48
std::vector< double > ValueVector
List of values (double).
Definition acados_types.hpp:33
std::unordered_map< std::string, ValueVector > ValueMap
Mapping between keys (std::string) and data (acados::ValueVector).
Definition acados_types.hpp:39