Add update_previous_solution into the warm_start

agimus_controller/agimus_controller/mpc.py

@@ -40,6 +40,7 @@ def run(self, initial_state: TrajectoryPoint, current_time_ns: int) -> OCPResult
         )
         timer3 = time.perf_counter_ns()
         self._ocp.solve(x0, x_init, u_init)
+        self._warm_start.update_previous_solution(self._ocp.debug_data.result)
         timer4 = time.perf_counter_ns()
 
         # Extract the solution.

agimus_controller/agimus_controller/warm_start_base.py

@@ -7,12 +7,18 @@
 class WarmStartBase(ABC):
     def __init__(self) -> None:
         super().__init__()
+        self._previous_solution: list[TrajectoryPoint] = list()
 
     @abstractmethod
     def generate(
         self,
         reference_trajectory: list[TrajectoryPoint],
-        previous_solution: list[TrajectoryPoint],
     ) -> tuple(np.ndarray, np.ndarray):
         """Returns x_init, u_init."""
         ...
+
+    def update_previous_solution(
+        self, previous_solution: list[TrajectoryPoint]
+    ) -> None:
+        """Stores internally the previous solution of the OCP"""
+        self._previous_solution = previous_solution