aligator  0.16.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
utils.hpp
Go to the documentation of this file.
1
2#pragma once
3
4#include "lqr-problem.hpp"
5#include <boost/core/span.hpp>
6#include <Eigen/SparseCore>
7
8namespace aligator::gar {
9namespace helpers {
12template <typename InType, typename OutScalar>
13void sparseAssignDenseBlock(Eigen::Index i0, Eigen::Index j0,
14 const Eigen::DenseBase<InType> &input,
15 Eigen::SparseMatrix<OutScalar> &out, bool update) {
16 assert(i0 + input.rows() <= out.rows() && "Inconsistent rows");
17 assert(j0 + input.cols() <= out.cols() && "Inconsistent cols");
18 using Eigen::Index;
19 for (Index i = 0; i < input.rows(); i++) {
20 for (Index j = 0; j < input.cols(); j++) {
21 if (update)
22 out.coeffRef(i0 + i, j0 + j) = input(i, j);
23 else
24 out.insert(i0 + i, j0 + j) = input(i, j);
25 }
26 }
27}
28
29template <typename Scalar>
30void sparseAssignDiagonal(Eigen::Index i0, Eigen::Index i1, Scalar value,
31 Eigen::SparseMatrix<Scalar> &out, bool update) {
32 using Eigen::Index;
33 assert(i0 <= i1 && "i0 should be lesser than i1. Can't assign empty range.");
34 assert(i1 <= out.rows() && "Inconsistent rows");
35 assert(i1 <= out.cols() && "Inconsistent cols");
36 for (Index kk = i0; kk < i1; kk++) {
37 if (update)
38 out.coeffRef(kk, kk) = value;
39 else
40 out.insert(kk, kk) = value;
41 }
42}
43} // namespace helpers
44
45template <typename Scalar>
47 const Scalar mueq, Eigen::SparseMatrix<Scalar> &mat,
48 Eigen::Matrix<Scalar, -1, 1> &rhs, bool update);
49
50template <typename Scalar>
51[[nodiscard]] std::array<Scalar, 3> lqrComputeKktError(
52 const LqrProblemTpl<Scalar> &problem,
53 boost::span<const typename math_types<Scalar>::VectorXs> xs,
54 boost::span<const typename math_types<Scalar>::VectorXs> us,
55 boost::span<const typename math_types<Scalar>::VectorXs> vs,
56 boost::span<const typename math_types<Scalar>::VectorXs> lbdas,
57 const Scalar mueq,
58 const std::optional<typename math_types<Scalar>::ConstVectorRef> &theta =
59 std::nullopt,
60 bool verbose = false);
61
63template <typename Scalar>
65 const auto &knots = problem.stages;
66 const uint nc0 = problem.nc0();
67 const size_t N = knots.size() - 1UL;
68 uint nrows = nc0;
69 for (size_t t = 0; t <= N; t++) {
70 const auto &model = knots[t];
71 nrows += model.nx + model.nu + model.nc;
72 if (t != N)
73 nrows += model.nx;
74 }
75 return nrows;
76}
77
79template <typename Scalar>
81 const LqrProblemTpl<Scalar> &problem,
82 const typename math_types<Scalar>::ConstVectorRef solution,
83 std::vector<typename math_types<Scalar>::VectorXs> &xs,
84 std::vector<typename math_types<Scalar>::VectorXs> &us,
85 std::vector<typename math_types<Scalar>::VectorXs> &vs,
86 std::vector<typename math_types<Scalar>::VectorXs> &lbdas) {
87 const uint N = (uint)problem.horizon();
88 xs.resize(N + 1);
89 us.resize(N + 1);
90 vs.resize(N + 1);
91 lbdas.resize(N + 1);
92 const uint nc0 = problem.nc0();
93
94 lbdas[0] = solution.head(nc0);
95
96 uint idx = nc0;
97 for (size_t t = 0; t <= N; t++) {
98 const LqrKnotTpl<Scalar> &knot = problem.stages[t];
99 const uint n = knot.nx + knot.nu + knot.nc;
100 auto seg = solution.segment(idx, n);
101 xs[t] = seg.head(knot.nx);
102 us[t] = seg.segment(knot.nx, knot.nu);
103 vs[t] = seg.segment(knot.nx + knot.nu, knot.nc);
104 idx += n;
105 if (t < N) {
106 lbdas[t + 1] = solution.segment(idx, knot.nx2);
107 idx += knot.nx2;
108 }
109 }
110}
111
112template <typename Scalar>
113[[nodiscard]] auto lqrInitializeSolution(const LqrProblemTpl<Scalar> &problem) {
114 using VectorXs = typename math_types<Scalar>::VectorXs;
115 std::vector<VectorXs> xs;
116 std::vector<VectorXs> us;
117 std::vector<VectorXs> vs;
118 std::vector<VectorXs> lbdas;
119 const uint N = (uint)problem.horizon();
120
121 xs.resize(N + 1);
122 us.resize(N + 1);
123 vs.resize(N + 1);
124 lbdas.resize(N + 1);
125
126 lbdas[0].setZero(problem.nc0());
127 for (uint i = 0; i <= N; i++) {
128 const LqrKnotTpl<Scalar> &kn = problem.stages[i];
129 xs[i].setZero(kn.nx);
130 us[i].setZero(kn.nu);
131 vs[i].setZero(kn.nc);
132 if (i == N)
133 break;
134 lbdas[i + 1].setZero(kn.nx2);
135 }
136 if (problem.stages.back().nu == 0) {
137 us.pop_back();
138 }
139 return std::make_tuple(std::move(xs), std::move(us), std::move(vs),
140 std::move(lbdas));
141}
142
143#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
144extern template void lqrCreateSparseMatrix<context::Scalar>(
145 const LqrProblemTpl<context::Scalar> &problem, const context::Scalar mueq,
146 Eigen::SparseMatrix<context::Scalar> &mat, context::VectorXs &rhs,
147 bool update);
148extern template std::array<context::Scalar, 3>
150 const LqrProblemTpl<context::Scalar> &,
151 boost::span<const context::VectorXs>, boost::span<const context::VectorXs>,
152 boost::span<const context::VectorXs>, boost::span<const context::VectorXs>,
153 const context::Scalar, const std::optional<context::ConstVectorRef> &,
154 bool);
155#endif
156
157} // namespace aligator::gar
void sparseAssignDiagonal(Eigen::Index i0, Eigen::Index i1, Scalar value, Eigen::SparseMatrix< Scalar > &out, bool update)
Definition utils.hpp:30
void sparseAssignDenseBlock(Eigen::Index i0, Eigen::Index j0, const Eigen::DenseBase< InType > &input, Eigen::SparseMatrix< OutScalar > &out, bool update)
Helper to assign a dense matrix into a range of coefficients of a sparse matrix.
Definition utils.hpp:13
void lqrCreateSparseMatrix(const LqrProblemTpl< Scalar > &problem, const Scalar mueq, Eigen::SparseMatrix< Scalar > &mat, Eigen::Matrix< Scalar, -1, 1 > &rhs, bool update)
auto lqrInitializeSolution(const LqrProblemTpl< Scalar > &problem)
Definition utils.hpp:113
void lqrDenseSolutionToTraj(const LqrProblemTpl< Scalar > &problem, const typename math_types< Scalar >::ConstVectorRef solution, std::vector< typename math_types< Scalar >::VectorXs > &xs, std::vector< typename math_types< Scalar >::VectorXs > &us, std::vector< typename math_types< Scalar >::VectorXs > &vs, std::vector< typename math_types< Scalar >::VectorXs > &lbdas)
Convert dense RHS solution to its trajectory [x,u,v,lambda] solution.
Definition utils.hpp:80
std::array< Scalar, 3 > lqrComputeKktError(const LqrProblemTpl< Scalar > &problem, boost::span< const typename math_types< Scalar >::VectorXs > xs, boost::span< const typename math_types< Scalar >::VectorXs > us, boost::span< const typename math_types< Scalar >::VectorXs > vs, boost::span< const typename math_types< Scalar >::VectorXs > lbdas, const Scalar mueq, const std::optional< typename math_types< Scalar >::ConstVectorRef > &theta=std::nullopt, bool verbose=false)
uint lqrNumRows(const LqrProblemTpl< Scalar > &problem)
Compute the number of rows in the problem matrix.
Definition utils.hpp:64
unsigned int uint
Definition logger.hpp:9
Struct describing a stage of a constrained LQ problem.
int horizon() const noexcept
uint nc0() const noexcept
Dimension of the initial condition constraint.
Typedefs for math (Eigen vectors, matrices) depending on scalar type.
Definition math.hpp:122