Repository: agimus-project/agimus_controller
Pull request #201: Improve the MPC ROS node
Author: login: jmirabel
Created: 2025-03-12T15:24:37Z | Merged: 2025-03-13T10:29:05Z

- Add initialization of the OCP
- Use WarmStartShiftPreviousSolution
- Fix logic of the buffer size handling
- Fix debug data (requires https://github.com/agimus-project/agimus_msgs/pull/6)


Diff:
diff --git a/agimus_controller/agimus_controller/mpc_data.py b/agimus_controller/agimus_controller/mpc_data.py
index 60c88220..fe92c608 100644
--- a/agimus_controller/agimus_controller/mpc_data.py
+++ b/agimus_controller/agimus_controller/mpc_data.py
@@ -1,6 +1,7 @@
 from dataclasses import dataclass, field
 import numpy as np
 import numpy.typing as npt
+import typing as T
 
 from agimus_controller.trajectory import TrajectoryPoint
 
@@ -19,7 +20,10 @@ class OCPDebugData:
     # Debug data
     result: OCPResults = OCPResults()
     references: list[TrajectoryPoint] = field(default_factory=list)
-    collision_distance_residuals: list[dict[np.float64]] = field(default_factory=list)
+    residuals: T.Dict[str, T.List[npt.NDArray[np.float64]]] = field(
+        default_factory=dict
+    )
+
     # Solver infos
     kkt_norm: np.float64 = 0.0
     nb_iter: np.int64 = 0
diff --git a/agimus_controller/agimus_controller/ocp_base.py b/agimus_controller/agimus_controller/ocp_base.py
index e7efe2ec..08cdd0e4 100644
--- a/agimus_controller/agimus_controller/ocp_base.py
+++ b/agimus_controller/agimus_controller/ocp_base.py
@@ -55,6 +55,7 @@ def solve(
         x0: npt.NDArray[np.float64],
         x_warmstart: list[npt.NDArray[np.float64]],
         u_warmstart: list[npt.NDArray[np.float64]],
+        use_iteration_limits_and_timeout: bool = True,
     ) -> None:
         """Solver for the OCP. This method should be implemented by the derived class.
         The method should solve the OCP for the given initial state and warmstart values.
@@ -63,6 +64,8 @@ def solve(
             x0 (npt.NDArray[np.float64]): current state of the robot.
             x_warmstart (list[npt.NDArray[np.float64]]): Warmstart values for the state. This should be of size `n_controls + 1`.
             u_warmstart (list[npt.NDArray[np.float64]]): Warmstart values for the control inputs. This should be of size `n_controls`.
+            use_iteration_limits_and_timeout (bool): Set to False to skip the max solve time and max iterations of the OCP.
+                This is useful for the first call to solve.
         """
         pass
 
diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index cbb82823..bc5c8d00 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -58,6 +58,8 @@ def __init__(
         self._solver.use_filter_line_search = self._ocp_params.use_filter_line_search
 
         # Parameters of the solver
+        if self._ocp_params.max_solve_time is not None:
+            self._solver.max_solve_time = self._ocp_params.max_solve_time
         self._solver.termination_tolerance = self._ocp_params.termination_tolerance
         self._solver.max_qp_iters = self._ocp_params.qp_iters
         self._solver.eps_abs = self._ocp_params.eps_abs
@@ -106,6 +108,7 @@ def solve(
         x0: npt.NDArray[np.float64],
         x_warmstart: list[npt.NDArray[np.float64]],
         u_warmstart: list[npt.NDArray[np.float64]],
+        use_iteration_limits_and_timeout: bool = True,
     ) -> None:
         """Solves the OCP.
         The results can be accessed through the ocp_results property.
@@ -118,9 +121,19 @@ def solve(
         # Set the initial state
         self._problem.x0 = x0
         # Solve the OCP
-        res = self._solver.solve(
-            x_warmstart, u_warmstart, self._ocp_params.solver_iters
+        max_iters = (
+            self._ocp_params.solver_iters if use_iteration_limits_and_timeout else 1000
         )
+        # This if is only meant as a way of smoothing the integration of attribute
+        # `SolverCSQP.max_solve_time`. If the user did not set `max_solve_time`, then
+        # don't touch the attribute.
+        if self._ocp_params.max_solve_time is not None:
+            self._solver.max_solve_time = (
+                self._ocp_params.max_solve_time
+                if use_iteration_limits_and_timeout
+                else float("inf")
+            )
+        res = self._solver.solve(x_warmstart, u_warmstart, max_iters)
         ocp_results = OCPResults(
             states=self._solver.xs,
             ricatti_gains=self._solver.K,
@@ -132,6 +145,23 @@ def solve(
             self._debug_data.kkt_norm = self._solver.KKT
             self._debug_data.nb_iter = int(self._solver.iter)
             self._debug_data.nb_qp_iter = int(self._solver.qp_iters)
+            # For now, we don't publish any residuals for performance reasons.
+            # See https://github.com/agimus-project/agimus_controller/pull/201#discussion_r1993106004
+            # if len(self._debug_data.residuals) == 0:
+            #     names = (
+            #         self._problem.runningModels[0]
+            #         .differential.costs.costs.todict()
+            #         .keys()
+            #     )
+            # else:
+            #     names = self._debug_data.residuals.keys()
+            # self._debug_data.residuals = {name: [] for name in names}
+
+            # for data in self._problem.runningDatas:
+            #     for name in names:
+            #         self._debug_data.residuals[name].append(
+            #             data.differential.costs.costs[name].residual.r
+            #         )
 
         # Store the results
         self._ocp_results = ocp_results
diff --git a/agimus_controller/agimus_controller/ocp_param_base.py b/agimus_controller/agimus_controller/ocp_param_base.py
index 530b23ea..598b1418 100644
--- a/agimus_controller/agimus_controller/ocp_param_base.py
+++ b/agimus_controller/agimus_controller/ocp_param_base.py
@@ -1,4 +1,5 @@
 from dataclasses import dataclass, field
+import typing as T
 
 
 @dataclass
@@ -53,6 +54,9 @@ class OCPParamsBaseCroco:
     termination_tolerance: float = (
         1e-3  # Termination tolerance (norm of the KKT conditions).
     )
+    max_solve_time: T.Optional[float] = (
+        None  # timeout of the OCP solve function. Set it to None to skip using it.
+    )
     eps_abs: float = 1e-6  # Absolute tolerance of the solver.
     eps_rel: float = 0.0  # Relative tolerance of the solver.
     callbacks: bool = False  # Flag to enable/disable callbacks.
diff --git a/agimus_controller/agimus_controller/warm_start_shift_previous_solution.py b/agimus_controller/agimus_controller/warm_start_shift_previous_solution.py
index 02ad41df..2946f7b6 100644
--- a/agimus_controller/agimus_controller/warm_start_shift_previous_solution.py
+++ b/agimus_controller/agimus_controller/warm_start_shift_previous_solution.py
@@ -78,7 +78,7 @@ def generate(
             [initial_state.robot_configuration, initial_state.robot_velocity]
         )
         # TODO is copy needed ?
-        xinit = self._previous_solution.states[1:].copy()
+        xinit = self._previous_solution.states.copy()
         uinit = self._previous_solution.feed_forward_terms.copy()
         return x0, xinit, uinit
 
diff --git a/agimus_controller_ros/agimus_controller_ros/agimus_controller.py b/agimus_controller_ros/agimus_controller_ros/agimus_controller.py
index 4f0a5188..cb99416d 100644
--- a/agimus_controller_ros/agimus_controller_ros/agimus_controller.py
+++ b/agimus_controller_ros/agimus_controller_ros/agimus_controller.py
@@ -25,6 +25,9 @@
 from agimus_controller.ocp.ocp_croco_goal_reaching import OCPCrocoGoalReaching
 from agimus_controller.ocp_param_base import OCPParamsBaseCroco
 from agimus_controller.warm_start_reference import WarmStartReference
+from agimus_controller.warm_start_shift_previous_solution import (
+    WarmStartShiftPreviousSolution,
+)
 from agimus_controller.factory.robot_model import RobotModels, RobotModelParameters
 
 
@@ -55,6 +58,15 @@ def __init__(self, node_name: str = "agimus_controller_node") -> None:
             )
             for collision_pair_name in self.params.collision_pairs_names
         ]
+        self.ocp_params = OCPParamsBaseCroco(
+            dt=self.params.ocp.dt,
+            dt_factor_n_seq=self.params.ocp.dt_factor_n_seq,
+            horizon_size=self.params.ocp.horizon_size,
+            solver_iters=self.params.ocp.max_iter,
+            callbacks=self.params.ocp.activate_callback,
+            qp_iters=self.params.ocp.max_qp_iter,
+            use_debug_data=self.params.publish_debug_data,
+        )
         self.last_point = None
         self.first_run_done = False
         self.rmodel = None
@@ -188,27 +200,36 @@ def initialize_ros_attributes(self) -> None:
                     reliability=ReliabilityPolicy.BEST_EFFORT,
                 ),
             )
-        self.create_timer(1.0 / self.params.rate, self.run_callback)
+
+        self._init_timer = self.create_timer(0.1, self.initialization_callback)
 
     def setup_mpc(self):
         """Creates mpc, ocp, warmstart"""
+        ocp = OCPCrocoGoalReaching(self.robot_models, self.ocp_params)
+        ws_shift = WarmStartShiftPreviousSolution()
+        ws_shift.setup(self.robot_models, self.ocp_params)
 
-        ocp_params = OCPParamsBaseCroco(
-            dt=self.params.ocp.dt,
-            dt_factor_n_seq=self.params.ocp.dt_factor_n_seq,
-            horizon_size=self.params.ocp.horizon_size,
-            solver_iters=self.params.ocp.max_iter,
-            callbacks=self.params.ocp.activate_callback,
-            qp_iters=self.params.ocp.max_qp_iter,
-            use_debug_data=self.params.publish_debug_data,
+        # Use WarmStartReference for initialization
+        ws_ref = WarmStartReference()
+        ws_ref.setup(self.robot_models._robot_model)
+
+        np_sensor_msg: lfc_py_types.Sensor = sensor_msg_to_numpy(self.sensor_msg)
+        initial_state = TrajectoryPoint(
+            time_ns=self.get_clock().now().nanoseconds,
+            robot_configuration=np_sensor_msg.joint_state.position,
+            robot_velocity=np_sensor_msg.joint_state.velocity,
+            robot_acceleration=np.zeros_like(np_sensor_msg.joint_state.velocity),
         )
-        self.ocp_params = ocp_params
+        reference_trajectory = self.traj_buffer.horizon
+        ocp.set_reference_weighted_trajectory(reference_trajectory)
+
+        reference_trajectory_points = [el.point for el in reference_trajectory]
+        x0, x_init, u_init = ws_ref.generate(initial_state, reference_trajectory_points)
+        ocp.solve(x0, x_init, u_init, use_iteration_limits_and_timeout=False)
+        ws_shift.update_previous_solution(ocp.ocp_results)
 
-        ocp = OCPCrocoGoalReaching(self.robot_models, ocp_params)
-        ws = WarmStartReference()
-        ws.setup(self.robot_models._robot_model)
         self.mpc = MPC()
-        self.mpc.setup(ocp=ocp, warm_start=ws, buffer=self.traj_buffer)
+        self.mpc.setup(ocp=ocp, warm_start=ws_shift, buffer=self.traj_buffer)
 
     def sensor_callback(self, sensor_msg: Sensor) -> None:
         """Update the sensor_msg attribute of the class."""
@@ -224,7 +245,7 @@ def joint_states_callback(self, joint_states_msg: JointState) -> None:
             return
         if self.q0 is None:
             self.q0 = np.array(joint_states_msg.position)
-            self.create_robot_models()
+            self.destroy_subscription(self.state_subscriber)
 
     def mpc_input_callback(self, msg: MpcInput) -> None:
         """Fill the new point msg in the trajectory buffer."""
@@ -264,13 +285,14 @@ def create_robot_models(self) -> None:
 
         self.get_logger().info("Robot Models initialized")
 
-    def buffer_has_twice_horizon_points(self) -> bool:
+    def buffer_has_enough_data(self, ratio: float) -> bool:
         """
-        Return true if buffer size has more than two times
+        Return True if buffer size is more than `ratio` times
         the horizon size and False otherwise.
         """
         return (
-            len(self.traj_buffer) * self.ocp_params.dt >= 2 * self.ocp_params.total_time
+            len(self.traj_buffer) * self.ocp_params.dt
+            >= ratio * self.ocp_params.total_time
         )
 
     def send_control_msg(self, ocp_res: OCPResults) -> None:
@@ -283,40 +305,71 @@ def send_control_msg(self, ocp_res: OCPResults) -> None:
         )
         self.control_publisher.publish(control_numpy_to_msg(ctrl_msg))
 
-    def run_callback(self, *args) -> None:
-        """
-        Timer callback that checks we can start solve before doing it,
-        then publish messages related to the OCP.
-        """
-        if self.params.constant_delay:
-            if self._ocp_res is not None:
-                # Publish the result from previous OCP.
-                self.send_control_msg(self._ocp_res)
-                control = self._ocp_res.feed_forward_terms[0].copy()
-                self._ocp_res = None
-            else:
-                control = None
+    def initialization_callback(self) -> None:
         if self.sensor_msg is None:
             self.get_logger().warn(
                 "Waiting for sensor messages to arrive...",
                 throttle_duration_sec=5.0,
             )
             return
-        if self.rmodel is None:
+
+        if self.q0 is None:
             self.get_logger().warn(
-                "Waiting for robot model to arrive...",
+                "Waiting for robot descriptions...",
                 throttle_duration_sec=5.0,
             )
             return
-        if self.mpc is None:
-            self.setup_mpc()
 
-        if not self.buffer_has_twice_horizon_points():
+        # Wait for enough data in buffer
+        if not self.buffer_has_enough_data(2):
             self.get_logger().warn(
-                f"Waiting for buffer to be filled... Current size {len(self.traj_buffer)}",
+                f"MPC is waiting for the buffer to be filled... Current size {len(self.traj_buffer)}",
                 throttle_duration_sec=5.0,
             )
             return
+
+        # Stop the initialization loop.
+        self.destroy_timer(self._init_timer)
+
+        self.create_robot_models()
+        self.setup_mpc()
+
+        self.get_logger().info(
+            "MPC is initialized and buffer has enough data. Starting to compute controls."
+        )
+        # Start the MPC loop
+        self.create_timer(1.0 / self.params.rate, self.run_callback)
+
+    def run_callback(self, *args) -> None:
+        """
+        Timer callback that checks we can start solve before doing it,
+        then publish messages related to the OCP.
+        """
+        if self.params.constant_delay:
+            if self._ocp_res is not None:
+                # Publish the result from previous OCP.
+                self.send_control_msg(self._ocp_res)
+                control = self._ocp_res.feed_forward_terms[0].copy()
+                self._ocp_res = None
+            else:
+                control = None
+
+        # Check the buffer size.
+        # If size is inferior to `1.5 * required size`, send a warning message. Note that the
+        # ratio here must be strictly inferior to the ratio that triggers starting.
+        # If size is inferior to the required size, send an error message and return.
+        if not self.buffer_has_enough_data(1.5):
+            if self.buffer_has_enough_data(1):
+                self.get_logger().warn(
+                    f"MPC is running and the buffer size becomes low. Current size {len(self.traj_buffer)}",
+                    throttle_duration_sec=1.0,
+                )
+            else:
+                self.get_logger().error(
+                    f"MPC is running but the buffer does not have enough data. Current size {len(self.traj_buffer)}",
+                    throttle_duration_sec=1.0,
+                )
+                return
         if self.params.publish_debug_data:
             start_compute_time = self.get_clock().now()
         self.np_sensor_msg: lfc_py_types.Sensor = sensor_msg_to_numpy(self.sensor_msg)
diff --git a/agimus_controller_ros/agimus_controller_ros/ros_utils.py b/agimus_controller_ros/agimus_controller_ros/ros_utils.py
index e623376a..0c9b6184 100644
--- a/agimus_controller_ros/agimus_controller_ros/ros_utils.py
+++ b/agimus_controller_ros/agimus_controller_ros/ros_utils.py
@@ -4,7 +4,7 @@
 from linear_feedback_controller_msgs_py.numpy_conversions import matrix_numpy_to_msg
 
 from geometry_msgs.msg import Pose
-from agimus_msgs.msg import MpcInput, MpcDebug
+from agimus_msgs.msg import MpcInput, MpcDebug, Residual
 
 from agimus_controller.trajectory import (
     TrajectoryPoint,
@@ -63,12 +63,10 @@ def mpc_debug_data_to_msg(mpc_debug_data: MPCDebugData) -> MpcDebug:
     mpc_debug_msg.control_predictions = matrix_numpy_to_msg(
         np.array(mpc_debug_data.ocp.result.feed_forward_terms)
     )
-    if mpc_debug_data.ocp.collision_distance_residuals is not None:
-        mpc_debug_msg.collision_distance_residuals = matrix_numpy_to_msg(
-            np.array(mpc_debug_data.ocp.collision_distance_residuals)
+    for name, data in mpc_debug_data.ocp.residuals.items():
+        mpc_debug_msg.residuals.append(
+            Residual(name=name, data=matrix_numpy_to_msg(np.asarray(data)))
         )
-    else:
-        mpc_debug_msg.collision_distance_residuals = matrix_numpy_to_msg(np.zeros((1)))
 
     mpc_debug_msg.kkt_norm = mpc_debug_data.ocp.kkt_norm
     mpc_debug_msg.nb_iter = mpc_debug_data.ocp.nb_iter


Reviews:
user: login: MaximilienNaveau
state: APPROVED
author_association: COLLABORATOR
submitted_at: 2025-03-12T16:42:30Z
user: login: TheoMF
state: APPROVED
author_association: COLLABORATOR
submitted_at: 2025-03-13T09:33:28Z
user: login: jmirabel
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-13T09:42:34Z
user: login: jmirabel
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-13T09:43:19Z
user: login: jmirabel
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-13T09:50:20Z
user: login: TheoMF
state: COMMENTED
author_association: COLLABORATOR
submitted_at: 2025-03-13T09:52:07Z
user: login: jmirabel
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-13T09:52:58Z
user: login: jmirabel
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-13T09:54:09Z
user: login: TheoMF
state: COMMENTED
author_association: COLLABORATOR
submitted_at: 2025-03-13T09:58:48Z
user: login: jmirabel
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-13T10:02:22Z
user: login: jmirabel
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-13T10:03:33Z
user: login: jmirabel
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-13T10:10:05Z

Review comments:
diff_hunk: @@ -118,9 +121,20 @@ def solve(
         # Set the initial state
         self._problem.x0 = x0
         # Solve the OCP
-        res = self._solver.solve(
-            x_warmstart, u_warmstart, self._ocp_params.solver_iters
+        max_iters = (
+            self._ocp_params.solver_iters if use_iteration_limits_and_timeout else 1000
         )
+        # If mim_solvers is at the latest state
+        has_max_solve_time = hasattr(self._solver, "max_solve_time")
+        if use_iteration_limits_and_timeout and has_max_solve_time:
+            self._solver.max_solve_time = float("inf")
+        res = self._solver.solve(x_warmstart, u_warmstart, max_iters)
+        if (
+            use_iteration_limits_and_timeout
+            and has_max_solve_time
+            and self._ocp_params.max_solve_time is not None
+        ):
+            self._solver.max_solve_time = self._ocp_params.max_solve_time
path: agimus_controller/agimus_controller/ocp_base_croco.py
user: login: TheoMF
body: ```suggestion
        if not use_iteration_limits_and_timeout and has_max_solve_time:
            self._solver.max_solve_time = float("inf")
        elif (
            use_iteration_limits_and_timeout
            and has_max_solve_time
            and self._ocp_params.max_solve_time is not None 
        ):
            self._solver.max_solve_time = self._ocp_params.max_solve_time     
        res = self._solver.solve(x_warmstart, u_warmstart, max_iters)
```
created_at: 2025-03-13T09:22:51Z
author_association: COLLABORATOR
diff_hunk: @@ -132,6 +146,21 @@ def solve(
             self._debug_data.kkt_norm = self._solver.KKT
             self._debug_data.nb_iter = int(self._solver.iter)
             self._debug_data.nb_qp_iter = int(self._solver.qp_iters)
+            if len(self._debug_data.residuals) == 0:
+                names = (
+                    self._problem.runningModels[0]
+                    .differential.costs.costs.todict()
+                    .keys()
+                )
path: agimus_controller/agimus_controller/ocp_base_croco.py
user: login: TheoMF
body: I'm not sure about that, because this way we would add residual for state, control, and gripper pose, which we don't need because we already publish the references and the predictions for them
created_at: 2025-03-13T09:26:03Z
author_association: COLLABORATOR
diff_hunk: @@ -132,6 +146,21 @@ def solve(
             self._debug_data.kkt_norm = self._solver.KKT
             self._debug_data.nb_iter = int(self._solver.iter)
             self._debug_data.nb_qp_iter = int(self._solver.qp_iters)
+            if len(self._debug_data.residuals) == 0:
+                names = (
+                    self._problem.runningModels[0]
+                    .differential.costs.costs.todict()
+                    .keys()
+                )
path: agimus_controller/agimus_controller/ocp_base_croco.py
user: login: jmirabel
body: The question is:
Do you think it is problematic to include them?
created_at: 2025-03-13T09:42:34Z
author_association: CONTRIBUTOR
diff_hunk: @@ -118,9 +121,20 @@ def solve(
         # Set the initial state
         self._problem.x0 = x0
         # Solve the OCP
-        res = self._solver.solve(
-            x_warmstart, u_warmstart, self._ocp_params.solver_iters
+        max_iters = (
+            self._ocp_params.solver_iters if use_iteration_limits_and_timeout else 1000
         )
+        # If mim_solvers is at the latest state
+        has_max_solve_time = hasattr(self._solver, "max_solve_time")
+        if use_iteration_limits_and_timeout and has_max_solve_time:
+            self._solver.max_solve_time = float("inf")
+        res = self._solver.solve(x_warmstart, u_warmstart, max_iters)
+        if (
+            use_iteration_limits_and_timeout
+            and has_max_solve_time
+            and self._ocp_params.max_solve_time is not None
+        ):
+            self._solver.max_solve_time = self._ocp_params.max_solve_time
path: agimus_controller/agimus_controller/ocp_base_croco.py
user: login: jmirabel
body: Indeed I made a mistake here.
created_at: 2025-03-13T09:43:19Z
author_association: CONTRIBUTOR
diff_hunk: @@ -118,9 +121,20 @@ def solve(
         # Set the initial state
         self._problem.x0 = x0
         # Solve the OCP
-        res = self._solver.solve(
-            x_warmstart, u_warmstart, self._ocp_params.solver_iters
+        max_iters = (
+            self._ocp_params.solver_iters if use_iteration_limits_and_timeout else 1000
         )
+        # If mim_solvers is at the latest state
+        has_max_solve_time = hasattr(self._solver, "max_solve_time")
+        if use_iteration_limits_and_timeout and has_max_solve_time:
+            self._solver.max_solve_time = float("inf")
+        res = self._solver.solve(x_warmstart, u_warmstart, max_iters)
+        if (
+            use_iteration_limits_and_timeout
+            and has_max_solve_time
+            and self._ocp_params.max_solve_time is not None
+        ):
+            self._solver.max_solve_time = self._ocp_params.max_solve_time
path: agimus_controller/agimus_controller/ocp_base_croco.py
user: login: jmirabel
body: I simplified this portion of code.
created_at: 2025-03-13T09:50:20Z
author_association: CONTRIBUTOR
diff_hunk: @@ -132,6 +146,21 @@ def solve(
             self._debug_data.kkt_norm = self._solver.KKT
             self._debug_data.nb_iter = int(self._solver.iter)
             self._debug_data.nb_qp_iter = int(self._solver.qp_iters)
+            if len(self._debug_data.residuals) == 0:
+                names = (
+                    self._problem.runningModels[0]
+                    .differential.costs.costs.todict()
+                    .keys()
+                )
path: agimus_controller/agimus_controller/ocp_base_croco.py
user: login: TheoMF
body: I think we should avoid that because we're losing time in publishing these values
created_at: 2025-03-13T09:52:07Z
author_association: COLLABORATOR
diff_hunk: @@ -132,6 +146,21 @@ def solve(
             self._debug_data.kkt_norm = self._solver.KKT
             self._debug_data.nb_iter = int(self._solver.iter)
             self._debug_data.nb_qp_iter = int(self._solver.qp_iters)
+            if len(self._debug_data.residuals) == 0:
+                names = (
+                    self._problem.runningModels[0]
+                    .differential.costs.costs.todict()
+                    .keys()
+                )
path: agimus_controller/agimus_controller/ocp_base_croco.py
user: login: jmirabel
body: 1. It is debug data, so loosing time isn't that dramatic.
2. Are you sure we loose a significant amount of time?
created_at: 2025-03-13T09:52:58Z
author_association: CONTRIBUTOR
diff_hunk: @@ -132,6 +146,21 @@ def solve(
             self._debug_data.kkt_norm = self._solver.KKT
             self._debug_data.nb_iter = int(self._solver.iter)
             self._debug_data.nb_qp_iter = int(self._solver.qp_iters)
+            if len(self._debug_data.residuals) == 0:
+                names = (
+                    self._problem.runningModels[0]
+                    .differential.costs.costs.todict()
+                    .keys()
+                )
path: agimus_controller/agimus_controller/ocp_base_croco.py
user: login: jmirabel
body: Do you have any suggestions?
If not, I would suggest not to block this PR.
created_at: 2025-03-13T09:54:09Z
author_association: CONTRIBUTOR
diff_hunk: @@ -132,6 +146,21 @@ def solve(
             self._debug_data.kkt_norm = self._solver.KKT
             self._debug_data.nb_iter = int(self._solver.iter)
             self._debug_data.nb_qp_iter = int(self._solver.qp_iters)
+            if len(self._debug_data.residuals) == 0:
+                names = (
+                    self._problem.runningModels[0]
+                    .differential.costs.costs.todict()
+                    .keys()
+                )
path: agimus_controller/agimus_controller/ocp_base_croco.py
user: login: TheoMF
body: the residuals will have the size of predictions so I think so, I remember on my laptop that not publishing the predictions made a gain of something like 1ms in ROS1
created_at: 2025-03-13T09:58:48Z
author_association: COLLABORATOR
diff_hunk: @@ -132,6 +146,21 @@ def solve(
             self._debug_data.kkt_norm = self._solver.KKT
             self._debug_data.nb_iter = int(self._solver.iter)
             self._debug_data.nb_qp_iter = int(self._solver.qp_iters)
+            if len(self._debug_data.residuals) == 0:
+                names = (
+                    self._problem.runningModels[0]
+                    .differential.costs.costs.todict()
+                    .keys()
+                )
path: agimus_controller/agimus_controller/ocp_base_croco.py
user: login: jmirabel
body: Maybe we should just send no residual at all then. All of them can be reconstructed from the state and control predictions.
created_at: 2025-03-13T10:02:22Z
author_association: CONTRIBUTOR
diff_hunk: @@ -132,6 +146,21 @@ def solve(
             self._debug_data.kkt_norm = self._solver.KKT
             self._debug_data.nb_iter = int(self._solver.iter)
             self._debug_data.nb_qp_iter = int(self._solver.qp_iters)
+            if len(self._debug_data.residuals) == 0:
+                names = (
+                    self._problem.runningModels[0]
+                    .differential.costs.costs.todict()
+                    .keys()
+                )
path: agimus_controller/agimus_controller/ocp_base_croco.py
user: login: jmirabel
body: This PR is needed to fix humble-devel as the messages have been changed.
I'll comment out this section of the code so has no residual is published. We can move discussion on the residual to another task.
Is this fine for you?
created_at: 2025-03-13T10:03:33Z
author_association: CONTRIBUTOR
diff_hunk: @@ -132,6 +146,21 @@ def solve(
             self._debug_data.kkt_norm = self._solver.KKT
             self._debug_data.nb_iter = int(self._solver.iter)
             self._debug_data.nb_qp_iter = int(self._solver.qp_iters)
+            if len(self._debug_data.residuals) == 0:
+                names = (
+                    self._problem.runningModels[0]
+                    .differential.costs.costs.todict()
+                    .keys()
+                )
path: agimus_controller/agimus_controller/ocp_base_croco.py
user: login: jmirabel
body: Created #202 
created_at: 2025-03-13T10:10:05Z
author_association: CONTRIBUTOR

PR comments:
user: login: jmirabel
created_at: 2025-03-12T15:38:41Z
body: @TheoMF this corresponds more or less to what we discussed earlier today.
author_association: CONTRIBUTOR
user: login: jmirabel
created_at: 2025-03-12T15:41:42Z
body: I keep for a later work the exposure of `max_solve_time` as a ROS parameter.
author_association: CONTRIBUTOR