Merge pull request #244 from Simple-Robotics/topic/nonmonotone-ls

Switch to nonmonotone linesearch

Author: ManifoldFR

CHANGELOG.md

@@ -12,11 +12,17 @@ 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))
+- Add enum value `StepAcceptanceStrategy::LINESEARCH_NONMONOTONE` ([#244](https://github.com/Simple-Robotics/aligator/pull/244))
 
 ### Changed
 
+- **API BREAKING:** Change enum value `StepAcceptanceStrategy::LINESEARCH` to `LINESEARCH_NONMONOTONE` ([#244](https://github.com/Simple-Robotics/aligator/pull/244))
+  - Add constructor argument `StepAcceptanceStrategy sa_strategy`, defaults to nonmonotone
+- 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

@@ -216,7 +216,7 @@ set(BOOST_REQUIRED_COMPONENTS filesystem)
 set_boost_default_options()
 export_boost_default_options()
 add_project_dependency(Boost REQUIRED COMPONENTS ${BOOST_REQUIRED_COMPONENTS})
-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")
 
 if(BUILD_WITH_PINOCCHIO_SUPPORT)
   message(STATUS "Building aligator with Pinocchio support.")

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

@@ -7,6 +7,7 @@
 #include "aligator/solvers/proxddp/solver-proxddp.hpp"
 
 #include <eigenpy/std-unique-ptr.hpp>
+#include <eigenpy/variant.hpp>
 
 namespace aligator {
 namespace python {
@@ -19,8 +20,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>();
@@ -74,6 +75,7 @@ void exposeProxDDP() {
       .def(PrintableVisitor<Results>());
 
   using SolverType = SolverProxDDPTpl<Scalar>;
+  using ls_variant_t = SolverType::LinesearchVariant::variant_t;
 
   auto cls =
       bp::class_<SolverType, boost::noncopyable>(
@@ -84,9 +86,10 @@ void exposeProxDDP() {
           " The solver instance initializes both a Workspace and a Results "
           "struct.",
           bp::init<const Scalar, const Scalar, std::size_t, VerboseLevel,
-                   HessianApprox>(
+                   StepAcceptanceStrategy, HessianApprox>(
               ("self"_a, "tol", "mu_init"_a = 1e-2, "max_iters"_a = 1000,
                "verbose"_a = VerboseLevel::QUIET,
+               "sa_strategy"_a = StepAcceptanceStrategy::LINESEARCH_NONMONOTONE,
                "hess_approx"_a = HessianApprox::GAUSS_NEWTON)))
           .def("cycleProblem", &SolverType::cycleProblem,
                ("self"_a, "problem", "data"),
@@ -110,7 +113,7 @@ void exposeProxDDP() {
           .def_readwrite("max_al_iters", &SolverType::max_al_iters,
                          "Maximum number of AL iterations.")
           .def_readwrite("ls_mode", &SolverType::ls_mode, "Linesearch mode.")
-          .def_readwrite("sa_strategy", &SolverType::sa_strategy,
+          .def_readwrite("sa_strategy", &SolverType::sa_strategy_,
                          "StepAcceptance strategy.")
           .def_readwrite("rollout_type", &SolverType::rollout_type_,
                          "Rollout type.")
@@ -120,7 +123,6 @@ void exposeProxDDP() {
                          "Minimum regularization value.")
           .def_readwrite("reg_max", &SolverType::reg_max,
                          "Maximum regularization value.")
-          .def_readwrite("lq_print_detailed", &SolverType::lq_print_detailed)
           .def("updateLQSubproblem", &SolverType::updateLQSubproblem, "self"_a)
           .def("computeCriterion", &SolverType::computeCriterion, "self"_a,
                "Compute problem stationarity.")
@@ -143,13 +145,25 @@ void exposeProxDDP() {
                "(target_tol) will not be synced when the latter changes and "
                "`solver.run()` is called.")
           .def(SolverVisitor<SolverType>())
+          .add_property("linesearch",
+                        bp::make_function(
+                            +[](const SolverType &s) -> const ls_variant_t & {
+                              return s.linesearch_;
+                            },
+                            eigenpy::ReturnInternalVariant<ls_variant_t>{}))
           .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/module.cpp

@@ -42,7 +42,9 @@ static void exposeEnums() {
 
   bp::enum_<StepAcceptanceStrategy>("StepAcceptanceStrategy",
                                     "Step acceptance strategy.")
-      .value("SA_LINESEARCH", StepAcceptanceStrategy::LINESEARCH)
+      .value("SA_LINESEARCH_ARMIJO", StepAcceptanceStrategy::LINESEARCH_ARMIJO)
+      .value("SA_LINESEARCH_NONMONOTONE",
+             StepAcceptanceStrategy::LINESEARCH_NONMONOTONE)
       .value("SA_FILTER", StepAcceptanceStrategy::FILTER)
       .export_values();
 }

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}