18#include <fmt/ostream.h>
19#include <unordered_map>
22#define ALIGATOR_FDDP_WARNING(msg) ALIGATOR_WARNING("SolverFDDP", msg)
33 using Problem = TrajOptProblemTpl<Scalar>;
43 using ExpModel = ExplicitDynamicsModelTpl<Scalar>;
46 using CallbackMap = std::unordered_map<std::string, CallbackPtr>;
95 VerboseLevel verbose = VerboseLevel::QUIET,
120 Workspace &workspace,
const Scalar alpha);
144 inline Scalar computeInfeasibility(
const Problem &problem);
154 assert(workspace.kktRhs.size() == results.gains_.size());
156 results.gains_ = workspace.kktRhs;
159 inline void increaseRegularization() {
164 inline void decreaseRegularization() {
170 inline Scalar computeCriterion(
Workspace &workspace);
173 void registerCallback(
const std::string &name,
CallbackPtr cb) {
178 void removeCallback(
const std::string &name) {
callbacks_.erase(name); }
179 auto getCallback(
const std::string &name) ->
CallbackPtr {
190 void invokeCallbacks(
Workspace &workspace, Results &results) {
192 cb.second->call(workspace, results);
196 bool run(
const Problem &problem,
const std::vector<VectorXs> &xs_init = {},
197 const std::vector<VectorXs> &us_init = {});
199 static const ExplicitDynamicsData &
200 stage_get_dynamics_data(
const StageDataTpl<Scalar> &data) {
201 const DynamicsDataTpl<Scalar> &dd = *
data.dynamics_data;
202 return static_cast<const ExplicitDynamicsData &
>(dd);
208#include "./solver-fddp.hxx"
210#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
211#include "./solver-fddp.txx"
#define ALIGATOR_NOMALLOC_SCOPED
void set_default_options(std::size_t, bool=true)
constexpr auto data(C &c) noexcept(noexcept(c.data())) -> decltype(c.data())
A table logging utility to log the trace of the numerical solvers.
The feasible DDP (FDDP) algorithm, from Mastalli et al. (2020).
ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
ExplicitDynamicsModelTpl< Scalar > ExpModel
void updateExpectedImprovement(Workspace &workspace, Results &results) const
Pre-compute parts of the directional derivatives – this is done before linesearch.
WorkspaceFDDPTpl< Scalar > Workspace
ALIGATOR_DEPRECATED const Results & getResults() const
Get the solver results.
void setup(const Problem &problem)
Allocate workspace and results structs.
TrajOptProblemTpl< Scalar > Problem
Scalar reg_inc_factor_
Regularization increase factor.
static Scalar forwardPass(const Problem &problem, const Results &results, Workspace &workspace, const Scalar alpha)
Perform a nonlinear rollout, keeping an infeasibility gap.
void expectedImprovement(Workspace &workspace, Scalar &d1, Scalar &d2) const
Finish computing the directional derivatives – this is done within linesearch.
Linesearch< Scalar >::Options ls_params
bool force_initial_condition_
Scalar reg_dec_factor_
Regularization decrease factor.
ALIGATOR_DEPRECATED const Workspace & getWorkspace() const
Get a const reference to the solver's workspace.
void setNumThreads(const std::size_t num_threads)
std::unordered_map< std::string, CallbackPtr > CallbackMap
ResultsFDDPTpl< Scalar > Results
std::size_t max_iters
Maximum number of iterations for the solver.
shared_ptr< CallbackBaseTpl< Scalar > > CallbackPtr
std::size_t getNumThreads() const
ManifoldAbstractTpl< Scalar > Manifold
QFunctionTpl< Scalar > QParams
ValueFunctionTpl< Scalar > VParams
std::size_t num_threads_
Number of threads to use when evaluating the problem or its derivatives.
CallbackMap callbacks_
Callbacks.
SolverFDDPTpl(const Scalar tol=1e-6, VerboseLevel verbose=VerboseLevel::QUIET, const Scalar reg_init=1e-9, const std::size_t max_iters=200)