Switch to nonmonotone linesearch

CHANGELOG.md

@@ -12,11 +12,14 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Add a multibody friction cone cost ([#234](https://github.com/Simple-Robotics/aligator/pull/234))
 - Add a `GravityCompensationResidual`, modelling $r(x,u) = Bu - G(q)$ ([#235](https://github.com/Simple-Robotics/aligator/pull/235))
 - Add Pixi support ([#240](https://github.com/Simple-Robotics/aligator/pull/240))
+- Added a nonmonotone linesearch procedure ([#244](https://github.com/Simple-Robotics/aligator/pull/244))
 
 ### Changed
 
+- The minimum required version of proxsuite-nlp is now 0.10.0 ([#244](https://github.com/Simple-Robotics/aligator/pull/244))
 - `SolverProxDDP`: add temporary vectors for linesearch
 - `SolverProxDDP`: remove exceptions from `computeMultipliers()`, return a bool flag
+- HistoryCallback: take solver instance as argument
 - `gar`: rework and move sparse matrix utilities to `gar/utils.hpp`
 - `SolverProxDDP`: Rename `maxRefinementSteps_` and `refinementThreshold_` to snake-case
 - `SolverProxDDP`: make `linesearch_` public

CMakeLists.txt

@@ -251,7 +251,7 @@ if(BUILD_PYTHON_INTERFACE)
   set(${PYLIB_NAME}_INSTALL_DIR ${PYTHON_SITELIB}/${PROJECT_NAME})
 endif()
 
-add_project_dependency(proxsuite-nlp 0.8.0 REQUIRED PKG_CONFIG_REQUIRES "proxsuite-nlp >= 0.8.0")
+add_project_dependency(proxsuite-nlp 0.10.0 REQUIRED PKG_CONFIG_REQUIRES "proxsuite-nlp >= 0.10.0")
 
 set(LIB_SOURCES src/utils/exceptions.cpp src/utils/logger.cpp)
 

bindings/python/aligator/utils/plotting.py

@@ -4,7 +4,14 @@
 from aligator import HistoryCallback, Results
 
 
-def plot_convergence(cb: HistoryCallback, ax: plt.Axes, res: Results = None):
+def plot_convergence(
+    cb: HistoryCallback,
+    ax: plt.Axes,
+    res: Results = None,
+    *,
+    show_al_iters=False,
+    legend_kwargs={},
+):
     from proxsuite_nlp.utils import plot_pd_errs
 
     prim_infeas = cb.prim_infeas.tolist()
@@ -14,7 +21,28 @@ def plot_convergence(cb: HistoryCallback, ax: plt.Axes, res: Results = None):
         dual_infeas.append(res.dual_infeas)
     plot_pd_errs(ax, prim_infeas, dual_infeas)
     ax.grid(axis="y", which="major")
-    return
+    handles, labels = ax.get_legend_handles_labels()
+    labels += [
+        "Prim. err $p$",
+        "Dual err $d$",
+    ]
+    if show_al_iters:
+        prim_tols = np.array(cb.prim_tols)
+        al_iters = np.array(cb.al_index)
+        labels.append("$\\eta_k$")
+
+        itrange = np.arange(len(al_iters))
+        if itrange.size > 0:
+            if al_iters.max() > 0:
+                labels.append("AL iters")
+            ax.step(itrange, prim_tols, c="green", alpha=0.9, lw=1.1)
+            al_change = al_iters[1:] - al_iters[:-1]
+            al_change_idx = itrange[:-1][al_change > 0]
+
+            ax.vlines(al_change_idx, *ax.get_ylim(), colors="gray", lw=4.0, alpha=0.5)
+
+    ax.legend(labels=labels, **legend_kwargs)
+    return labels
 
 
 def plot_se2_pose(
@@ -50,14 +78,17 @@ def plot_controls_traj(
     joint_names=None,
     rmodel=None,
     figsize=(6.4, 6.4),
+    xlabel="Time (s)",
 ) -> tuple[plt.Figure, list[plt.Axes]]:
     t0 = times[0]
     tf = times[-1]
     us = np.asarray(us)
     nu = us.shape[1]
     nrows, r = divmod(nu, ncols)
     nrows += int(r > 0)
-    if axes is None:
+
+    make_new_plot = axes is None
+    if make_new_plot:
         fig, axes = plt.subplots(nrows, ncols, sharex="col", figsize=figsize)
     else:
         fig = axes.flat[0].get_figure()
@@ -77,9 +108,13 @@ def plot_controls_traj(
             ax.set_ylim(*ylim)
         if joint_names is not None:
             joint_name = joint_names[i].lower()
-            ax.set_ylabel(joint_name)
-    fig.supxlabel("Time $t$")
-    fig.suptitle("Control trajectories")
+            ax.set_title(joint_name, fontsize=8)
+    if nu > 1:
+        fig.supxlabel(xlabel)
+        fig.suptitle("Control trajectories")
+    else:
+        axes[0].set_xlabel(xlabel)
+        axes[0].set_title("Control trajectories")
     fig.tight_layout()
     return fig, axes
 
@@ -92,6 +127,7 @@ def plot_velocity_traj(
     ncols=2,
     vel_limit=None,
     figsize=(6.4, 6.4),
+    xlabel="Time (s)",
 ) -> tuple[plt.Figure, list[plt.Axes]]:
     vs = np.asarray(vs)
     nv = rmodel.nv
@@ -111,7 +147,7 @@ def plot_velocity_traj(
     tf = times[-1]
 
     if axes is None:
-        fig, axes = plt.subplots(nrows, ncols, figsize=figsize)
+        fig, axes = plt.subplots(nrows, ncols, sharex=True, figsize=figsize)
         fig: plt.Figure
     else:
         fig = axes.flat[0].get_figure()
@@ -127,9 +163,9 @@ def plot_velocity_traj(
             ax.hlines(-vel_limit[i], t0, tf, colors="k", linestyles="--")
             ax.hlines(+vel_limit[i], t0, tf, colors="r", linestyles="dashdot")
             ax.set_ylim(*ylim)
-        ax.set_ylabel(joint_name)
+        ax.set_title(joint_name, fontsize=8)
 
-    fig.supxlabel("Time $t$")
+    fig.supxlabel(xlabel)
     fig.suptitle("Velocity trajectories")
     fig.tight_layout()
     return fig, axes

bindings/python/src/expose-callbacks.cpp

@@ -2,21 +2,29 @@
 #include "aligator/python/callbacks.hpp"
 #include "aligator/helpers/history-callback.hpp"
 
+#include "aligator/solvers/proxddp/solver-proxddp.hpp"
+#include "aligator/solvers/fddp/solver-fddp.hpp"
+
 namespace aligator {
 namespace python {
 
 using context::Scalar;
+using context::SolverFDDP;
+using context::SolverProxDDP;
+using HistoryCallback = HistoryCallbackTpl<Scalar>;
+
+#define ctor(Solver)                                                           \
+  bp::init<Solver *, bool, bool>(                                              \
+      ("self"_a, "solver", "store_pd_vars"_a = true, "store_values"_a = true))
 
 void exposeHistoryCallback() {
-  using HistoryCallback = HistoryCallbackTpl<Scalar>;
 
   bp::scope in_history =
       bp::class_<HistoryCallback, bp::bases<CallbackBase>>(
           "HistoryCallback", "Store the history of solver's variables.",
-          bp::init<bool, bool, bool>((bp::arg("self"),
-                                      bp::arg("store_pd_vars") = true,
-                                      bp::arg("store_values") = true,
-                                      bp::arg("store_residuals") = true)))
+          bp::no_init)
+          .def(ctor(SolverProxDDP))
+          .def(ctor(SolverFDDP))
 #define _c(name) def_readonly(#name, &HistoryCallback::name)
           ._c(xs)
           ._c(us)
@@ -36,9 +44,8 @@ void exposeHistoryCallback() {
 void exposeCallbacks() {
   bp::register_ptr_to_python<shared_ptr<CallbackBase>>();
 
-  bp::class_<CallbackWrapper, boost::noncopyable>("BaseCallback",
-                                                  "Base callback for solvers.",
-                                                  bp::init<>(bp::args("self")))
+  bp::class_<CallbackWrapper, boost::noncopyable>(
+      "BaseCallback", "Base callback for solvers.", bp::init<>(("self"_a)))
       .def("call", bp::pure_virtual(&CallbackWrapper::call),
            bp::args("self", "workspace", "results"));
 

bindings/python/src/expose-solver-prox.cpp

@@ -19,8 +19,8 @@ void exposeProxDDP() {
   using context::VectorRef;
   using context::Workspace;
 
-  eigenpy::register_symbolic_link_to_registered_type<
-      Linesearch<Scalar>::Options>();
+  using LsOptions = Linesearch<Scalar>::Options;
+  eigenpy::register_symbolic_link_to_registered_type<LsOptions>();
   eigenpy::register_symbolic_link_to_registered_type<LinesearchStrategy>();
   eigenpy::register_symbolic_link_to_registered_type<
       proxsuite::nlp::LSInterpolation>();
@@ -143,13 +143,20 @@ void exposeProxDDP() {
                "(target_tol) will not be synced when the latter changes and "
                "`solver.run()` is called.")
           .def(SolverVisitor<SolverType>())
+          .def_readonly("linesearch", &SolverType::linesearch_)
           .def("run", &SolverType::run,
                ("self"_a, "problem", "xs_init"_a = bp::list(),
                 "us_init"_a = bp::list(), "vs_init"_a = bp::list(),
                 "lams_init"_a = bp::list()),
                "Run the algorithm. Can receive initial guess for "
                "multiplier trajectory.");
 
+  bp::class_<NonmonotoneLinesearch<Scalar>, bp::bases<Linesearch<Scalar>>>(
+      "NonmonotoneLinesearch", bp::no_init)
+      .def(bp::init<LsOptions>(("self"_a, "options")))
+      .def_readwrite("avg_eta", &NonmonotoneLinesearch<Scalar>::avg_eta)
+      .def_readwrite("beta_dec", &NonmonotoneLinesearch<Scalar>::beta_dec);
+
   {
     using AlmParams = SolverType::AlmParams;
     bp::scope scope{cls};

bindings/python/src/gar/expose-utils.cpp

@@ -7,6 +7,7 @@ namespace aligator::python {
 using namespace gar;
 
 using context::Scalar;
+using context::VectorXs;
 using lqr_t = LQRProblemTpl<context::Scalar>;
 
 bp::dict lqr_sol_initialize_wrap(const lqr_t &problem) {
@@ -20,6 +21,15 @@ bp::dict lqr_sol_initialize_wrap(const lqr_t &problem) {
   return out;
 }
 
+bp::tuple lqr_create_sparse_wrap(const lqr_t &problem, const Scalar mudyn,
+                                 const Scalar mueq, bool update) {
+  Eigen::SparseMatrix<Scalar> mat;
+  VectorXs rhs;
+  lqrCreateSparseMatrix(problem, mudyn, mueq, mat, rhs, update);
+  mat.makeCompressed();
+  return bp::make_tuple(mat, rhs);
+}
+
 void exposeGarUtils() {
 
   bp::def(
@@ -30,8 +40,8 @@ void exposeGarUtils() {
       },
       ("problem"_a, "mudyn", "mueq"));
 
-  bp::def("lqrCreateSparseMatrix", lqrCreateSparseMatrix<Scalar>,
-          ("problem"_a, "mudyn", "mueq", "mat", "rhs", "update"),
+  bp::def("lqrCreateSparseMatrix", lqr_create_sparse_wrap,
+          ("problem"_a, "mudyn", "mueq", "update"),
           "Create or update a sparse matrix from an LQRProblem.");
 
   bp::def("lqrInitializeSolution", lqr_sol_initialize_wrap, ("problem"_a));

examples/acrobot.py

@@ -70,10 +70,10 @@ class Args(ArgsBase):
     )
 
 tol = 1e-3
-mu_init = 10.0
+mu_init = 1e-2
 solver = aligator.SolverProxDDP(tol, mu_init=mu_init, verbose=aligator.VERBOSE)
 solver.max_iters = 200
-solver.rollout_type = aligator.ROLLOUT_LINEAR
+solver.rollout_type = aligator.ROLLOUT_NONLINEAR
 solver.linear_solver_choice = aligator.LQ_SOLVER_STAGEDENSE
 solver.setup(problem)
 
@@ -96,23 +96,28 @@ class Args(ArgsBase):
     from aligator.utils.plotting import plot_controls_traj
 
     times = np.linspace(0, Tf, nsteps + 1)
-    fig1 = plot_controls_traj(times, res.us, ncols=1, rmodel=rmodel, figsize=(6.4, 3.2))
+    fig1, axes = plot_controls_traj(
+        times, res.us, ncols=1, rmodel=rmodel, figsize=(6.4, 3.2)
+    )
+    plt.title("Controls (N/m)")
     fig1.tight_layout()
     xs = np.stack(res.xs)
     vs = xs[:, nq:]
 
     theta_s = np.zeros((nsteps + 1, 2))
     theta_s0 = space.difference(space.neutral(), x0)[:2]
     theta_s = theta_s0 + np.cumsum(vs * timestep, axis=0)
-    fig2 = plt.figure(figsize=(6.4, 6.4))
-    plt.subplot(211)
+    fig2 = plt.figure(figsize=(6.4, 3.2))
+    plt.subplot(1, 2, 1)
     plt.plot(times, theta_s, label=("$\\theta_0$", "$\\theta_1$"))
-    plt.title("Joint angles")
+    plt.title("Joint angles (rad)")
+    plt.xlabel("Time (s)")
     plt.legend()
-    plt.subplot(212)
+    plt.subplot(1, 2, 2)
     plt.plot(times, xs[:, nq:], label=("$\\dot\\theta_0$", "$\\dot\\theta_1$"))
     plt.legend()
-    plt.title("Joint velocities")
+    plt.title("Joint velocities (rad/s)")
+    plt.xlabel("Time (s)")
     fig2.tight_layout()
 
     _fig_dict = {"controls": fig1, "velocities": fig2}