5#include <boost/core/span.hpp>
6#include <Eigen/SparseCore>
12template <
typename InType,
typename OutScalar>
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");
19 for (Index i = 0; i < input.rows(); i++) {
20 for (Index j = 0; j < input.cols(); j++) {
22 out.coeffRef(i0 + i, j0 + j) = input(i, j);
24 out.insert(i0 + i, j0 + j) = input(i, j);
29template <
typename Scalar>
31 Eigen::SparseMatrix<Scalar> &out,
bool update) {
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++) {
38 out.coeffRef(kk, kk) = value;
40 out.insert(kk, kk) = value;
45template <
typename Scalar>
47 const Scalar mueq, Eigen::SparseMatrix<Scalar> &mat,
48 Eigen::Matrix<Scalar, -1, 1> &rhs,
bool update);
50template <
typename Scalar>
60 bool verbose =
false);
63template <
typename Scalar>
65 const auto &knots = problem.
stages;
67 const size_t N = knots.size() - 1UL;
69 for (
size_t t = 0; t <= N; t++) {
70 const auto &model = knots[t];
71 nrows += model.nx + model.nu + model.nc;
79template <
typename Scalar>
94 lbdas[0] = solution.head(nc0);
97 for (
size_t t = 0; t <= N; t++) {
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);
106 lbdas[t + 1] = solution.segment(idx, knot.
nx2);
112template <
typename Scalar>
115 std::vector<VectorXs> xs;
116 std::vector<VectorXs> us;
117 std::vector<VectorXs> vs;
118 std::vector<VectorXs> lbdas;
126 lbdas[0].setZero(problem.
nc0());
127 for (
uint i = 0; i <= N; i++) {
129 xs[i].setZero(kn.
nx);
130 us[i].setZero(kn.
nu);
131 vs[i].setZero(kn.
nc);
134 lbdas[i + 1].setZero(kn.
nx2);
136 if (problem.
stages.back().nu == 0) {
139 return std::make_tuple(std::move(xs), std::move(us), std::move(vs),
143#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
145 const LqrProblemTpl<context::Scalar> &problem,
const context::Scalar mueq,
146 Eigen::SparseMatrix<context::Scalar> &mat, context::VectorXs &rhs,
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>,
void sparseAssignDiagonal(Eigen::Index i0, Eigen::Index i1, Scalar value, Eigen::SparseMatrix< Scalar > &out, bool update)
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.
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)
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.
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.
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.