10#include <boost/core/make_span.hpp>
18template <
class T,
class A>
21 return boost::make_span(vec.data() + i0, i1 - i0);
25template <
class T,
class A>
27 size_t i0,
size_t i1) {
28 return boost::make_span(vec.data() + i0, i1 - i0);
63 ,
vt(other.
vt, alloc) {}
66 :
Vxx(std::move(other.
Vxx), alloc)
67 ,
vx(std::move(other.
vx), alloc)
68 ,
Vxt(std::move(other.
Vxt), alloc)
69 ,
Vtt(std::move(other.
Vtt), alloc)
70 ,
vt(std::move(other.
vt), alloc) {}
112 using CostToGo =
typename StageFactorType::CostToGo;
120 :
mat({nx, nc}, {nx, nc})
130 boost::span<StageFactorType> datas);
134 const std::optional<ConstVectorRef> &theta_);
142 boost::span<const StageFactorType> datas,
143 boost::span<VectorXs> xs, boost::span<VectorXs> us,
144 boost::span<VectorXs> vs, boost::span<VectorXs> lbdas,
145 const std::optional<ConstVectorRef> &theta_ = std::nullopt);
148#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
ArenaMatrix & setZero(Index newSize)
Block matrix class, with a fixed or dynamic-size number of row and column blocks.
const RowDimsType & rowDims() const
A convenience subclass of std::pmr::polymorphic_allocator for bytes.
boost::span< T > make_span_from_indices(std::vector< T, A > &vec, size_t i0, size_t i1)
Create a boost::span object from a vector and two indices.
Struct describing a stage of a constrained LQ problem.
kkt0_t(uint nx, uint nc, uint nth)
BlkMatrix< RowMatrixXs, 2, 1 > fth
BlkMatrix< VectorXs, 2, 1 > ff
BlkMatrix< MatrixXs, 2, 2 > mat
BunchKaufman< MatrixXs > chol
Kernel for use in Riccati-like algorithms for the proximal LQ subproblem.
static void terminalSolve(const KnotType &model, const Scalar mueq, StageFactorType &d)
static bool forwardImpl(boost::span< const KnotType > stages, boost::span< const StageFactorType > datas, boost::span< VectorXs > xs, boost::span< VectorXs > us, boost::span< VectorXs > vs, boost::span< VectorXs > lbdas, const std::optional< ConstVectorRef > &theta_=std::nullopt)
Forward sweep.
LqrKnotTpl< Scalar > KnotType
ALIGATOR_DYNAMIC_TYPEDEFS_WITH_ROW_TYPES(Scalar)
static bool backwardImpl(boost::span< const KnotType > stages, const Scalar mueq, boost::span< StageFactorType > datas)
static void stageKernelSolve(const KnotType &model, StageFactorType &d, CostToGo &vn, const Scalar mueq)
static void computeInitial(VectorRef x0, VectorRef lbd0, const kkt0_t &kkt0, const std::optional< ConstVectorRef > &theta_)
Solve initial stage.
StageFactor< Scalar > StageFactorType
typename StageFactorType::CostToGo CostToGo
CostToGo & operator=(CostToGo &&)=default
ArenaMatrix< VectorXs > vx
CostToGo(CostToGo &&other) noexcept=default
ArenaMatrix< MatrixXs > Vtt
ArenaMatrix< MatrixXs > Vxt
CostToGo(CostToGo &&other, const allocator_type &alloc)
ArenaMatrix< MatrixXs > Vxx
::aligator::polymorphic_allocator allocator_type
CostToGo & operator=(const CostToGo &)=default
CostToGo(const CostToGo &other, const allocator_type &alloc={})
ArenaMatrix< VectorXs > vt
allocator_type get_allocator() const
CostToGo(uint nx, uint nth, const allocator_type &alloc={})
Per-node struct for all computations in the factorization.
StageFactor(const StageFactor &other, const allocator_type &alloc={})
ArenaMatrix< MatrixXs > Gxhat
BlkMatrix< RowMatrixXs, 3, 1 > fb
ArenaMatrix< MatrixXs > Rhat
BlkMatrix< RowMatrixXs, 3, 1 > fth
BlkMatrix< VectorXs, 3, 1 > ff
ArenaMatrix< VectorXs > rhat
ArenaMatrix< MatrixXs > Qhat
ArenaMatrix< MatrixXs > Guhat
allocator_type get_allocator() const
ALIGATOR_DYNAMIC_TYPEDEFS_WITH_ROW_TYPES(Scalar)
StageFactor(StageFactor &&) noexcept=default
ArenaMatrix< RowMatrixXs > BtV
ArenaMatrix< VectorXs > qhat
BlkMatrix< MatrixXs, 2, 2 > kktMat
ArenaMatrix< RowMatrixXs > AtV
ArenaMatrix< MatrixXs > Shat
::aligator::polymorphic_allocator allocator_type
BunchKaufman< MatrixXs > kktChol
StageFactor(uint nx, uint nu, uint nc, uint nx2, uint nth, const allocator_type &alloc={})