quadrotor_obstacles : use the plotting util

Author: ManifoldFR

examples/quadrotor_obstacles.py

@@ -324,6 +324,10 @@ def setup() -> aligator.TrajOptProblem:
     tol = 1e-4
     if args.bounds:
         mu_init = 1e1
+        if args.obstacles:
+            mu_init = 1.0
+    # elif args.obstacles:
+    #     mu_init = 1e-2
     else:
         mu_init = 1e-6
     verbose = aligator.VerboseLevel.VERBOSE
@@ -371,6 +375,8 @@ def plot_costate_value() -> plt.Figure:
 
     root_pt_opt = np.stack(xs_opt)[:, :3]
     if args.plot:
+        from aligator.utils import plotting
+
         if len(results.lams) > 0:
             plot_costate_value()
 
@@ -388,13 +394,13 @@ def plot_costate_value() -> plt.Figure:
         plt.legend(["$x$", "$y$", "$z$"])
         ax1.scatter([times_wp[-1]] * 3, x_term[:3], marker=".", c=["C0", "C1", "C2"])
         ax2: plt.Axes = fig.add_subplot(1, nplot, 3)
-        n_iter = np.arange(len(history_cb.prim_infeas.tolist()))
-        ax2.semilogy(
-            n_iter[1:], history_cb.prim_infeas.tolist()[1:], label="Primal err."
+        plotting.plot_convergence(
+            history_cb,
+            ax2,
+            res=results,
+            show_al_iters=True,
+            legend_kwargs=dict(fontsize=8),
         )
-        ax2.semilogy(n_iter, history_cb.dual_infeas.tolist(), label="Dual err.")
-        ax2.set_xlabel("Iterations")
-        ax2.legend()
 
         fig.tight_layout()
         for ext in ["png", "pdf"]: