Merge pull request #348 from Simple-Robotics/topic/simplify-solver

Revamp ProxDDP solver : remove proximal iteration over co-state, temporarily remove implicit dynamics

Author: ManifoldFR

.github/workflows/macos-linux-pixi.yml

@@ -39,7 +39,7 @@ jobs:
       fail-fast: false
       matrix:
         os: [ubuntu-latest, macos-latest, macos-13]
-        environment: [all, all-py39]
+        environment: [all, all-py310]
         build_type: [Release, Debug]
 
     steps:

CHANGELOG.md

@@ -7,24 +7,56 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## [Unreleased]
 
+This release brings a major change to the ProxDDP solver, which no longer uses proximal iteration the co-state (the dynamics' Lagrange multiplies). This choice is made to increase the solver's overall performance.
+
+The ProxDDP solver now defaults to using a linear rollout for the state-control trajectory update.
+
+Furthermore, on a temporary basis, implicit discrete dynamics **are no longer supported** in the API and solvers.
+
 ### Fixed
 
 - gar : fix missing move assignment operator in `LqrProblemTpl`
 - Fix C++20 support. Fix `consteval` compilation errors related to fmt
+- Fix `aligator::gar::ParallelRiccatiSolver` missing from docs
+- Fix missing set of Pinocchio-support (cost, dynamics, etc) classes
 
 ### Changed
 
+- gar : allow setting number of refinement iterations for condensed KKT solver
+- ProxDDP solver : change default rollout type to `RolloutType::LINEAR`/`ROLLOUT_LINEAR`
+- solvers/proxddp : make `Workspace` an allocator-aware class
+- core/manifold-base : mark some functions `[[nodiscard]]`
 - readme/cmake : update actually expected minimum version of eigenpy to 3.9
 - solvers : make proxddp algo's Results class copyable again (in C++ and Python) (https://github.com/Simple-Robotics/aligator/pull/322)
 - python/visitors : also set `__copy__` method on exposed class with `CopyableVisitor` (https://github.com/Simple-Robotics/aligator/pull/322)
 - python : make `Results` copyable (using `CopyableVisitor`) (https://github.com/Simple-Robotics/aligator/pull/322)
+- ProxDDP solver : remove proximal iteration/ALM over co-states
+- ProxDDP solver : throw when given invalid linesearch enum value
+- core/linesearches : move `LinesearchOptions` struct out of the `Linesearch` template class, add CTAD
+- Change all tests to use Catch2 instead of Boost.Test
+- The Riccati algorithms now run faster after the dual-regularisation on co-states and the QR for the implicit dynamics have been removed. The algos might be less numerically accurate.
+
+#### Changes to dynamics
+
+- Remove explicit dynamics (incl. explicit integrators) from `DynamicsModel` class hierarchy
+- Make `ExplicitDynamicsModel` used everywhere in API (e.g. `StageModel` now takes/stores `polymorphic<ExplicitDynamicsModel>`)
+- Directly store state space repr dim and actual dim (`nx` and `ndx`) in `ManifoldAbstract` class
 
 ### Added
 
 - gar: add CTAD for the `ParallelRiccatiSolver` and `ProximalRiccatiSolver` classes
 - testing: added a test_mpc.py script to test parallel and serial mpc implementations (https://github.com/Simple-Robotics/aligator/pull/331)
 - modelling : added wheeled inverted pendulum dynamics (https://github.com/Simple-Robotics/aligator/pull/326)
 - CMake option to `BUILD_STANDALONE_PYTHON_INTERFACE` (https://github.com/Simple-Robotics/aligator/pull/347)
+- utils: add free function `forwardDynamics()` (replaces previous struct) in `aligator/utils/forward-dyn.hpp`
+
+### Removed
+
+- fwd.hpp : remove deprecated typedef `ODEDataTpl`
+- gar: remove CHOLDMOD backend (https://github.com/Simple-Robotics/aligator/pull/345)
+- gar: remove support for implicit dynamics in LQ solver interface
+  - remove member `LqrKnotTpl::E`
+  - simplify Riccati kernel algorithm
 
 ## [0.15.0] - 2025-05-23
 

CMakeLists.txt

@@ -149,11 +149,6 @@ cmake_dependent_option(
   OFF
 )
 option(BUILD_WITH_OPENMP_SUPPORT "Build the library with the OpenMP support" ON)
-option(
-  BUILD_WITH_CHOLMOD_SUPPORT
-  "Build with support for backends enabled by Cholmod"
-  OFF
-)
 option(BUILD_BENCHMARKS "Build benchmarks" ON)
 option(BUILD_EXAMPLES "Build examples" ON)
 cmake_dependent_option(
@@ -236,19 +231,6 @@ if(BUILD_CROCODDYL_COMPAT)
   list(APPEND CFLAGS_DEPENDENCIES "-DALIGATOR_WITH_CROCODDYL_COMPAT")
 endif()
 
-if(BUILD_WITH_CHOLMOD_SUPPORT)
-  set(
-    CMAKE_MODULE_PATH
-    ${JRL_CMAKE_MODULES}/find-external/CHOLMOD
-    ${CMAKE_MODULE_PATH}
-  )
-  ADD_PROJECT_DEPENDENCY(CHOLMOD REQUIRED)
-  message(
-    STATUS
-    "Build with CHOLMOD support (LGPL). See CHOLMOD/Doc/License.txt for further details."
-  )
-endif()
-
 if(BUILD_PYTHON_INTERFACE)
   set(PYTHON_COMPONENTS Interpreter Development NumPy)
   ADD_PROJECT_DEPENDENCY(
@@ -372,11 +354,6 @@ function(create_library)
     target_compile_definitions(${PROJECT_NAME} PUBLIC ALIGATOR_WITH_PINOCCHIO)
   endif()
 
-  if(BUILD_WITH_CHOLMOD_SUPPORT)
-    target_link_libraries(${PROJECT_NAME} PUBLIC CHOLMOD::CHOLMOD)
-    target_compile_definitions(${PROJECT_NAME} PUBLIC ALIGATOR_WITH_CHOLMOD)
-  endif()
-
   if(BUILD_WITH_OPENMP_SUPPORT)
     target_link_libraries(${PROJECT_NAME} PUBLIC OpenMP::OpenMP_CXX)
   endif()

README.md

@@ -75,6 +75,7 @@ cmake --build . -jNCPUS
 * (optional) [Pinocchio](https://github.com/stack-of-tasks/pinocchio) | [conda](https://anaconda.org/conda-forge/pinocchio)
 * (optional) [Crocoddyl](https://github.com/loco-3d/crocoddyl) | [conda](https://anaconda.org/conda-forge/crocoddyl)
 * (optional) [example-robot-data](https://github.com/Gepetto/example-robot-data) | [conda](https://anaconda.org/conda-forge/example-robot-data) (required for some examples and benchmarks)
+* (optional) [Catch2](https://github.com/catchorg/Catch2) | [conda](https://anaconda.org/conda-forge/catch2) for testing
 * a C++17 compliant compiler
 
 #### Python dependencies

bench/gar-riccati.cpp

@@ -15,46 +15,50 @@
 
 using namespace aligator::gar;
 
-const uint nx = 36;
-const uint nu = 12;
+static constexpr uint nx = 36;
+static constexpr uint nu = 12;
+static constexpr uint nc = 32;
+static constexpr double mueq = 1e-11;
+static std::mt19937 rng;
+static normal_unary_op normal_op{rng};
 
 static void BM_serial(benchmark::State &state) {
   uint horz = (uint)state.range(0);
-  VectorXs x0 = VectorXs::NullaryExpr(nx, normal_unary_op{});
-  const LqrProblemTpl<double> problem = generate_problem(x0, horz, nx, nu);
+  VectorXs x0 = VectorXs::NullaryExpr(nx, normal_op);
+  const LqrProblemTpl<double> problem =
+      generateLqProblem(rng, x0, horz, nx, nu, 0, nc);
   ProximalRiccatiSolver<double> solver(problem);
-  const double mu = 1e-11;
   auto [xs, us, vs, lbdas] = lqrInitializeSolution(problem);
   for (auto _ : state) {
-    solver.backward(mu, mu);
+    solver.backward(mueq);
     solver.forward(xs, us, vs, lbdas);
   }
 }
 
 #ifdef ALIGATOR_MULTITHREADING
 template <uint NPROC> static void BM_parallel(benchmark::State &state) {
   uint horz = (uint)state.range(0);
-  VectorXs x0 = VectorXs::NullaryExpr(nx, normal_unary_op{});
-  LqrProblemTpl<double> problem = generate_problem(x0, horz, nx, nu);
+  VectorXs x0 = VectorXs::NullaryExpr(nx, normal_op);
+  LqrProblemTpl<double> problem =
+      generateLqProblem(rng, x0, horz, nx, nu, 0, nc);
   ParallelRiccatiSolver<double> solver(problem, NPROC);
-  const double mu = 1e-11;
   auto [xs, us, vs, lbdas] = lqrInitializeSolution(problem);
   for (auto _ : state) {
-    solver.backward(mu, mu);
+    solver.backward(mueq);
     solver.forward(xs, us, vs, lbdas);
   }
 }
 #endif
 
 static void BM_stagedense(benchmark::State &state) {
   uint horz = (uint)state.range(0);
-  VectorXs x0 = VectorXs::NullaryExpr(nx, normal_unary_op{});
-  LqrProblemTpl<double> problem = generate_problem(x0, horz, nx, nu);
+  VectorXs x0 = VectorXs::NullaryExpr(nx, normal_op);
+  LqrProblemTpl<double> problem =
+      generateLqProblem(rng, x0, horz, nx, nu, 0, nc);
   RiccatiSolverDense<double> solver(problem);
-  const double mu = 1e-11;
   auto [xs, us, vs, lbdas] = lqrInitializeSolution(problem);
   for (auto _ : state) {
-    solver.backward(mu, mu);
+    solver.backward(mueq);
     solver.forward(xs, us, vs, lbdas);
   }
 }

bench/lqr.cpp

@@ -1,11 +1,11 @@
 /// @file
 /// @brief Linear-quadratic regulator
 
+#include "aligator/core/traj-opt-problem.hpp"
 #include "aligator/solvers/proxddp/solver-proxddp.hpp"
 #include "aligator/solvers/fddp/solver-fddp.hpp"
 #include "aligator/utils/rollout.hpp"
 #include "aligator/modelling/costs/quad-costs.hpp"
-
 #include "aligator/modelling/linear-discrete-dynamics.hpp"
 
 #include <benchmark/benchmark.h>

bindings/python/aligator/utils/plotting.py

@@ -3,25 +3,59 @@
 
 from aligator import HistoryCallback, Results
 
+_ROOT_10 = 10.0**0.5
+
+
+def plot_pd_errs(ax0, prim_errs, dual_errs):
+    import matplotlib.pyplot as plt
+
+    ax0: plt.Axes
+    prim_errs = np.asarray(prim_errs)
+    dual_errs = np.asarray(dual_errs)
+    ax0.plot(prim_errs, c="tab:blue")
+    ax0.set_xlabel("Iterations")
+    col2 = "tab:orange"
+    ax0.plot(dual_errs, c=col2)
+    ax0.spines["top"].set_visible(False)
+    ax0.spines["right"].set_color(col2)
+    ax0.yaxis.label.set_color(col2)
+    ax0.set_yscale("log")
+    ax0.legend(["Primal error $p$", "Dual error $d$"])
+    ax0.set_title("Solver primal-dual residuals")
+
+    # handle scaling
+    yhigh = ax0.get_ylim()[1]
+    if len(prim_errs) == 0 or len(dual_errs) == 0:
+        return
+    mach_eps = np.finfo(float).eps
+    dmask = dual_errs > 2 * mach_eps
+    pmask = prim_errs > 2 * mach_eps
+    ymin = np.finfo(float).max
+    if dmask.any():
+        ymin = np.min(dual_errs[dmask])
+    if pmask.any() and sum(prim_errs > 0) > 0:
+        ymin = min(np.min(prim_errs[pmask]), ymin)
+    ax0.set_ylim(ymin / _ROOT_10, yhigh)
+
 
 def plot_convergence(
     cb: HistoryCallback,
     ax: plt.Axes,
     res: Results = None,
     *,
     show_al_iters=False,
+    target_tol: float = None,
     legend_kwargs={},
 ):
-    from proxsuite_nlp.utils import plot_pd_errs
-
     prim_infeas = cb.prim_infeas.tolist()
     dual_infeas = cb.dual_infeas.tolist()
     if res is not None:
         prim_infeas.append(res.primal_infeas)
         dual_infeas.append(res.dual_infeas)
     plot_pd_errs(ax, prim_infeas, dual_infeas)
+
     ax.grid(axis="y", which="major")
-    handles, labels = ax.get_legend_handles_labels()
+    _, labels = ax.get_legend_handles_labels()
     labels += [
         "Prim. err $p$",
         "Dual err $d$",
@@ -41,6 +75,9 @@ def plot_convergence(
 
             ax.vlines(al_change_idx, *ax.get_ylim(), colors="gray", lw=4.0, alpha=0.5)
 
+    if target_tol:
+        ax.axhline(target_tol, color="k", lw=1.2)
+
     ax.legend(labels=labels, **legend_kwargs)
     return labels
 

bindings/python/include/aligator/python/fwd.hpp

@@ -1,6 +1,5 @@
 #pragma once
 
-#include "aligator/fwd.hpp"
 #include "aligator/context.hpp"
 
 namespace boost {

bindings/python/include/aligator/python/modelling/explicit-dynamics.hpp

@@ -1,5 +1,5 @@
 /// @file
-/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, INRIA
+/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, 2022-2025 INRIA
 #pragma once
 
 #include "aligator/python/fwd.hpp"
@@ -16,16 +16,16 @@ using context::DynamicsData;
 /// child virtual class (e.g. integrator classes).
 /// @tparam ExplicitBase The derived virtual class that is being exposed.
 /// @sa PyStageFunction
-template <class ExplicitBase = context::ExplicitDynamics>
+template <class Base = context::ExplicitDynamics>
 struct PyExplicitDynamics final
-    : ExplicitBase,
-      PolymorphicWrapper<PyExplicitDynamics<ExplicitBase>, ExplicitBase> {
+    : Base,
+      PolymorphicWrapper<PyExplicitDynamics<Base>, Base> {
   using Scalar = context::Scalar;
   ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
   // All functions in the interface take this type for output
   using Data = ExplicitDynamicsDataTpl<Scalar>;
 
-  using ExplicitBase::ExplicitBase;
+  using Base::Base;
 
   void forward(const ConstVectorRef &x, const ConstVectorRef &u,
                Data &data) const {
@@ -37,14 +37,11 @@ struct PyExplicitDynamics final
     ALIGATOR_PYTHON_OVERRIDE_PURE(void, "dForward", x, u, boost::ref(data));
   }
 
-  shared_ptr<DynamicsData> createData() const {
-    ALIGATOR_PYTHON_OVERRIDE(shared_ptr<DynamicsData>, ExplicitBase,
-                             createData, );
+  shared_ptr<Data> createData() const {
+    ALIGATOR_PYTHON_OVERRIDE(shared_ptr<Data>, Base, createData, );
   }
 
-  shared_ptr<DynamicsData> default_createData() const {
-    return ExplicitBase::createData();
-  }
+  shared_ptr<Data> default_createData() const { return Base::createData(); }
 };
 
 } // namespace python

bindings/python/src/expose-cartesian-product.cpp

@@ -38,7 +38,7 @@ void exposeCartesianProduct() {
           ("self"_a, "left", "right")))
       .def(
           "getComponent",
-          +[](CartesianProduct const &m, std::size_t i) -> const Manifold & {
+          +[](const CartesianProduct &m, std::size_t i) -> const Manifold & {
             if (i >= m.numComponents()) {
               PyErr_SetString(PyExc_IndexError, "Index out of bounds.");
               bp::throw_error_already_set();
@@ -47,8 +47,12 @@ void exposeCartesianProduct() {
           },
           bp::return_internal_reference<>(), ("self"_a, "i"),
           "Get the i-th component of the Cartesian product.")
-      .def("addComponent", &CartesianProduct::addComponent<PolymorphicManifold>,
-           ("self"_a, "c"), "Add a component to the Cartesian product.")
+      .def(
+          "addComponent",
+          +[](CartesianProduct &m, const PolymorphicManifold &c) {
+            m.addComponent(c);
+          },
+          ("self"_a, "c"), "Add a component to the Cartesian product.")
       .add_property("num_components", &CartesianProduct::numComponents,
                     "Get the number of components in the Cartesian product.")
       .def(