aligator  0.15.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
workspace.hpp
Go to the documentation of this file.
1
4#pragma once
5
9
11
12namespace aligator {
13
14template <typename Scalar>
16 std::vector<xyz::polymorphic<ConstraintSetTpl<Scalar>>> components;
17 for (size_t i = 0; i < constraints.size(); i++) {
18 components.push_back(constraints.sets[i]);
19 }
20 return ConstraintSetProductTpl<Scalar>{components, constraints.dims()};
21}
22
27template <typename Scalar> struct WorkspaceTpl : WorkspaceBaseTpl<Scalar> {
31 using VecBool = Eigen::Matrix<bool, Eigen::Dynamic, 1>;
34 using BlkJacobianType = BlkMatrix<MatrixXs, -1, 2>; // jacobians
35
36 using Base::dyn_slacks;
37 using Base::nsteps;
39
41
42 gar::LqrProblemTpl<Scalar> lqr_problem; //< Linear-quadratic subproblem
43
46 std::vector<VectorXs> Lxs; //< State gradients
47 std::vector<VectorXs> Lus; //< Control gradients
48 std::vector<VectorXs> Lvs; //< Path multiplier gradients
50
53 using Base::trial_us;
54 using Base::trial_xs;
55 std::vector<VectorXs> trial_vs;
56 std::vector<VectorXs> trial_lams;
58
61 // used for linesearch
62 std::vector<VectorXs> lams_plus;
63 std::vector<VectorXs> vs_plus;
64 std::vector<VectorXs> vs_pdal;
66
68 std::vector<VectorXs> shifted_constraints;
71 std::vector<VectorXs> cstr_lx_corr;
72 std::vector<VectorXs> cstr_lu_corr;
75 std::vector<BlkJacobianType> cstr_proj_jacs;
77 std::vector<VecBool> active_constraints;
79 std::vector<ConstraintSetProduct> cstr_product_sets;
80
83 std::vector<VectorXs> dxs;
84 std::vector<VectorXs> dus;
85 std::vector<VectorXs> dvs;
86 std::vector<VectorXs> dlams;
88
91 std::vector<VectorXs> prev_xs;
92 std::vector<VectorXs> prev_us;
93 std::vector<VectorXs> prev_vs;
95
101 std::vector<VectorXs> stage_infeasibilities;
107 Scalar inner_criterion = 0.;
108
110 : Base() {}
111 explicit WorkspaceTpl(const allocator_type &alloc)
112 : Base()
113 , lqr_problem(alloc) {}
115 const allocator_type &alloc = {});
116
117 WorkspaceTpl(const WorkspaceTpl &) = delete;
119
122
124 shared_ptr<StageDataTpl<Scalar>> data);
125
126 allocator_type get_allocator() const { return lqr_problem.get_allocator(); }
127
128 friend std::ostream &operator<<(std::ostream &oss, const WorkspaceTpl &self) {
129 return oss << fmt::format("{}", self);
130 }
131};
132
133#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
134extern template struct WorkspaceTpl<context::Scalar>;
135#endif
136} // namespace aligator
137
138template <typename Scalar>
139struct fmt::formatter<aligator::WorkspaceTpl<Scalar>> {
140 constexpr auto parse(format_parse_context &ctx) const
141 -> decltype(ctx.begin()) {
142 return ctx.end();
143 }
144
145 auto format(const aligator::WorkspaceTpl<Scalar> &ws,
146 format_context &ctx) const -> decltype(ctx.out()) {
147 return fmt::format_to(ctx.out(),
148 "Workspace {{"
149 "\n nsteps: \t{:d}"
150 "\n n_multipliers:\t{:d}"
151 "\n}}",
152 ws.nsteps, ws.dlams.size());
153 }
154};
Block matrix class, with a fixed or dynamic-size number of row and column blocks.
A convenience subclass of std::pmr::polymorphic_allocator for bytes.
Definition allocator.hpp:16
Main package namespace.
auto getConstraintProductSet(const ConstraintStackTpl< Scalar > &constraints)
Definition workspace.hpp:15
Cartesian product of multiple constraint sets. This class makes computing multipliers and Jacobian ma...
Convenience class to manage a stack of constraints.
const std::vector< long > & dims() const
Get the set of dimensions for each constraint in the stack.
std::size_t size() const
std::vector< PolySet > sets
Data struct for stage models StageModelTpl.
A stage in the control problem.
Trajectory optimization problem.
std::size_t nsteps
Number of steps in the problem.
std::vector< VectorXs > trial_us
TrajOptDataTpl< Scalar > problem_data
Problem data.
std::vector< VectorXs > dyn_slacks
Dynamical infeasibility gaps.
std::vector< VectorXs > trial_xs
Workspace for solver SolverProxDDP.
Definition workspace.hpp:27
ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
WorkspaceTpl(const WorkspaceTpl &)=delete
VectorXs state_dual_infeas
Dual infeasibility in the states for each stage of the problem.
WorkspaceTpl(const allocator_type &alloc)
std::vector< VectorXs > dlams
Definition workspace.hpp:86
std::vector< VectorXs > dvs
Definition workspace.hpp:85
allocator_type get_allocator() const
ConstraintSetProductTpl< Scalar > ConstraintSetProduct
Definition workspace.hpp:33
std::vector< VectorXs > trial_lams
Definition workspace.hpp:56
std::vector< VectorXs > vs_plus
Definition workspace.hpp:63
std::vector< VecBool > active_constraints
Masks for active constraint sets.
Definition workspace.hpp:77
std::vector< VectorXs > dxs
Definition workspace.hpp:83
std::vector< VectorXs > lams_plus
Definition workspace.hpp:62
std::vector< VectorXs > Lus
Definition workspace.hpp:47
std::vector< VectorXs > shifted_constraints
Shifted constraints the projection operators should be applied to.
Definition workspace.hpp:68
WorkspaceTpl(const TrajOptProblemTpl< Scalar > &problem, const allocator_type &alloc={})
std::vector< VectorXs > Lxs
Definition workspace.hpp:46
std::vector< VectorXs > Lvs
Definition workspace.hpp:48
Eigen::Matrix< bool, Eigen::Dynamic, 1 > VecBool
Definition workspace.hpp:31
VectorXs stage_cstr_violations
Constraint violation measures for each stage and constraint.
Definition workspace.hpp:99
std::vector< BlkJacobianType > cstr_proj_jacs
Definition workspace.hpp:75
gar::LqrProblemTpl< Scalar > lqr_problem
Definition workspace.hpp:42
std::vector< VectorXs > vs_pdal
Definition workspace.hpp:64
std::vector< VectorXs > prev_vs
Definition workspace.hpp:93
std::vector< VectorXs > prev_us
Definition workspace.hpp:92
std::vector< VectorXs > dus
Definition workspace.hpp:84
VectorXs stage_inner_crits
Subproblem termination criterion for each stage.
Definition workspace.hpp:97
StageModelTpl< Scalar > StageModel
Definition workspace.hpp:29
std::vector< VectorXs > stage_infeasibilities
Stagewise infeasibilities.
WorkspaceTpl & operator=(const WorkspaceTpl &)=delete
WorkspaceTpl & operator=(WorkspaceTpl &&)=default
::aligator::polymorphic_allocator allocator_type
Definition workspace.hpp:40
WorkspaceBaseTpl< Scalar > Base
Definition workspace.hpp:30
friend std::ostream & operator<<(std::ostream &oss, const WorkspaceTpl &self)
std::vector< ConstraintSetProduct > cstr_product_sets
Cartesian products of the constraint sets of each stage.
Definition workspace.hpp:79
BlkMatrix< MatrixXs, -1, 2 > BlkJacobianType
Definition workspace.hpp:34
gar::LqrKnotTpl< Scalar > KnotType
Definition workspace.hpp:32
std::vector< VectorXs > cstr_lu_corr
Definition workspace.hpp:72
Scalar inner_criterion
Overall subproblem termination criterion.
std::vector< VectorXs > prev_xs
Definition workspace.hpp:91
std::vector< VectorXs > cstr_lx_corr
Definition workspace.hpp:71
WorkspaceTpl(WorkspaceTpl &&)=default
void cycleAppend(const TrajOptProblemTpl< Scalar > &problem, shared_ptr< StageDataTpl< Scalar > > data)
VectorXs control_dual_infeas
Dual infeasibility in the controls for each stage of the problem.
std::vector< VectorXs > trial_vs
Definition workspace.hpp:55
Struct describing a stage of a constrained LQ problem.