aligator  0.16.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
results.hpp
Go to the documentation of this file.
1#pragma once
2
5
6namespace aligator {
7
8template <typename Scalar>
9struct ResultsFDDPTpl final : ResultsBaseTpl<Scalar> {
10
13 using BlockXs = Eigen::Block<MatrixXs, -1, -1>;
14
15 using Base::gains_;
16 using Base::us;
17 using Base::xs;
18
20 : Base() {}
21 explicit ResultsFDDPTpl(const TrajOptProblemTpl<Scalar> &problem);
22};
23
24template <typename Scalar>
26 const TrajOptProblemTpl<Scalar> &problem) {
27 if (!problem.checkIntegrity())
28 ALIGATOR_RUNTIME_ERROR("Problem failed integrity check.");
29 using StageModel = StageModelTpl<Scalar>;
30
31 const std::size_t nsteps = problem.numSteps();
32 xs.resize(nsteps + 1);
33 us.resize(nsteps);
34
35 problem.initializeSolution(xs, us);
36
37 gains_.resize(nsteps);
38
39 for (std::size_t i = 0; i < nsteps; i++) {
40 const StageModel &sm = *problem.stages_[i];
41
42 const int ndx = sm.ndx1();
43 const int nu = sm.nu();
44
45 gains_[i].setZero(nu, ndx + 1);
46 }
47 this->m_isInitialized = true;
48}
49
50} // namespace aligator
51
52#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
53#include "./results.txx"
54#endif
#define ALIGATOR_RUNTIME_ERROR(...)
Definition exceptions.hpp:7
Main package namespace.
std::vector< MatrixXs > gains_
std::vector< VectorXs > xs
States.
ResultsBaseTpl< Scalar > Base
Definition results.hpp:12
std::vector< VectorXs > us
Controls.
std::vector< MatrixXs > gains_
Riccati gains.
Eigen::Block< MatrixXs, -1, -1 > BlockXs
Definition results.hpp:13
A stage in the control problem.
Trajectory optimization problem.
void initializeSolution(std::vector< VectorXs > &xs, std::vector< VectorXs > &us) const
Execute the initialization strategy to generate an initial candidate solution to the problem.
std::vector< xyz::polymorphic< StageModel > > stages_
Stages of the control problem.
std::size_t numSteps() const