From 2b74b106e8e468e5e73ad2d9e1d4e940d0810906 Mon Sep 17 00:00:00 2001
From: Arthur <arthur.haffemayer@yahoo.fr>
Date: Wed, 11 Dec 2024 14:09:04 +0000
Subject: [PATCH 01/95] creating the ocp param base class & the ocp base croco

---
 .../agimus_controller/ocp_base_croco.py       | 45 +++++++++++++++++++
 .../agimus_controller/ocp_param_base.py       | 14 ++++++
 2 files changed, 59 insertions(+)
 create mode 100644 agimus_controller/agimus_controller/ocp_base_croco.py
 create mode 100644 agimus_controller/agimus_controller/ocp_param_base.py

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
new file mode 100644
index 00000000..78a23001
--- /dev/null
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -0,0 +1,45 @@
+from abc import ABC, abstractmethod
+
+import numpy as np
+import pinocchio as pin
+import crocoddyl
+
+from agimus_controller.mpc_data import OCPResults, OCPDebugData
+from agimus_controller.trajectory import WeightedTrajectoryPoint
+from agimus_controller.ocp_base import OCPBase
+
+class OCPCrocoBase(OCPBase):
+    def __init__(self, rmodel) -> None:
+        pass
+
+    @abstractmethod
+    def set_reference_horizon(
+        self, reference_trajectory: list[WeightedTrajectoryPoint]
+    ) -> None:
+        ...
+
+    @abstractmethod
+    @property
+    def horizon_size() -> int:
+        ...
+
+    @abstractmethod
+    @property
+    def dt() -> int:
+        ...
+
+    @abstractmethod
+    def solve(
+        self, x0: np.ndarray, x_init: list[np.ndarray], u_init: list[np.ndarray]
+    ) -> None:
+        ...
+
+    @abstractmethod
+    @property
+    def ocp_results(self) -> OCPResults:
+        ...
+
+    @abstractmethod
+    @property
+    def debug_data(self) -> OCPDebugData:
+        ...
diff --git a/agimus_controller/agimus_controller/ocp_param_base.py b/agimus_controller/agimus_controller/ocp_param_base.py
new file mode 100644
index 00000000..6b340ce4
--- /dev/null
+++ b/agimus_controller/agimus_controller/ocp_param_base.py
@@ -0,0 +1,14 @@
+from dataclasses import dataclass
+import numpy as np
+import numpy.typing as npt
+
+@dataclass
+class OCPParamsBase:
+    """Input data structure of the OCP."""
+
+    dt: float # Integration step of the OCP
+    T: int # Number of time steps in the horizon
+    
+    X0: npt.NDArray[np.float64] # Initial state
+    
+

From 60648fc84f4a71b5405cd3b5eddba8e2b3e01174 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Wed, 11 Dec 2024 14:59:38 +0000
Subject: [PATCH 02/95] populate ocp param & croco

---
 .../agimus_controller/ocp_base.py             |  2 +-
 .../agimus_controller/ocp_base_croco.py       | 81 +++++++++++++++----
 .../agimus_controller/ocp_param_base.py       | 16 +++-
 3 files changed, 80 insertions(+), 19 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base.py b/agimus_controller/agimus_controller/ocp_base.py
index a0475b46..806934f6 100644
--- a/agimus_controller/agimus_controller/ocp_base.py
+++ b/agimus_controller/agimus_controller/ocp_base.py
@@ -27,7 +27,7 @@ def dt() -> int:
 
     @abstractmethod
     def solve(
-        self, x0: np.ndarray, x_init: list[np.ndarray], u_init: list[np.ndarray]
+        self, x_init: list[np.ndarray], u_init: list[np.ndarray]
     ) -> None:
         ...
 
diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 78a23001..f040f009 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -1,5 +1,5 @@
 from abc import ABC, abstractmethod
-
+from typing import List
 import numpy as np
 import pinocchio as pin
 import crocoddyl
@@ -7,32 +7,83 @@
 from agimus_controller.mpc_data import OCPResults, OCPDebugData
 from agimus_controller.trajectory import WeightedTrajectoryPoint
 from agimus_controller.ocp_base import OCPBase
+from agimus_controller.OCPCrocoBase import OCPCrocoBase
 
 class OCPCrocoBase(OCPBase):
-    def __init__(self, rmodel) -> None:
-        pass
-
-    @abstractmethod
-    def set_reference_horizon(
-        self, reference_trajectory: list[WeightedTrajectoryPoint]
-    ) -> None:
-        ...
+    def __init__(self, rmodel: pin.Model, cmodel: pin.GeometryModel, OCPParams: OCPParamsBase) -> None:
+        """Creates an instance of the OCPCrocoBase class. This is an example of an OCP class that inherits from OCPBase, 
+        for a simple goal reaching task. The class is used to solve the Optimal Control Problem (OCP) using the Crocoddyl library.
 
+        Args:
+            rmodel (pin.Model): Robot model.
+            cmodel (pin.GeometryModel): Collision Model of the robot.
+            OCPParams (OCPParamsBase): Input data structure of the OCP.
+        """
+        self._rmodel = rmodel
+        self._cmodel = cmodel
+        self._OCPParams = OCPParams
+        
+        
     @abstractmethod
     @property
     def horizon_size() -> int:
-        ...
+        """Number of time steps in the horizon."""
+        return self._OCPParams.T
 
     @abstractmethod
     @property
-    def dt() -> int:
-        ...
+    def dt() -> float64:
+        """Integration step of the OCP."""
+        return self._OCPParams.dt
+    
+    @abstractmethod
+    @property
+    def x0() -> np.ndarray:
+        """Initial state of the robot."""
+        return self._OCPParams.WeightedTrajectoryPoints[0].point.robot_configuration
 
     @abstractmethod
     def solve(
-        self, x0: np.ndarray, x_init: list[np.ndarray], u_init: list[np.ndarray]
-    ) -> None:
-        ...
+        self, x_init: List[np.ndarray], u_init: List[np.ndarray]
+    ) -> bool:
+        """Solves the OCP. Returns True if the OCP was solved successfully, False otherwise.
+
+        Args:
+            x_init (List[np.ndarray]): List of the states for the initial trajectory.
+            u_init (List[np.ndarray]): List of the controls for the initial trajectory.
+
+        Returns:
+            bool: True if the OCP was solved successfully, False otherwise.
+        """
+        ### Creation of the state and actuation models        
+        # Stat and actuation model
+        self._state = crocoddyl.StateMultibody(self._rmodel)
+        self._actuation = crocoddyl.ActuationModelFull(self._state)
+
+        # Running & terminal cost models
+        self._runningCostModel = crocoddyl.CostModelSum(self._state)
+        self._terminalCostModel = crocoddyl.CostModelSum(self._state)
+
+        ### Creation of cost terms
+        # State Regularization cost
+        xResidual = crocoddyl.ResidualModelState(self._state, self.x0)
+        xRegCost = crocoddyl.CostModelResidual(self._state, xResidual)
+
+        # Control Regularization cost
+        uResidual = crocoddyl.ResidualModelControl(self._state)
+        uRegCost = crocoddyl.CostModelResidual(self._state, uResidual)
+
+        # End effector frame cost
+        frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
+            self._state,
+            self._OCPParams.ee_name,
+            self._OCPParams.p_target,
+        )
+
+        goalTrackingCost = crocoddyl.CostModelResidual(
+            self._state, frameTranslationResidual
+        )
+        
 
     @abstractmethod
     @property
diff --git a/agimus_controller/agimus_controller/ocp_param_base.py b/agimus_controller/agimus_controller/ocp_param_base.py
index 6b340ce4..17dc9f65 100644
--- a/agimus_controller/agimus_controller/ocp_param_base.py
+++ b/agimus_controller/agimus_controller/ocp_param_base.py
@@ -1,14 +1,24 @@
 from dataclasses import dataclass
+from typing import List
+
 import numpy as np
 import numpy.typing as npt
 
+from agimus_controller.trajectory import WeightedTrajectoryPoint
+
 @dataclass
 class OCPParamsBase:
     """Input data structure of the OCP."""
 
+    # Data relevant to solve the OCP
     dt: float # Integration step of the OCP
     T: int # Number of time steps in the horizon
+    qp_iters: int # Number of QP iterations
+    solver_iters: int # Number of solver iterations
     
-    X0: npt.NDArray[np.float64] # Initial state
-    
-
+    # Weights, costs & helpers for the costs & constraints
+    WeightedTrajectoryPoints: List[WeightedTrajectoryPoint] # List of weighted trajectory points    
+    armature: npt.ArrayLike # Armature of the robot
+    ee_name: str # Name of the end-effector
+    p_target: npt.ArrayLike # Target position of the end-effector in R3
+        

From 3d36d168739e9e0990db883a72d693cd593e40d1 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Wed, 11 Dec 2024 15:54:04 +0000
Subject: [PATCH 03/95] setting up the params base class

---
 .../agimus_controller/ocp_param_base.py            | 14 +++++++++-----
 1 file changed, 9 insertions(+), 5 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_param_base.py b/agimus_controller/agimus_controller/ocp_param_base.py
index 17dc9f65..7929e83c 100644
--- a/agimus_controller/agimus_controller/ocp_param_base.py
+++ b/agimus_controller/agimus_controller/ocp_param_base.py
@@ -7,18 +7,22 @@
 from agimus_controller.trajectory import WeightedTrajectoryPoint
 
 @dataclass
-class OCPParamsBase:
+class OCPParamsCrocoBase:
     """Input data structure of the OCP."""
 
     # Data relevant to solve the OCP
-    dt: float # Integration step of the OCP
+    dt: np.float64 # Integration step of the OCP
     T: int # Number of time steps in the horizon
-    qp_iters: int # Number of QP iterations
+    qp_iters: int = 200 # Number of QP iterations (must be a multiple of 25).
     solver_iters: int # Number of solver iterations
+    termination_tolerance: np.float64 = 1e-3 # Termination tolerance (norm of the KKT conditions)
+    eps_abs: np.float64 = 1e-6 # Absolute tolerance of the solver
+    eps_rel: np.float64 = 0 # Relative tolerance of the solver
+    callbacks: bool = False # Flag to enable/disable callbacks
     
     # Weights, costs & helpers for the costs & constraints
     WeightedTrajectoryPoints: List[WeightedTrajectoryPoint] # List of weighted trajectory points    
-    armature: npt.ArrayLike # Armature of the robot
+    armature: npt.NDArray[np.float64] # Armature of the robot
     ee_name: str # Name of the end-effector
-    p_target: npt.ArrayLike # Target position of the end-effector in R3
+    p_target: npt.NDArray[np.float64] # Target position of the end-effector in R3
         

From 0820ea7ff8f4cd52e00b1c33d43a6c289af821d1 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Wed, 11 Dec 2024 17:18:36 +0000
Subject: [PATCH 04/95] add test for TestOcpBase

---
 agimus_controller/tests/test_ocp_base.py | 9 +++++++++
 1 file changed, 9 insertions(+)
 create mode 100644 agimus_controller/tests/test_ocp_base.py

diff --git a/agimus_controller/tests/test_ocp_base.py b/agimus_controller/tests/test_ocp_base.py
new file mode 100644
index 00000000..f5cc97df
--- /dev/null
+++ b/agimus_controller/tests/test_ocp_base.py
@@ -0,0 +1,9 @@
+import unittest
+
+from agimus_controller.ocp_base import OCPBase
+
+
+class TestOCPBase(unittest.TestCase):
+    def test_abstract_class_instantiation(self):
+        with self.assertRaises(TypeError):
+            OCPBase()
\ No newline at end of file

From 16fbafe3758198dac413aa22c7585bf7ac0b9efe Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Wed, 11 Dec 2024 17:26:20 +0000
Subject: [PATCH 05/95] change all relative paths to absolute ones

---
 .../agimus_controller/mpc_data.py             |   2 +-
 .../agimus_controller/ocp_base.py             |  26 +--
 .../agimus_controller/ocp_base_croco.py       | 152 ++++++++++++++----
 .../agimus_controller/ocp_param_base.py       |   2 +-
 .../agimus_controller/trajectory.py           |   2 +-
 agimus_controller/tests/test_ocp_base.py      |   2 +-
 6 files changed, 138 insertions(+), 48 deletions(-)

diff --git a/agimus_controller/agimus_controller/mpc_data.py b/agimus_controller/agimus_controller/mpc_data.py
index a01e0a94..19d0c25d 100644
--- a/agimus_controller/agimus_controller/mpc_data.py
+++ b/agimus_controller/agimus_controller/mpc_data.py
@@ -2,7 +2,7 @@
 import numpy as np
 import numpy.typing as npt
 
-from agimus_controller.trajectory import TrajectoryPoint
+from agimus_controller.agimus_controller.trajectory import TrajectoryPoint
 
 
 @dataclass
diff --git a/agimus_controller/agimus_controller/ocp_base.py b/agimus_controller/agimus_controller/ocp_base.py
index 806934f6..a4a87438 100644
--- a/agimus_controller/agimus_controller/ocp_base.py
+++ b/agimus_controller/agimus_controller/ocp_base.py
@@ -1,8 +1,11 @@
 from abc import ABC, abstractmethod
+from typing import List
+
 import numpy as np
+import numpy.typing as npt
 
-from agimus_controller.mpc_data import OCPResults, OCPDebugData
-from agimus_controller.trajectory import WeightedTrajectoryPoint
+from agimus_controller.agimus_controller.mpc_data import OCPResults, OCPDebugData
+from agimus_controller.agimus_controller.trajectory import WeightedTrajectoryPoint
 
 
 class OCPBase(ABC):
@@ -13,30 +16,35 @@ def __init__(self) -> None:
     def set_reference_horizon(
         self, reference_trajectory: list[WeightedTrajectoryPoint]
     ) -> None:
-        ...
+        pass
 
     @abstractmethod
     @property
     def horizon_size() -> int:
-        ...
+        pass
 
     @abstractmethod
     @property
     def dt() -> int:
-        ...
+        pass
+    
+    @abstractmethod
+    @property
+    def x0() -> npt.NDArray[np.float64]:
+        pass
 
     @abstractmethod
     def solve(
-        self, x_init: list[np.ndarray], u_init: list[np.ndarray]
+        self, x_init: List[npt.NDArray[np.float64]], u_init: List[npt.NDArray[np.float64]]
     ) -> None:
-        ...
+        pass
 
     @abstractmethod
     @property
     def ocp_results(self) -> OCPResults:
-        ...
+        pass
 
     @abstractmethod
     @property
     def debug_data(self) -> OCPDebugData:
-        ...
+        pass
diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index f040f009..a3140d25 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -1,16 +1,18 @@
 from abc import ABC, abstractmethod
 from typing import List
+
+import crocoddyl
+import mim_solvers
 import numpy as np
 import pinocchio as pin
-import crocoddyl
 
-from agimus_controller.mpc_data import OCPResults, OCPDebugData
-from agimus_controller.trajectory import WeightedTrajectoryPoint
-from agimus_controller.ocp_base import OCPBase
-from agimus_controller.OCPCrocoBase import OCPCrocoBase
+from agimus_controller.agimus_controller.mpc_data import OCPResults, OCPDebugData
+from agimus_controller.agimus_controller.trajectory import WeightedTrajectoryPoint
+from agimus_controller.agimus_controller.ocp_base import OCPBase
+from agimus_controller.agimus_controller.ocp_param_base import OCPParamsCrocoBase
 
 class OCPCrocoBase(OCPBase):
-    def __init__(self, rmodel: pin.Model, cmodel: pin.GeometryModel, OCPParams: OCPParamsBase) -> None:
+    def __init__(self, rmodel: pin.Model, cmodel: pin.GeometryModel, OCPParams: OCPParamsCrocoBase) -> None:
         """Creates an instance of the OCPCrocoBase class. This is an example of an OCP class that inherits from OCPBase, 
         for a simple goal reaching task. The class is used to solve the Optimal Control Problem (OCP) using the Crocoddyl library.
 
@@ -26,70 +28,150 @@ def __init__(self, rmodel: pin.Model, cmodel: pin.GeometryModel, OCPParams: OCPP
         
     @abstractmethod
     @property
-    def horizon_size() -> int:
+    def horizon_size(self) -> int:
         """Number of time steps in the horizon."""
         return self._OCPParams.T
 
     @abstractmethod
     @property
-    def dt() -> float64:
+    def dt(self) -> np.float64:
         """Integration step of the OCP."""
         return self._OCPParams.dt
     
     @abstractmethod
     @property
-    def x0() -> np.ndarray:
+    def x0(self) -> np.ndarray:
         """Initial state of the robot."""
         return self._OCPParams.WeightedTrajectoryPoints[0].point.robot_configuration
 
+    @abstractmethod
+    def x_init(self) -> np.ndarray:
+        """Initial guess of the states."""
+        x_init = []
+        for TrajectoryPoint in self._OCPParams.WeightedTrajectoryPoints:
+            x = np.array(TrajectoryPoint.point.robot_configuration + TrajectoryPoint.point.robot_velocity)
+            x_init.append(x)
+        return x_init
+    
+    @abstractmethod
+    def u_init(self) -> np.ndarray:
+        """Initial guess of the controls."""
+        u_init = [np.zeros(self._rmodel.nq) for _ in range(self.horizon_size) - 1] # For the panda nu = nq, but i'm not sure nu exists in pinocchio
+        return u_init
+    
     @abstractmethod
     def solve(
-        self, x_init: List[np.ndarray], u_init: List[np.ndarray]
+        self, 
     ) -> bool:
         """Solves the OCP. Returns True if the OCP was solved successfully, False otherwise.
 
-        Args:
-            x_init (List[np.ndarray]): List of the states for the initial trajectory.
-            u_init (List[np.ndarray]): List of the controls for the initial trajectory.
-
         Returns:
             bool: True if the OCP was solved successfully, False otherwise.
         """
-        ### Creation of the state and actuation models        
+        ### Creation of the state and actuation models
+        
         # Stat and actuation model
         self._state = crocoddyl.StateMultibody(self._rmodel)
         self._actuation = crocoddyl.ActuationModelFull(self._state)
 
+        self._runningModelList = []
         # Running & terminal cost models
-        self._runningCostModel = crocoddyl.CostModelSum(self._state)
-        self._terminalCostModel = crocoddyl.CostModelSum(self._state)
-
-        ### Creation of cost terms
-        # State Regularization cost
-        xResidual = crocoddyl.ResidualModelState(self._state, self.x0)
-        xRegCost = crocoddyl.CostModelResidual(self._state, xResidual)
-
-        # Control Regularization cost
-        uResidual = crocoddyl.ResidualModelControl(self._state)
-        uRegCost = crocoddyl.CostModelResidual(self._state, uResidual)
-
-        # End effector frame cost
-        frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
-            self._state,
-            self._OCPParams.ee_name,
-            self._OCPParams.p_target,
+        for t in range(self.horizon_size):
+            ### Creation of cost terms
+            # State Regularization cost
+            xResidual = crocoddyl.ResidualModelState(self._state, self.x0)
+            xRegCost = crocoddyl.CostModelResidual(self._state, xResidual)
+
+            # Control Regularization cost
+            uResidual = crocoddyl.ResidualModelControl(self._state)
+            uRegCost = crocoddyl.CostModelResidual(self._state, uResidual)
+
+            # End effector frame cost
+            frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
+                self._state,
+                self._OCPParams.ee_name,
+                self._OCPParams.p_target,
+            )
+
+            goalTrackingCost = crocoddyl.CostModelResidual(
+                self._state, frameTranslationResidual
+            )
+
+            # Adding costs to the models
+            if t < self.horizon_size - 1:
+                runningCostModel = crocoddyl.CostModelSum(self._state)
+                runningCostModel.addCost("stateReg", xRegCost, np.concatenate([self._OCPParams.WeightedTrajectoryPoints[t].weight.w_robot_configuration, self._OCPParams.WeightedTrajectoryPoints[t].weight.w_robot_velocity]))
+                runningCostModel.addCost("ctrlRegGrav", uRegCost, self._OCPParams.WeightedTrajectoryPoints[t].weight.w_robot_effort)
+                runningCostModel.addCost(
+                    "gripperPoseRM", goalTrackingCost, self._OCPParams.WeightedTrajectoryPoints[t].weight.w_end_effector_poses
+                )
+                # Create Differential Action Model (DAM), i.e. continuous dynamics and cost functions
+                self._running_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics(
+                    self._state,
+                    self._actuation,
+                    runningCostModel,
+                )
+                # Create Integrated Action Model (IAM), i.e. Euler integration of continuous dynamics and cost
+                runningModel = crocoddyl.IntegratedActionModelEuler(
+                    self._running_DAM, self.dt
+                )
+                runningModel.differential.armature = OCPParamsCrocoBase.armature
+                self._runningModelList.append(runningModel)
+            else:
+                terminalCostModel = crocoddyl.CostModelSum(self._state)
+                terminalCostModel.addCost("stateReg", xRegCost, np.concatenate([self._OCPParams.WeightedTrajectoryPoints[t].weight.w_robot_configuration, self._OCPParams.WeightedTrajectoryPoints[t].weight.w_robot_velocity]))
+                terminalCostModel.addCost(
+                    "gripperPose", goalTrackingCost, self._OCPParams.WeightedTrajectoryPoints[t].weight.w_end_effector_poses
+                )
+
+                self._terminal_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics(
+                    self._state,
+                    self._actuation,
+                    terminalCostModel
+                    )
+
+                self._terminalModel = crocoddyl.IntegratedActionModelEuler(
+                    self._terminal_DAM, 0.0
+                )
+                self._terminalModel.differential.armature = OCPParamsCrocoBase.armature
+
+        problem = crocoddyl.ShootingProblem(
+            self.x0, self._runningModelList, self._terminalModel
         )
+        # Create solver + callbacks
+        ocp = mim_solvers.SolverSQP(problem)
+
+        # Merit function
+        ocp.use_filter_line_search = False
+
+        # Parameters of the solver
+        ocp.termination_tolerance = OCPParamsCrocoBase.termination_tolerance
+        ocp.max_qp_iters = OCPParamsCrocoBase.qp_iters
+        ocp.eps_abs = OCPParamsCrocoBase.eps_abs
+        ocp.eps_rel = OCPParamsCrocoBase.eps_rel
 
-        goalTrackingCost = crocoddyl.CostModelResidual(
-            self._state, frameTranslationResidual
+        ocp.with_callbacks = OCPParamsCrocoBase.callbacks
+
+        result = ocp.solve(self.x_init, self.u_init, OCPParamsCrocoBase.solver_iters) 
+        
+        self.ocp_results = OCPResults(
+            states=ocp.xs,
+            ricatti_gains=ocp.Ks, # Not sure about this one
+            feed_forward_terms=ocp.us,
         )
         
+        return result
 
     @abstractmethod
     @property
     def ocp_results(self) -> OCPResults:
-        ...
+        """Output data structure of the OCP.
 
+        Returns:
+            OCPResults: Output data structure of the OCP. It contains the states, Ricatti gains and feed-forward terms.
+        """
+        return self._ocp_results
+        
     @abstractmethod
     @property
     def debug_data(self) -> OCPDebugData:
diff --git a/agimus_controller/agimus_controller/ocp_param_base.py b/agimus_controller/agimus_controller/ocp_param_base.py
index 7929e83c..db665383 100644
--- a/agimus_controller/agimus_controller/ocp_param_base.py
+++ b/agimus_controller/agimus_controller/ocp_param_base.py
@@ -4,7 +4,7 @@
 import numpy as np
 import numpy.typing as npt
 
-from agimus_controller.trajectory import WeightedTrajectoryPoint
+from agimus_controller.agimus_controller.trajectory import WeightedTrajectoryPoint
 
 @dataclass
 class OCPParamsCrocoBase:
diff --git a/agimus_controller/agimus_controller/trajectory.py b/agimus_controller/agimus_controller/trajectory.py
index 10332ad3..44e2bd43 100644
--- a/agimus_controller/agimus_controller/trajectory.py
+++ b/agimus_controller/agimus_controller/trajectory.py
@@ -12,7 +12,7 @@ class TrajectoryPoint:
     robot_configuration: np.ndarray
     robot_velocity: np.ndarray
     robot_acceleration: np.ndarray
-    robot_effort: np.ndarray
+    robot_effort: np.ndarray # Is robot effort the same as robot torque?
     forces: dict[Force]  # Dictionary of pinocchio.Force
     end_effector_poses: dict[SE3]  # Dictionary of pinocchio.SE3
 
diff --git a/agimus_controller/tests/test_ocp_base.py b/agimus_controller/tests/test_ocp_base.py
index f5cc97df..f114a5bb 100644
--- a/agimus_controller/tests/test_ocp_base.py
+++ b/agimus_controller/tests/test_ocp_base.py
@@ -1,6 +1,6 @@
 import unittest
 
-from agimus_controller.ocp_base import OCPBase
+from agimus_controller.agimus_controller.ocp_base import OCPBase
 
 
 class TestOCPBase(unittest.TestCase):

From 6ff4507d2f5a8b5343840192f658c2dae61858b1 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Wed, 11 Dec 2024 17:37:56 +0000
Subject: [PATCH 06/95] unit tests of the data class ocp param croco base

---
 .../tests/test_ocp_croco_base.py              | 32 +++++++++++
 .../tests/test_ocp_param_base.py              | 56 +++++++++++++++++++
 2 files changed, 88 insertions(+)
 create mode 100644 agimus_controller/tests/test_ocp_croco_base.py
 create mode 100644 agimus_controller/tests/test_ocp_param_base.py

diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
new file mode 100644
index 00000000..06bc0d55
--- /dev/null
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -0,0 +1,32 @@
+import unittest
+
+import example_robot_data as robex
+
+from agimus_controller.agimus_controller.ocp_base_croco import OCPCrocoBase
+from agimus_controller.agimus_controller.ocp_param_base import OCPParamsCrocoBase
+
+
+class TestOCPBase(unittest.TestCase):
+    
+    def setUp(self):
+        
+        # Load the robot
+        self.robot = robex.load('example_robot_description')
+        self.rmodel = self.robot.model
+        self.cmodel = self.robot.collision_model
+        
+        # Generate some fixed params for unit testing
+        self.OCPParams = OCPParamsCrocoBase(
+            dt = 0.01,
+            T = 100,
+            qp_iters=200,
+            solver_iters=200,
+            callbacks=True,
+            
+        )
+        return super().setUp()
+    
+    def test_abstract_class_instantiation(self):
+        with self.assertRaises(TypeError):
+            
+            OCPCrocoBase()
\ No newline at end of file
diff --git a/agimus_controller/tests/test_ocp_param_base.py b/agimus_controller/tests/test_ocp_param_base.py
new file mode 100644
index 00000000..a86fc3f8
--- /dev/null
+++ b/agimus_controller/tests/test_ocp_param_base.py
@@ -0,0 +1,56 @@
+import unittest
+import numpy as np
+
+from agimus_controller.agimus_controller.ocp_param_base import OCPParamsCrocoBase
+from agimus_controller.agimus_controller.trajectory import WeightedTrajectoryPoint, TrajectoryPoint, TrajectoryPointWeights
+
+class TestOCPParamsCrocoBase(unittest.TestCase):
+    """Test the OCPParamsCrocoBase class."""
+    
+    def __init__(self, methodName = "runTest"):
+        super().__init__(methodName)
+        
+    def test_initialization(self):
+        dt = np.float64(0.01)
+        T = 100
+        solver_iters = 50
+        point = TrajectoryPoint(
+            robot_configuration=np.array([1.0, 2.0, 3.0], dtype=np.float64),
+        )
+        weights = TrajectoryPointWeights(
+            w_robot_configuration=np.array([1.0, 2.0, 3.0], dtype=np.float64),
+        )
+        weighted_trajectory_point = WeightedTrajectoryPoint(
+            point, 
+            weights
+        )
+        weighted_trajectory_points = [weighted_trajectory_point]
+        armature = np.array([1.0, 2.0, 3.0], dtype=np.float64)
+        ee_name = "end_effector"
+        p_target = np.array([0.0, 0.0, 0.0], dtype=np.float64)
+
+        params = OCPParamsCrocoBase(
+            dt=dt,
+            T=T,
+            solver_iters=solver_iters,
+            WeightedTrajectoryPoints=weighted_trajectory_points,
+            armature=armature,
+            ee_name=ee_name,
+            p_target=p_target
+        )
+
+        self.assertEqual(params.dt, dt)
+        self.assertEqual(params.T, T)
+        self.assertEqual(params.qp_iters, 200)
+        self.assertEqual(params.solver_iters, solver_iters)
+        self.assertEqual(params.termination_tolerance, 1e-3)
+        self.assertEqual(params.eps_abs, 1e-6)
+        self.assertEqual(params.eps_rel, 0)
+        self.assertFalse(params.callbacks)
+        self.assertEqual(params.WeightedTrajectoryPoints, weighted_trajectory_points)
+        self.assertTrue(np.array_equal(params.armature, armature))
+        self.assertEqual(params.ee_name, ee_name)
+        self.assertTrue(np.array_equal(params.p_target, p_target))
+
+if __name__ == '__main__':
+    unittest.main()
\ No newline at end of file

From 1da77795648cc456e9412be5387fa25d27e13037 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Wed, 11 Dec 2024 17:43:26 +0000
Subject: [PATCH 07/95] first try on testing the ocp croco base class

---
 agimus_controller/tests/test_ocp_base.py      |  5 +-
 .../tests/test_ocp_croco_base.py              | 63 ++++++++++++++++---
 2 files changed, 57 insertions(+), 11 deletions(-)

diff --git a/agimus_controller/tests/test_ocp_base.py b/agimus_controller/tests/test_ocp_base.py
index f114a5bb..ef8678a2 100644
--- a/agimus_controller/tests/test_ocp_base.py
+++ b/agimus_controller/tests/test_ocp_base.py
@@ -6,4 +6,7 @@
 class TestOCPBase(unittest.TestCase):
     def test_abstract_class_instantiation(self):
         with self.assertRaises(TypeError):
-            OCPBase()
\ No newline at end of file
+            OCPBase()
+            
+if __name__ == '__main__':
+    unittest.main()
\ No newline at end of file
diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index 06bc0d55..6d9ddfda 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -1,32 +1,75 @@
 import unittest
 
+import numpy as np
 import example_robot_data as robex
 
 from agimus_controller.agimus_controller.ocp_base_croco import OCPCrocoBase
 from agimus_controller.agimus_controller.ocp_param_base import OCPParamsCrocoBase
 
 
-class TestOCPBase(unittest.TestCase):
+class TestOCPCrocoBase(unittest.TestCase):
+    
+    def __init__(self, *args, **kwargs):
+        pass
     
     def setUp(self):
-        
+        """Set up the test environment."""
         # Load the robot
         self.robot = robex.load('example_robot_description')
         self.rmodel = self.robot.model
         self.cmodel = self.robot.collision_model
         
-        # Generate some fixed params for unit testing
+        # Set some fixed params for unit testing
+        self.dt = 0.01
+        self.T = 100
+        self.qp_iters = 200
+        self.solve_iters = 200
+        self.callbacks = True
+        self.x0 = np.zeros(self.rmodel.nq + self.rmodel.nv)
         self.OCPParams = OCPParamsCrocoBase(
-            dt = 0.01,
-            T = 100,
-            qp_iters=200,
-            solver_iters=200,
-            callbacks=True,
-            
+            dt = self.dt,
+            T = self.T,
+            qp_iters=self.qp_iters,
+            solver_iters=self.solve_iters,
+            callbacks=self.callbacks,
+            ...,
         )
         return super().setUp()
     
     def test_abstract_class_instantiation(self):
+        """Test the instantiation of the OCPCrocoBase class."""
         with self.assertRaises(TypeError):
+            OCPCrocoBase(self.rmodel, self.cmodel, self.OCPParams)
             
-            OCPCrocoBase()
\ No newline at end of file
+    def test_horizon_size(self):
+        """Test the horizon_size property of the OCPCrocoBase class."""
+        ocp = OCPCrocoBase(self.rmodel, self.cmodel, self.OCPParams)
+        self.assertEqual(ocp.horizon_size, self.T)
+        
+    def test_dt(self):
+        """Test the dt property of the OCPCrocoBase class."""
+        ocp = OCPCrocoBase(self.rmodel, self.cmodel, self.OCPParams)
+        self.assertEqual(ocp.dt, self.dt)
+        
+    def test_x0(self):
+        """Test the x0 property of the OCPCrocoBase class."""
+        ocp = OCPCrocoBase(self.rmodel, self.cmodel, self.OCPParams)
+        self.assertTrue(np.array_equal(ocp.x0, self.x0))
+        
+    def test_x_init(self):
+        """Test the x_init method of the OCPCrocoBase class."""
+        ocp = OCPCrocoBase(self.rmodel, self.cmodel, self.OCPParams)
+        x_init = ocp.x_init()
+        self.assertEqual(len(x_init), self.T)
+        self.assertTrue(np.array_equal(x_init[0], self.x0))
+        
+    def test_u_init(self):
+        """Test the u_init method of the OCPCrocoBase class."""
+        ocp = OCPCrocoBase(self.rmodel, self.cmodel, self.OCPParams)
+        u_init = ocp.u_init()
+        self.assertEqual(len(u_init), self.T - 1)
+    
+    # def test_solve(self):
+    
+if __name__ == '__main__':
+    unittest.main()
\ No newline at end of file

From 0105d0ca8878b35eedc7a1df88dc9d334e9af9c7 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Wed, 11 Dec 2024 17:43:48 +0000
Subject: [PATCH 08/95] ocp croco set up

---
 agimus_controller/agimus_controller/ocp_base_croco.py |  2 +-
 agimus_controller/agimus_controller/ocp_param_base.py | 10 +++++-----
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index a3140d25..b39e9cfb 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -90,7 +90,7 @@ def solve(
             frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
                 self._state,
                 self._OCPParams.ee_name,
-                self._OCPParams.p_target,
+                self._OCPParams.WeightedTrajectoryPoints[t].point.end_effector_poses[self._OCPParams.ee_name].translation,
             )
 
             goalTrackingCost = crocoddyl.CostModelResidual(
diff --git a/agimus_controller/agimus_controller/ocp_param_base.py b/agimus_controller/agimus_controller/ocp_param_base.py
index db665383..adfa026a 100644
--- a/agimus_controller/agimus_controller/ocp_param_base.py
+++ b/agimus_controller/agimus_controller/ocp_param_base.py
@@ -13,16 +13,16 @@ class OCPParamsCrocoBase:
     # Data relevant to solve the OCP
     dt: np.float64 # Integration step of the OCP
     T: int # Number of time steps in the horizon
-    qp_iters: int = 200 # Number of QP iterations (must be a multiple of 25).
     solver_iters: int # Number of solver iterations
+    qp_iters: int = 200 # Number of QP iterations (must be a multiple of 25).
     termination_tolerance: np.float64 = 1e-3 # Termination tolerance (norm of the KKT conditions)
     eps_abs: np.float64 = 1e-6 # Absolute tolerance of the solver
     eps_rel: np.float64 = 0 # Relative tolerance of the solver
     callbacks: bool = False # Flag to enable/disable callbacks
     
     # Weights, costs & helpers for the costs & constraints
-    WeightedTrajectoryPoints: List[WeightedTrajectoryPoint] # List of weighted trajectory points    
-    armature: npt.NDArray[np.float64] # Armature of the robot
-    ee_name: str # Name of the end-effector
-    p_target: npt.NDArray[np.float64] # Target position of the end-effector in R3
+    WeightedTrajectoryPoints: List[WeightedTrajectoryPoint] | None = None # List of weighted trajectory points    
+    armature: npt.NDArray[np.float64] | None = None # Armature of the robot
+    ee_name: str | None = None # Name of the end-effector
+    p_target: npt.NDArray[np.float64] | None = None  # Target position of the end-effector in R3
         

From 4ddeea483d8e000e400bc492c099c3d8c3fea13d Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Wed, 11 Dec 2024 17:50:19 +0000
Subject: [PATCH 09/95] getting rid of target in the dataclass

---
 agimus_controller/agimus_controller/ocp_param_base.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/agimus_controller/agimus_controller/ocp_param_base.py b/agimus_controller/agimus_controller/ocp_param_base.py
index adfa026a..42e4102d 100644
--- a/agimus_controller/agimus_controller/ocp_param_base.py
+++ b/agimus_controller/agimus_controller/ocp_param_base.py
@@ -24,5 +24,4 @@ class OCPParamsCrocoBase:
     WeightedTrajectoryPoints: List[WeightedTrajectoryPoint] | None = None # List of weighted trajectory points    
     armature: npt.NDArray[np.float64] | None = None # Armature of the robot
     ee_name: str | None = None # Name of the end-effector
-    p_target: npt.NDArray[np.float64] | None = None  # Target position of the end-effector in R3
         

From b2eac487a563140056c4212b4fff879233fb6645 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Wed, 11 Dec 2024 17:50:51 +0000
Subject: [PATCH 10/95] getting rid of target in the dataclass 2

---
 agimus_controller/tests/test_ocp_param_base.py | 2 --
 1 file changed, 2 deletions(-)

diff --git a/agimus_controller/tests/test_ocp_param_base.py b/agimus_controller/tests/test_ocp_param_base.py
index a86fc3f8..e9ae4455 100644
--- a/agimus_controller/tests/test_ocp_param_base.py
+++ b/agimus_controller/tests/test_ocp_param_base.py
@@ -36,7 +36,6 @@ def test_initialization(self):
             WeightedTrajectoryPoints=weighted_trajectory_points,
             armature=armature,
             ee_name=ee_name,
-            p_target=p_target
         )
 
         self.assertEqual(params.dt, dt)
@@ -50,7 +49,6 @@ def test_initialization(self):
         self.assertEqual(params.WeightedTrajectoryPoints, weighted_trajectory_points)
         self.assertTrue(np.array_equal(params.armature, armature))
         self.assertEqual(params.ee_name, ee_name)
-        self.assertTrue(np.array_equal(params.p_target, p_target))
 
 if __name__ == '__main__':
     unittest.main()
\ No newline at end of file

From 4f07f491152cca422d908fa385c62529841bfd17 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Wed, 11 Dec 2024 17:54:56 +0000
Subject: [PATCH 11/95] [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
---
 .../agimus_controller/ocp_base.py             |   6 +-
 .../agimus_controller/ocp_base_croco.py       | 100 +++++++++++++-----
 .../agimus_controller/ocp_param_base.py       |  30 +++---
 .../agimus_controller/trajectory.py           |   2 +-
 agimus_controller/tests/test_ocp_base.py      |   7 +-
 .../tests/test_ocp_croco_base.py              |  24 ++---
 .../tests/test_ocp_param_base.py              |  23 ++--
 7 files changed, 125 insertions(+), 67 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base.py b/agimus_controller/agimus_controller/ocp_base.py
index a4a87438..f57743a4 100644
--- a/agimus_controller/agimus_controller/ocp_base.py
+++ b/agimus_controller/agimus_controller/ocp_base.py
@@ -27,7 +27,7 @@ def horizon_size() -> int:
     @property
     def dt() -> int:
         pass
-    
+
     @abstractmethod
     @property
     def x0() -> npt.NDArray[np.float64]:
@@ -35,7 +35,9 @@ def x0() -> npt.NDArray[np.float64]:
 
     @abstractmethod
     def solve(
-        self, x_init: List[npt.NDArray[np.float64]], u_init: List[npt.NDArray[np.float64]]
+        self,
+        x_init: List[npt.NDArray[np.float64]],
+        u_init: List[npt.NDArray[np.float64]],
     ) -> None:
         pass
 
diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index b39e9cfb..7c912da2 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -11,9 +11,15 @@
 from agimus_controller.agimus_controller.ocp_base import OCPBase
 from agimus_controller.agimus_controller.ocp_param_base import OCPParamsCrocoBase
 
+
 class OCPCrocoBase(OCPBase):
-    def __init__(self, rmodel: pin.Model, cmodel: pin.GeometryModel, OCPParams: OCPParamsCrocoBase) -> None:
-        """Creates an instance of the OCPCrocoBase class. This is an example of an OCP class that inherits from OCPBase, 
+    def __init__(
+        self,
+        rmodel: pin.Model,
+        cmodel: pin.GeometryModel,
+        OCPParams: OCPParamsCrocoBase,
+    ) -> None:
+        """Creates an instance of the OCPCrocoBase class. This is an example of an OCP class that inherits from OCPBase,
         for a simple goal reaching task. The class is used to solve the Optimal Control Problem (OCP) using the Crocoddyl library.
 
         Args:
@@ -24,8 +30,7 @@ def __init__(self, rmodel: pin.Model, cmodel: pin.GeometryModel, OCPParams: OCPP
         self._rmodel = rmodel
         self._cmodel = cmodel
         self._OCPParams = OCPParams
-        
-        
+
     @abstractmethod
     @property
     def horizon_size(self) -> int:
@@ -37,7 +42,7 @@ def horizon_size(self) -> int:
     def dt(self) -> np.float64:
         """Integration step of the OCP."""
         return self._OCPParams.dt
-    
+
     @abstractmethod
     @property
     def x0(self) -> np.ndarray:
@@ -49,19 +54,24 @@ def x_init(self) -> np.ndarray:
         """Initial guess of the states."""
         x_init = []
         for TrajectoryPoint in self._OCPParams.WeightedTrajectoryPoints:
-            x = np.array(TrajectoryPoint.point.robot_configuration + TrajectoryPoint.point.robot_velocity)
+            x = np.array(
+                TrajectoryPoint.point.robot_configuration
+                + TrajectoryPoint.point.robot_velocity
+            )
             x_init.append(x)
         return x_init
-    
+
     @abstractmethod
     def u_init(self) -> np.ndarray:
         """Initial guess of the controls."""
-        u_init = [np.zeros(self._rmodel.nq) for _ in range(self.horizon_size) - 1] # For the panda nu = nq, but i'm not sure nu exists in pinocchio
+        u_init = [
+            np.zeros(self._rmodel.nq) for _ in range(self.horizon_size) - 1
+        ]  # For the panda nu = nq, but i'm not sure nu exists in pinocchio
         return u_init
-    
+
     @abstractmethod
     def solve(
-        self, 
+        self,
     ) -> bool:
         """Solves the OCP. Returns True if the OCP was solved successfully, False otherwise.
 
@@ -69,7 +79,7 @@ def solve(
             bool: True if the OCP was solved successfully, False otherwise.
         """
         ### Creation of the state and actuation models
-        
+
         # Stat and actuation model
         self._state = crocoddyl.StateMultibody(self._rmodel)
         self._actuation = crocoddyl.ActuationModelFull(self._state)
@@ -90,7 +100,9 @@ def solve(
             frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
                 self._state,
                 self._OCPParams.ee_name,
-                self._OCPParams.WeightedTrajectoryPoints[t].point.end_effector_poses[self._OCPParams.ee_name].translation,
+                self._OCPParams.WeightedTrajectoryPoints[t]
+                .point.end_effector_poses[self._OCPParams.ee_name]
+                .translation,
             )
 
             goalTrackingCost = crocoddyl.CostModelResidual(
@@ -100,10 +112,31 @@ def solve(
             # Adding costs to the models
             if t < self.horizon_size - 1:
                 runningCostModel = crocoddyl.CostModelSum(self._state)
-                runningCostModel.addCost("stateReg", xRegCost, np.concatenate([self._OCPParams.WeightedTrajectoryPoints[t].weight.w_robot_configuration, self._OCPParams.WeightedTrajectoryPoints[t].weight.w_robot_velocity]))
-                runningCostModel.addCost("ctrlRegGrav", uRegCost, self._OCPParams.WeightedTrajectoryPoints[t].weight.w_robot_effort)
                 runningCostModel.addCost(
-                    "gripperPoseRM", goalTrackingCost, self._OCPParams.WeightedTrajectoryPoints[t].weight.w_end_effector_poses
+                    "stateReg",
+                    xRegCost,
+                    np.concatenate(
+                        [
+                            self._OCPParams.WeightedTrajectoryPoints[
+                                t
+                            ].weight.w_robot_configuration,
+                            self._OCPParams.WeightedTrajectoryPoints[
+                                t
+                            ].weight.w_robot_velocity,
+                        ]
+                    ),
+                )
+                runningCostModel.addCost(
+                    "ctrlRegGrav",
+                    uRegCost,
+                    self._OCPParams.WeightedTrajectoryPoints[t].weight.w_robot_effort,
+                )
+                runningCostModel.addCost(
+                    "gripperPoseRM",
+                    goalTrackingCost,
+                    self._OCPParams.WeightedTrajectoryPoints[
+                        t
+                    ].weight.w_end_effector_poses,
                 )
                 # Create Differential Action Model (DAM), i.e. continuous dynamics and cost functions
                 self._running_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics(
@@ -119,16 +152,31 @@ def solve(
                 self._runningModelList.append(runningModel)
             else:
                 terminalCostModel = crocoddyl.CostModelSum(self._state)
-                terminalCostModel.addCost("stateReg", xRegCost, np.concatenate([self._OCPParams.WeightedTrajectoryPoints[t].weight.w_robot_configuration, self._OCPParams.WeightedTrajectoryPoints[t].weight.w_robot_velocity]))
                 terminalCostModel.addCost(
-                    "gripperPose", goalTrackingCost, self._OCPParams.WeightedTrajectoryPoints[t].weight.w_end_effector_poses
+                    "stateReg",
+                    xRegCost,
+                    np.concatenate(
+                        [
+                            self._OCPParams.WeightedTrajectoryPoints[
+                                t
+                            ].weight.w_robot_configuration,
+                            self._OCPParams.WeightedTrajectoryPoints[
+                                t
+                            ].weight.w_robot_velocity,
+                        ]
+                    ),
+                )
+                terminalCostModel.addCost(
+                    "gripperPose",
+                    goalTrackingCost,
+                    self._OCPParams.WeightedTrajectoryPoints[
+                        t
+                    ].weight.w_end_effector_poses,
                 )
 
                 self._terminal_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics(
-                    self._state,
-                    self._actuation,
-                    terminalCostModel
-                    )
+                    self._state, self._actuation, terminalCostModel
+                )
 
                 self._terminalModel = crocoddyl.IntegratedActionModelEuler(
                     self._terminal_DAM, 0.0
@@ -152,14 +200,14 @@ def solve(
 
         ocp.with_callbacks = OCPParamsCrocoBase.callbacks
 
-        result = ocp.solve(self.x_init, self.u_init, OCPParamsCrocoBase.solver_iters) 
-        
+        result = ocp.solve(self.x_init, self.u_init, OCPParamsCrocoBase.solver_iters)
+
         self.ocp_results = OCPResults(
             states=ocp.xs,
-            ricatti_gains=ocp.Ks, # Not sure about this one
+            ricatti_gains=ocp.Ks,  # Not sure about this one
             feed_forward_terms=ocp.us,
         )
-        
+
         return result
 
     @abstractmethod
@@ -171,7 +219,7 @@ def ocp_results(self) -> OCPResults:
             OCPResults: Output data structure of the OCP. It contains the states, Ricatti gains and feed-forward terms.
         """
         return self._ocp_results
-        
+
     @abstractmethod
     @property
     def debug_data(self) -> OCPDebugData:
diff --git a/agimus_controller/agimus_controller/ocp_param_base.py b/agimus_controller/agimus_controller/ocp_param_base.py
index 42e4102d..22e819eb 100644
--- a/agimus_controller/agimus_controller/ocp_param_base.py
+++ b/agimus_controller/agimus_controller/ocp_param_base.py
@@ -6,22 +6,26 @@
 
 from agimus_controller.agimus_controller.trajectory import WeightedTrajectoryPoint
 
+
 @dataclass
 class OCPParamsCrocoBase:
     """Input data structure of the OCP."""
 
     # Data relevant to solve the OCP
-    dt: np.float64 # Integration step of the OCP
-    T: int # Number of time steps in the horizon
-    solver_iters: int # Number of solver iterations
-    qp_iters: int = 200 # Number of QP iterations (must be a multiple of 25).
-    termination_tolerance: np.float64 = 1e-3 # Termination tolerance (norm of the KKT conditions)
-    eps_abs: np.float64 = 1e-6 # Absolute tolerance of the solver
-    eps_rel: np.float64 = 0 # Relative tolerance of the solver
-    callbacks: bool = False # Flag to enable/disable callbacks
-    
+    dt: np.float64  # Integration step of the OCP
+    T: int  # Number of time steps in the horizon
+    solver_iters: int  # Number of solver iterations
+    qp_iters: int = 200  # Number of QP iterations (must be a multiple of 25).
+    termination_tolerance: np.float64 = (
+        1e-3  # Termination tolerance (norm of the KKT conditions)
+    )
+    eps_abs: np.float64 = 1e-6  # Absolute tolerance of the solver
+    eps_rel: np.float64 = 0  # Relative tolerance of the solver
+    callbacks: bool = False  # Flag to enable/disable callbacks
+
     # Weights, costs & helpers for the costs & constraints
-    WeightedTrajectoryPoints: List[WeightedTrajectoryPoint] | None = None # List of weighted trajectory points    
-    armature: npt.NDArray[np.float64] | None = None # Armature of the robot
-    ee_name: str | None = None # Name of the end-effector
-        
+    WeightedTrajectoryPoints: List[
+        WeightedTrajectoryPoint
+    ] | None = None  # List of weighted trajectory points
+    armature: npt.NDArray[np.float64] | None = None  # Armature of the robot
+    ee_name: str | None = None  # Name of the end-effector
diff --git a/agimus_controller/agimus_controller/trajectory.py b/agimus_controller/agimus_controller/trajectory.py
index 44e2bd43..25c2b129 100644
--- a/agimus_controller/agimus_controller/trajectory.py
+++ b/agimus_controller/agimus_controller/trajectory.py
@@ -12,7 +12,7 @@ class TrajectoryPoint:
     robot_configuration: np.ndarray
     robot_velocity: np.ndarray
     robot_acceleration: np.ndarray
-    robot_effort: np.ndarray # Is robot effort the same as robot torque?
+    robot_effort: np.ndarray  # Is robot effort the same as robot torque?
     forces: dict[Force]  # Dictionary of pinocchio.Force
     end_effector_poses: dict[SE3]  # Dictionary of pinocchio.SE3
 
diff --git a/agimus_controller/tests/test_ocp_base.py b/agimus_controller/tests/test_ocp_base.py
index ef8678a2..e22805ff 100644
--- a/agimus_controller/tests/test_ocp_base.py
+++ b/agimus_controller/tests/test_ocp_base.py
@@ -7,6 +7,7 @@ class TestOCPBase(unittest.TestCase):
     def test_abstract_class_instantiation(self):
         with self.assertRaises(TypeError):
             OCPBase()
-            
-if __name__ == '__main__':
-    unittest.main()
\ No newline at end of file
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index 6d9ddfda..3d120f53 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -8,17 +8,17 @@
 
 
 class TestOCPCrocoBase(unittest.TestCase):
-    
+
     def __init__(self, *args, **kwargs):
         pass
-    
+
     def setUp(self):
         """Set up the test environment."""
         # Load the robot
         self.robot = robex.load('example_robot_description')
         self.rmodel = self.robot.model
         self.cmodel = self.robot.collision_model
-        
+
         # Set some fixed params for unit testing
         self.dt = 0.01
         self.T = 100
@@ -35,41 +35,41 @@ def setUp(self):
             ...,
         )
         return super().setUp()
-    
+
     def test_abstract_class_instantiation(self):
         """Test the instantiation of the OCPCrocoBase class."""
         with self.assertRaises(TypeError):
             OCPCrocoBase(self.rmodel, self.cmodel, self.OCPParams)
-            
+
     def test_horizon_size(self):
         """Test the horizon_size property of the OCPCrocoBase class."""
         ocp = OCPCrocoBase(self.rmodel, self.cmodel, self.OCPParams)
         self.assertEqual(ocp.horizon_size, self.T)
-        
+
     def test_dt(self):
         """Test the dt property of the OCPCrocoBase class."""
         ocp = OCPCrocoBase(self.rmodel, self.cmodel, self.OCPParams)
         self.assertEqual(ocp.dt, self.dt)
-        
+
     def test_x0(self):
         """Test the x0 property of the OCPCrocoBase class."""
         ocp = OCPCrocoBase(self.rmodel, self.cmodel, self.OCPParams)
         self.assertTrue(np.array_equal(ocp.x0, self.x0))
-        
+
     def test_x_init(self):
         """Test the x_init method of the OCPCrocoBase class."""
         ocp = OCPCrocoBase(self.rmodel, self.cmodel, self.OCPParams)
         x_init = ocp.x_init()
         self.assertEqual(len(x_init), self.T)
         self.assertTrue(np.array_equal(x_init[0], self.x0))
-        
+
     def test_u_init(self):
         """Test the u_init method of the OCPCrocoBase class."""
         ocp = OCPCrocoBase(self.rmodel, self.cmodel, self.OCPParams)
         u_init = ocp.u_init()
         self.assertEqual(len(u_init), self.T - 1)
-    
+
     # def test_solve(self):
-    
+
 if __name__ == '__main__':
-    unittest.main()
\ No newline at end of file
+    unittest.main()
diff --git a/agimus_controller/tests/test_ocp_param_base.py b/agimus_controller/tests/test_ocp_param_base.py
index e9ae4455..6ffe8e55 100644
--- a/agimus_controller/tests/test_ocp_param_base.py
+++ b/agimus_controller/tests/test_ocp_param_base.py
@@ -2,14 +2,19 @@
 import numpy as np
 
 from agimus_controller.agimus_controller.ocp_param_base import OCPParamsCrocoBase
-from agimus_controller.agimus_controller.trajectory import WeightedTrajectoryPoint, TrajectoryPoint, TrajectoryPointWeights
+from agimus_controller.agimus_controller.trajectory import (
+    WeightedTrajectoryPoint,
+    TrajectoryPoint,
+    TrajectoryPointWeights,
+)
+
 
 class TestOCPParamsCrocoBase(unittest.TestCase):
     """Test the OCPParamsCrocoBase class."""
-    
-    def __init__(self, methodName = "runTest"):
+
+    def __init__(self, methodName="runTest"):
         super().__init__(methodName)
-        
+
     def test_initialization(self):
         dt = np.float64(0.01)
         T = 100
@@ -20,10 +25,7 @@ def test_initialization(self):
         weights = TrajectoryPointWeights(
             w_robot_configuration=np.array([1.0, 2.0, 3.0], dtype=np.float64),
         )
-        weighted_trajectory_point = WeightedTrajectoryPoint(
-            point, 
-            weights
-        )
+        weighted_trajectory_point = WeightedTrajectoryPoint(point, weights)
         weighted_trajectory_points = [weighted_trajectory_point]
         armature = np.array([1.0, 2.0, 3.0], dtype=np.float64)
         ee_name = "end_effector"
@@ -50,5 +52,6 @@ def test_initialization(self):
         self.assertTrue(np.array_equal(params.armature, armature))
         self.assertEqual(params.ee_name, ee_name)
 
-if __name__ == '__main__':
-    unittest.main()
\ No newline at end of file
+
+if __name__ == "__main__":
+    unittest.main()

From 61beafad73312d2188a89131f282b22e8ec9f16a Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Thu, 12 Dec 2024 11:39:22 +0000
Subject: [PATCH 12/95] fixing imports

---
 agimus_controller/agimus_controller/mpc_data.py       | 2 +-
 agimus_controller/agimus_controller/ocp_base.py       | 4 ++--
 agimus_controller/agimus_controller/ocp_base_croco.py | 8 ++++----
 agimus_controller/agimus_controller/ocp_param_base.py | 2 +-
 agimus_controller/tests/test_ocp_base.py              | 2 +-
 agimus_controller/tests/test_ocp_croco_base.py        | 4 ++--
 6 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/agimus_controller/agimus_controller/mpc_data.py b/agimus_controller/agimus_controller/mpc_data.py
index 19d0c25d..a01e0a94 100644
--- a/agimus_controller/agimus_controller/mpc_data.py
+++ b/agimus_controller/agimus_controller/mpc_data.py
@@ -2,7 +2,7 @@
 import numpy as np
 import numpy.typing as npt
 
-from agimus_controller.agimus_controller.trajectory import TrajectoryPoint
+from agimus_controller.trajectory import TrajectoryPoint
 
 
 @dataclass
diff --git a/agimus_controller/agimus_controller/ocp_base.py b/agimus_controller/agimus_controller/ocp_base.py
index f57743a4..375e3f90 100644
--- a/agimus_controller/agimus_controller/ocp_base.py
+++ b/agimus_controller/agimus_controller/ocp_base.py
@@ -4,8 +4,8 @@
 import numpy as np
 import numpy.typing as npt
 
-from agimus_controller.agimus_controller.mpc_data import OCPResults, OCPDebugData
-from agimus_controller.agimus_controller.trajectory import WeightedTrajectoryPoint
+from agimus_controller.mpc_data import OCPResults, OCPDebugData
+from agimus_controller.trajectory import WeightedTrajectoryPoint
 
 
 class OCPBase(ABC):
diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 7c912da2..943aabde 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -6,10 +6,10 @@
 import numpy as np
 import pinocchio as pin
 
-from agimus_controller.agimus_controller.mpc_data import OCPResults, OCPDebugData
-from agimus_controller.agimus_controller.trajectory import WeightedTrajectoryPoint
-from agimus_controller.agimus_controller.ocp_base import OCPBase
-from agimus_controller.agimus_controller.ocp_param_base import OCPParamsCrocoBase
+from agimus_controller.mpc_data import OCPResults, OCPDebugData
+from agimus_controller.trajectory import WeightedTrajectoryPoint
+from agimus_controller.ocp_base import OCPBase
+from agimus_controller.ocp_param_base import OCPParamsCrocoBase
 
 
 class OCPCrocoBase(OCPBase):
diff --git a/agimus_controller/agimus_controller/ocp_param_base.py b/agimus_controller/agimus_controller/ocp_param_base.py
index 22e819eb..acd24343 100644
--- a/agimus_controller/agimus_controller/ocp_param_base.py
+++ b/agimus_controller/agimus_controller/ocp_param_base.py
@@ -4,7 +4,7 @@
 import numpy as np
 import numpy.typing as npt
 
-from agimus_controller.agimus_controller.trajectory import WeightedTrajectoryPoint
+from agimus_controller.trajectory import WeightedTrajectoryPoint
 
 
 @dataclass
diff --git a/agimus_controller/tests/test_ocp_base.py b/agimus_controller/tests/test_ocp_base.py
index e22805ff..af02a0fa 100644
--- a/agimus_controller/tests/test_ocp_base.py
+++ b/agimus_controller/tests/test_ocp_base.py
@@ -1,6 +1,6 @@
 import unittest
 
-from agimus_controller.agimus_controller.ocp_base import OCPBase
+from agimus_controller.ocp_base import OCPBase
 
 
 class TestOCPBase(unittest.TestCase):
diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index 3d120f53..f0395500 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -3,8 +3,8 @@
 import numpy as np
 import example_robot_data as robex
 
-from agimus_controller.agimus_controller.ocp_base_croco import OCPCrocoBase
-from agimus_controller.agimus_controller.ocp_param_base import OCPParamsCrocoBase
+from agimus_controller.ocp_base_croco import OCPCrocoBase
+from agimus_controller.ocp_param_base import OCPParamsCrocoBase
 
 
 class TestOCPCrocoBase(unittest.TestCase):

From 5d0ced3b890e071c5256cc82cd36dfe316d83205 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Thu, 12 Dec 2024 11:41:14 +0000
Subject: [PATCH 13/95] fixing imports 2

---
 agimus_controller/tests/test_ocp_param_base.py | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/agimus_controller/tests/test_ocp_param_base.py b/agimus_controller/tests/test_ocp_param_base.py
index 6ffe8e55..22299a24 100644
--- a/agimus_controller/tests/test_ocp_param_base.py
+++ b/agimus_controller/tests/test_ocp_param_base.py
@@ -1,6 +1,7 @@
 import unittest
 import numpy as np
 
+<<<<<<< Updated upstream
 from agimus_controller.agimus_controller.ocp_param_base import OCPParamsCrocoBase
 from agimus_controller.agimus_controller.trajectory import (
     WeightedTrajectoryPoint,
@@ -8,6 +9,10 @@
     TrajectoryPointWeights,
 )
 
+=======
+from agimus_controller.ocp_param_base import OCPParamsCrocoBase
+from agimus_controller.trajectory import WeightedTrajectoryPoint, TrajectoryPoint, TrajectoryPointWeights
+>>>>>>> Stashed changes
 
 class TestOCPParamsCrocoBase(unittest.TestCase):
     """Test the OCPParamsCrocoBase class."""

From 6be9c9ff80699878e7955caa23426c429b672664 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Thu, 12 Dec 2024 11:42:45 +0000
Subject: [PATCH 14/95] removing x0 property ocp base

---
 agimus_controller/agimus_controller/ocp_base.py | 17 ++++++-----------
 1 file changed, 6 insertions(+), 11 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base.py b/agimus_controller/agimus_controller/ocp_base.py
index 375e3f90..f54bdfcd 100644
--- a/agimus_controller/agimus_controller/ocp_base.py
+++ b/agimus_controller/agimus_controller/ocp_base.py
@@ -16,22 +16,17 @@ def __init__(self) -> None:
     def set_reference_horizon(
         self, reference_trajectory: list[WeightedTrajectoryPoint]
     ) -> None:
-        pass
+        ...
 
     @abstractmethod
     @property
     def horizon_size() -> int:
-        pass
+        ...
 
     @abstractmethod
     @property
     def dt() -> int:
-        pass
-
-    @abstractmethod
-    @property
-    def x0() -> npt.NDArray[np.float64]:
-        pass
+        ...
 
     @abstractmethod
     def solve(
@@ -39,14 +34,14 @@ def solve(
         x_init: List[npt.NDArray[np.float64]],
         u_init: List[npt.NDArray[np.float64]],
     ) -> None:
-        pass
+        ...
 
     @abstractmethod
     @property
     def ocp_results(self) -> OCPResults:
-        pass
+        ...
 
     @abstractmethod
     @property
     def debug_data(self) -> OCPDebugData:
-        pass
+        ...

From bf1dde784fe21b2ea3d406595ab2c28de14276f2 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Thu, 12 Dec 2024 13:58:10 +0100
Subject: [PATCH 15/95] Update
 agimus_controller/agimus_controller/trajectory.py

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 agimus_controller/agimus_controller/trajectory.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/agimus_controller/agimus_controller/trajectory.py b/agimus_controller/agimus_controller/trajectory.py
index 25c2b129..10332ad3 100644
--- a/agimus_controller/agimus_controller/trajectory.py
+++ b/agimus_controller/agimus_controller/trajectory.py
@@ -12,7 +12,7 @@ class TrajectoryPoint:
     robot_configuration: np.ndarray
     robot_velocity: np.ndarray
     robot_acceleration: np.ndarray
-    robot_effort: np.ndarray  # Is robot effort the same as robot torque?
+    robot_effort: np.ndarray
     forces: dict[Force]  # Dictionary of pinocchio.Force
     end_effector_poses: dict[SE3]  # Dictionary of pinocchio.SE3
 

From 4c42f41761c73d9b72bf6375f39ef8e32d69fcf5 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Thu, 12 Dec 2024 13:59:52 +0100
Subject: [PATCH 16/95] Update agimus_controller/agimus_controller/ocp_base.py

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 agimus_controller/agimus_controller/ocp_base.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base.py b/agimus_controller/agimus_controller/ocp_base.py
index f54bdfcd..b0c59e78 100644
--- a/agimus_controller/agimus_controller/ocp_base.py
+++ b/agimus_controller/agimus_controller/ocp_base.py
@@ -31,8 +31,8 @@ def dt() -> int:
     @abstractmethod
     def solve(
         self,
-        x_init: List[npt.NDArray[np.float64]],
-        u_init: List[npt.NDArray[np.float64]],
+        x_init: list[npt.NDArray[np.float64]],
+        u_init: list[npt.NDArray[np.float64]],
     ) -> None:
         ...
 

From eab8f12f9fe15da9b5e2d70f15836c8b62c5fbdf Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Fri, 13 Dec 2024 11:18:45 +0000
Subject: [PATCH 17/95] docstring + bringing back x0 in the solve + changing
 the name x_init to x_warmstart for clarity's sake

---
 .../agimus_controller/ocp_base.py             | 40 +++++++++++++++++--
 1 file changed, 37 insertions(+), 3 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base.py b/agimus_controller/agimus_controller/ocp_base.py
index b0c59e78..770c6b34 100644
--- a/agimus_controller/agimus_controller/ocp_base.py
+++ b/agimus_controller/agimus_controller/ocp_base.py
@@ -1,5 +1,4 @@
 from abc import ABC, abstractmethod
-from typing import List
 
 import numpy as np
 import numpy.typing as npt
@@ -9,6 +8,10 @@
 
 
 class OCPBase(ABC):
+    """Base class for the Optimal Control Problem (OCP) solver. This class defines the interface for the OCP solver.
+    If you want to implement a new OCP solver, you should derive from this class and implement the abstract methods.
+    If you want to use Crocoddyl, you should inherit from the OCPCrocoBase class instead.
+    """
     def __init__(self) -> None:
         pass
 
@@ -16,32 +19,63 @@ def __init__(self) -> None:
     def set_reference_horizon(
         self, reference_trajectory: list[WeightedTrajectoryPoint]
     ) -> None:
+        """Set the reference trajectory and the weights of the costs for the OCP solver. This method should be implemented by the derived class."""
         ...
 
     @abstractmethod
     @property
     def horizon_size() -> int:
+        """Returns the horizon size of the OCP.
+
+        Returns:
+            int: size of the horizon.
+        """
         ...
 
     @abstractmethod
     @property
     def dt() -> int:
+        """Returns the time step of the OCP.
+
+        Returns:
+            int: time step of the OCP.
+        """
         ...
 
     @abstractmethod
     def solve(
         self,
-        x_init: list[npt.NDArray[np.float64]],
-        u_init: list[npt.NDArray[np.float64]],
+        x0: npt.NDArray[np.float64],
+        x_warmstart: list[npt.NDArray[np.float64]],
+        u_warmstart: list[npt.NDArray[np.float64]],
     ) -> 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.
+        
+        Args:
+            x0 (npt.NDArray[np.float64]): current state of the robot.
+            x_warmstart (list[npt.NDArray[np.float64]]): Warmstart values for the state. This doesn't include the current state.
+            u_warmstart (list[npt.NDArray[np.float64]]): Warmstart values for the control inputs. 
+        """
         ...
 
     @abstractmethod
     @property
     def ocp_results(self) -> OCPResults:
+        """Returns the results of the OCP solver.
+        The solve method should be called before calling this method.
+
+        Returns:
+            OCPResults: Class containing the results of the OCP solver.
+        """
         ...
 
     @abstractmethod
     @property
     def debug_data(self) -> OCPDebugData:
+        """Returns the debug data of the OCP solver.
+
+        Returns:
+            OCPDebugData: Class containing the debug data of the OCP solver.
+        """
         ...

From 894fce4f073f7417f03d6b158775cc5795ebf5d2 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Fri, 13 Dec 2024 11:18:57 +0000
Subject: [PATCH 18/95] [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
---
 agimus_controller/agimus_controller/ocp_base.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base.py b/agimus_controller/agimus_controller/ocp_base.py
index 770c6b34..1f6e545f 100644
--- a/agimus_controller/agimus_controller/ocp_base.py
+++ b/agimus_controller/agimus_controller/ocp_base.py
@@ -12,6 +12,7 @@ class OCPBase(ABC):
     If you want to implement a new OCP solver, you should derive from this class and implement the abstract methods.
     If you want to use Crocoddyl, you should inherit from the OCPCrocoBase class instead.
     """
+
     def __init__(self) -> None:
         pass
 
@@ -51,11 +52,11 @@ def solve(
     ) -> 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.
-        
+
         Args:
             x0 (npt.NDArray[np.float64]): current state of the robot.
             x_warmstart (list[npt.NDArray[np.float64]]): Warmstart values for the state. This doesn't include the current state.
-            u_warmstart (list[npt.NDArray[np.float64]]): Warmstart values for the control inputs. 
+            u_warmstart (list[npt.NDArray[np.float64]]): Warmstart values for the control inputs.
         """
         ...
 

From 09c47cc9a85aa0a01c90065a87cc681b67a446d4 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Fri, 13 Dec 2024 11:35:16 +0000
Subject: [PATCH 19/95] adding armature in the robot model factory

---
 .../agimus_controller/factory/robot_model.py   | 18 +++++++++++++++---
 .../agimus_controller/ocp_param_base.py        |  2 --
 2 files changed, 15 insertions(+), 5 deletions(-)

diff --git a/agimus_controller/agimus_controller/factory/robot_model.py b/agimus_controller/agimus_controller/factory/robot_model.py
index c8be7d6e..851b2d49 100644
--- a/agimus_controller/agimus_controller/factory/robot_model.py
+++ b/agimus_controller/agimus_controller/factory/robot_model.py
@@ -1,10 +1,12 @@
-from copy import deepcopy
 from dataclasses import dataclass
-import numpy as np
+from copy import deepcopy
 from pathlib import Path
-import pinocchio as pin
 from typing import Union
 
+import numpy as np
+import numpy.typing as npt
+import pinocchio as pin
+
 
 @dataclass
 class RobotModelParameters:
@@ -15,6 +17,7 @@ class RobotModelParameters:
     srdf = Path() | str
     collision_as_capsule = False
     self_collision = False
+    armature = npt.NDArray[np.float64]
 
 
 class RobotModelFactory:
@@ -94,3 +97,12 @@ def create_model_parameters(self) -> RobotModelParameters:
     def print_model(self):
         print("full model =\n", self._complete_model)
         print("reduced model =\n", self._rmodel)
+
+    @property
+    def armature(self) -> npt.NDArray[np.float64]:
+        """Armature of the robot.
+
+        Returns:
+            npt.NDArray[np.float64]: Armature of the robot.
+        """
+        return self._params.armature
\ No newline at end of file
diff --git a/agimus_controller/agimus_controller/ocp_param_base.py b/agimus_controller/agimus_controller/ocp_param_base.py
index acd24343..9c5d5791 100644
--- a/agimus_controller/agimus_controller/ocp_param_base.py
+++ b/agimus_controller/agimus_controller/ocp_param_base.py
@@ -27,5 +27,3 @@ class OCPParamsCrocoBase:
     WeightedTrajectoryPoints: List[
         WeightedTrajectoryPoint
     ] | None = None  # List of weighted trajectory points
-    armature: npt.NDArray[np.float64] | None = None  # Armature of the robot
-    ee_name: str | None = None  # Name of the end-effector

From a55807da8ee04cb34ad19f93de0c4d3b821f9757 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Fri, 13 Dec 2024 11:40:03 +0000
Subject: [PATCH 20/95] getting rid of the x0 property

---
 .../agimus_controller/ocp_base_croco.py             | 13 ++-----------
 1 file changed, 2 insertions(+), 11 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 943aabde..e81e4d3d 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -10,7 +10,7 @@
 from agimus_controller.trajectory import WeightedTrajectoryPoint
 from agimus_controller.ocp_base import OCPBase
 from agimus_controller.ocp_param_base import OCPParamsCrocoBase
-
+from agimus_controller.factory.robot_model import RobotModel
 
 class OCPCrocoBase(OCPBase):
     def __init__(
@@ -19,8 +19,7 @@ def __init__(
         cmodel: pin.GeometryModel,
         OCPParams: OCPParamsCrocoBase,
     ) -> None:
-        """Creates an instance of the OCPCrocoBase class. This is an example of an OCP class that inherits from OCPBase,
-        for a simple goal reaching task. The class is used to solve the Optimal Control Problem (OCP) using the Crocoddyl library.
+        """Defines common behavior for all OCP using croccodyl. This is an abstract class with some helpers to design OCPs in a more friendly way.
 
         Args:
             rmodel (pin.Model): Robot model.
@@ -31,24 +30,16 @@ def __init__(
         self._cmodel = cmodel
         self._OCPParams = OCPParams
 
-    @abstractmethod
     @property
     def horizon_size(self) -> int:
         """Number of time steps in the horizon."""
         return self._OCPParams.T
 
-    @abstractmethod
     @property
     def dt(self) -> np.float64:
         """Integration step of the OCP."""
         return self._OCPParams.dt
 
-    @abstractmethod
-    @property
-    def x0(self) -> np.ndarray:
-        """Initial state of the robot."""
-        return self._OCPParams.WeightedTrajectoryPoints[0].point.robot_configuration
-
     @abstractmethod
     def x_init(self) -> np.ndarray:
         """Initial guess of the states."""

From f7db9b64e9f588d5ba03a216cff4357752651b91 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Fri, 13 Dec 2024 11:43:07 +0000
Subject: [PATCH 21/95] getting rid of x init and u init method

---
 .../agimus_controller/ocp_base_croco.py       | 30 +++----------------
 1 file changed, 4 insertions(+), 26 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index e81e4d3d..6fb97fb6 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -1,9 +1,7 @@
-from abc import ABC, abstractmethod
-from typing import List
-
 import crocoddyl
 import mim_solvers
 import numpy as np
+import numpy.typing as npt
 import pinocchio as pin
 
 from agimus_controller.mpc_data import OCPResults, OCPDebugData
@@ -40,29 +38,11 @@ def dt(self) -> np.float64:
         """Integration step of the OCP."""
         return self._OCPParams.dt
 
-    @abstractmethod
-    def x_init(self) -> np.ndarray:
-        """Initial guess of the states."""
-        x_init = []
-        for TrajectoryPoint in self._OCPParams.WeightedTrajectoryPoints:
-            x = np.array(
-                TrajectoryPoint.point.robot_configuration
-                + TrajectoryPoint.point.robot_velocity
-            )
-            x_init.append(x)
-        return x_init
-
-    @abstractmethod
-    def u_init(self) -> np.ndarray:
-        """Initial guess of the controls."""
-        u_init = [
-            np.zeros(self._rmodel.nq) for _ in range(self.horizon_size) - 1
-        ]  # For the panda nu = nq, but i'm not sure nu exists in pinocchio
-        return u_init
-
-    @abstractmethod
     def solve(
         self,
+        x0: npt.NDArray[np.float64],
+        x_warmstart: list[npt.NDArray[np.float64]],
+        u_warmstart: list[npt.NDArray[np.float64]],
     ) -> bool:
         """Solves the OCP. Returns True if the OCP was solved successfully, False otherwise.
 
@@ -201,7 +181,6 @@ def solve(
 
         return result
 
-    @abstractmethod
     @property
     def ocp_results(self) -> OCPResults:
         """Output data structure of the OCP.
@@ -211,7 +190,6 @@ def ocp_results(self) -> OCPResults:
         """
         return self._ocp_results
 
-    @abstractmethod
     @property
     def debug_data(self) -> OCPDebugData:
         ...

From 92212d21a959e845151cb635bfb8a60a09e56baf Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Fri, 13 Dec 2024 11:54:10 +0000
Subject: [PATCH 22/95] changing the robot model input for the init

---
 .../agimus_controller/ocp_base_croco.py       | 20 +++++++++++++------
 1 file changed, 14 insertions(+), 6 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 6fb97fb6..57435088 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -5,16 +5,14 @@
 import pinocchio as pin
 
 from agimus_controller.mpc_data import OCPResults, OCPDebugData
-from agimus_controller.trajectory import WeightedTrajectoryPoint
 from agimus_controller.ocp_base import OCPBase
 from agimus_controller.ocp_param_base import OCPParamsCrocoBase
-from agimus_controller.factory.robot_model import RobotModel
+from agimus_controller.factory.robot_model import RobotModelFactory
 
 class OCPCrocoBase(OCPBase):
     def __init__(
         self,
-        rmodel: pin.Model,
-        cmodel: pin.GeometryModel,
+        robot_model: RobotModelFactory,
         OCPParams: OCPParamsCrocoBase,
     ) -> None:
         """Defines common behavior for all OCP using croccodyl. This is an abstract class with some helpers to design OCPs in a more friendly way.
@@ -24,8 +22,13 @@ def __init__(
             cmodel (pin.GeometryModel): Collision Model of the robot.
             OCPParams (OCPParamsBase): Input data structure of the OCP.
         """
-        self._rmodel = rmodel
-        self._cmodel = cmodel
+        # Setting the robot model
+        self._robot_model = robot_model
+        self._rmodel = self._robot_model._rmodel
+        self._cmodel = self._robot_model._complete_collision_model
+        self._armature = self._robot_model._params.armature
+        
+        # Setting the OCP parameters
         self._OCPParams = OCPParams
 
     @property
@@ -46,6 +49,11 @@ def solve(
     ) -> bool:
         """Solves the OCP. Returns True if the OCP was solved successfully, False otherwise.
 
+        Args:
+            x0 (npt.NDArray[np.float64]): Current state of the robot.
+            x_warmstart (list[npt.NDArray[np.float64]]): Warmstart values for the state. This doesn't include the current state.
+            u_warmstart (list[npt.NDArray[np.float64]]): Warmstart values for the control inputs.
+
         Returns:
             bool: True if the OCP was solved successfully, False otherwise.
         """

From 0e8d145916ffecf1a4f3372860d06253a4d046c5 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Fri, 13 Dec 2024 11:57:38 +0000
Subject: [PATCH 23/95] getting rid of the trajectory points

---
 agimus_controller/agimus_controller/ocp_param_base.py | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_param_base.py b/agimus_controller/agimus_controller/ocp_param_base.py
index 9c5d5791..b127fd2e 100644
--- a/agimus_controller/agimus_controller/ocp_param_base.py
+++ b/agimus_controller/agimus_controller/ocp_param_base.py
@@ -22,8 +22,3 @@ class OCPParamsCrocoBase:
     eps_abs: np.float64 = 1e-6  # Absolute tolerance of the solver
     eps_rel: np.float64 = 0  # Relative tolerance of the solver
     callbacks: bool = False  # Flag to enable/disable callbacks
-
-    # Weights, costs & helpers for the costs & constraints
-    WeightedTrajectoryPoints: List[
-        WeightedTrajectoryPoint
-    ] | None = None  # List of weighted trajectory points

From 95ab7c7811f30999a341a9d4b1a5142587f8d480 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Fri, 13 Dec 2024 11:58:03 +0000
Subject: [PATCH 24/95] getting rid of useless imports

---
 agimus_controller/agimus_controller/ocp_param_base.py | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_param_base.py b/agimus_controller/agimus_controller/ocp_param_base.py
index b127fd2e..57eafdfc 100644
--- a/agimus_controller/agimus_controller/ocp_param_base.py
+++ b/agimus_controller/agimus_controller/ocp_param_base.py
@@ -1,11 +1,6 @@
 from dataclasses import dataclass
-from typing import List
 
 import numpy as np
-import numpy.typing as npt
-
-from agimus_controller.trajectory import WeightedTrajectoryPoint
-
 
 @dataclass
 class OCPParamsCrocoBase:

From ef8bd36f97391b2cee3019c94e12e78b603e0b4e Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Fri, 13 Dec 2024 12:10:07 +0000
Subject: [PATCH 25/95] changing the solve function to only solving and
 creating properties for the running models & terminal model

---
 .../agimus_controller/ocp_base_croco.py       | 137 ++++--------------
 1 file changed, 27 insertions(+), 110 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 57435088..8ade68be 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -1,3 +1,5 @@
+from abc import abstractmethod
+
 import crocoddyl
 import mim_solvers
 import numpy as np
@@ -28,6 +30,10 @@ def __init__(
         self._cmodel = self._robot_model._complete_collision_model
         self._armature = self._robot_model._params.armature
         
+        # Stat and actuation model
+        self._state = crocoddyl.StateMultibody(self._rmodel)
+        self._actuation = crocoddyl.ActuationModelFull(self._state)
+
         # Setting the OCP parameters
         self._OCPParams = OCPParams
 
@@ -41,6 +47,18 @@ def dt(self) -> np.float64:
         """Integration step of the OCP."""
         return self._OCPParams.dt
 
+    @abstractmethod
+    @property
+    def runningModelList(self) -> list[crocoddyl.ActionModelAbstract]:
+        """List of running models."""
+        ...
+            
+    @abstractmethod
+    @property
+    def terminalModel(self) -> crocoddyl.ActionModelAbstract:
+        """Terminal model."""
+        ...    
+    
     def solve(
         self,
         x0: npt.NDArray[np.float64],
@@ -59,117 +77,15 @@ def solve(
         """
         ### Creation of the state and actuation models
 
-        # Stat and actuation model
-        self._state = crocoddyl.StateMultibody(self._rmodel)
-        self._actuation = crocoddyl.ActuationModelFull(self._state)
-
-        self._runningModelList = []
-        # Running & terminal cost models
-        for t in range(self.horizon_size):
-            ### Creation of cost terms
-            # State Regularization cost
-            xResidual = crocoddyl.ResidualModelState(self._state, self.x0)
-            xRegCost = crocoddyl.CostModelResidual(self._state, xResidual)
-
-            # Control Regularization cost
-            uResidual = crocoddyl.ResidualModelControl(self._state)
-            uRegCost = crocoddyl.CostModelResidual(self._state, uResidual)
-
-            # End effector frame cost
-            frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
-                self._state,
-                self._OCPParams.ee_name,
-                self._OCPParams.WeightedTrajectoryPoints[t]
-                .point.end_effector_poses[self._OCPParams.ee_name]
-                .translation,
-            )
-
-            goalTrackingCost = crocoddyl.CostModelResidual(
-                self._state, frameTranslationResidual
-            )
-
-            # Adding costs to the models
-            if t < self.horizon_size - 1:
-                runningCostModel = crocoddyl.CostModelSum(self._state)
-                runningCostModel.addCost(
-                    "stateReg",
-                    xRegCost,
-                    np.concatenate(
-                        [
-                            self._OCPParams.WeightedTrajectoryPoints[
-                                t
-                            ].weight.w_robot_configuration,
-                            self._OCPParams.WeightedTrajectoryPoints[
-                                t
-                            ].weight.w_robot_velocity,
-                        ]
-                    ),
-                )
-                runningCostModel.addCost(
-                    "ctrlRegGrav",
-                    uRegCost,
-                    self._OCPParams.WeightedTrajectoryPoints[t].weight.w_robot_effort,
-                )
-                runningCostModel.addCost(
-                    "gripperPoseRM",
-                    goalTrackingCost,
-                    self._OCPParams.WeightedTrajectoryPoints[
-                        t
-                    ].weight.w_end_effector_poses,
-                )
-                # Create Differential Action Model (DAM), i.e. continuous dynamics and cost functions
-                self._running_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics(
-                    self._state,
-                    self._actuation,
-                    runningCostModel,
-                )
-                # Create Integrated Action Model (IAM), i.e. Euler integration of continuous dynamics and cost
-                runningModel = crocoddyl.IntegratedActionModelEuler(
-                    self._running_DAM, self.dt
-                )
-                runningModel.differential.armature = OCPParamsCrocoBase.armature
-                self._runningModelList.append(runningModel)
-            else:
-                terminalCostModel = crocoddyl.CostModelSum(self._state)
-                terminalCostModel.addCost(
-                    "stateReg",
-                    xRegCost,
-                    np.concatenate(
-                        [
-                            self._OCPParams.WeightedTrajectoryPoints[
-                                t
-                            ].weight.w_robot_configuration,
-                            self._OCPParams.WeightedTrajectoryPoints[
-                                t
-                            ].weight.w_robot_velocity,
-                        ]
-                    ),
-                )
-                terminalCostModel.addCost(
-                    "gripperPose",
-                    goalTrackingCost,
-                    self._OCPParams.WeightedTrajectoryPoints[
-                        t
-                    ].weight.w_end_effector_poses,
-                )
-
-                self._terminal_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics(
-                    self._state, self._actuation, terminalCostModel
-                )
-
-                self._terminalModel = crocoddyl.IntegratedActionModelEuler(
-                    self._terminal_DAM, 0.0
-                )
-                self._terminalModel.differential.armature = OCPParamsCrocoBase.armature
 
         problem = crocoddyl.ShootingProblem(
-            self.x0, self._runningModelList, self._terminalModel
+            x0, self.runningModelList, self.terminalModel
         )
         # Create solver + callbacks
-        ocp = mim_solvers.SolverSQP(problem)
+        ocp = mim_solvers.SolverCSQP(problem)
 
         # Merit function
-        ocp.use_filter_line_search = False
+        ocp.use_filter_line_search = self._OCPParams.use_filter_line_search
 
         # Parameters of the solver
         ocp.termination_tolerance = OCPParamsCrocoBase.termination_tolerance
@@ -179,16 +95,17 @@ def solve(
 
         ocp.with_callbacks = OCPParamsCrocoBase.callbacks
 
-        result = ocp.solve(self.x_init, self.u_init, OCPParamsCrocoBase.solver_iters)
+        x_init = [x0] + x_warmstart
+        u_init = u_warmstart
+        
+        result = ocp.solve(x_init, u_init, OCPParamsCrocoBase.solver_iters)
 
         self.ocp_results = OCPResults(
             states=ocp.xs,
-            ricatti_gains=ocp.Ks,  # Not sure about this one
+            ricatti_gains=ocp.K,
             feed_forward_terms=ocp.us,
         )
 
-        return result
-
     @property
     def ocp_results(self) -> OCPResults:
         """Output data structure of the OCP.
@@ -196,7 +113,7 @@ def ocp_results(self) -> OCPResults:
         Returns:
             OCPResults: Output data structure of the OCP. It contains the states, Ricatti gains and feed-forward terms.
         """
-        return self._ocp_results
+        return self.ocp_results
 
     @property
     def debug_data(self) -> OCPDebugData:

From 5cda79038671bb9367d8ef0bfff2bd57fed803bb Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Fri, 13 Dec 2024 12:10:51 +0000
Subject: [PATCH 26/95] changing the result of solve

---
 agimus_controller/agimus_controller/ocp_base_croco.py | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 8ade68be..289bb5b0 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -66,14 +66,12 @@ def solve(
         u_warmstart: list[npt.NDArray[np.float64]],
     ) -> bool:
         """Solves the OCP. Returns True if the OCP was solved successfully, False otherwise.
+        The results can be accessed through the ocp_results property.
 
         Args:
             x0 (npt.NDArray[np.float64]): Current state of the robot.
             x_warmstart (list[npt.NDArray[np.float64]]): Warmstart values for the state. This doesn't include the current state.
             u_warmstart (list[npt.NDArray[np.float64]]): Warmstart values for the control inputs.
-
-        Returns:
-            bool: True if the OCP was solved successfully, False otherwise.
         """
         ### Creation of the state and actuation models
 

From 6e54b6ecc98c8c0c6b87d88f66dfc51bc7acdb2a Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Fri, 13 Dec 2024 12:11:15 +0000
Subject: [PATCH 27/95] [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
---
 .../agimus_controller/factory/robot_model.py         |  2 +-
 .../agimus_controller/ocp_base_croco.py              | 12 ++++++------
 .../agimus_controller/ocp_param_base.py              |  1 +
 3 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/agimus_controller/agimus_controller/factory/robot_model.py b/agimus_controller/agimus_controller/factory/robot_model.py
index 851b2d49..43efd76b 100644
--- a/agimus_controller/agimus_controller/factory/robot_model.py
+++ b/agimus_controller/agimus_controller/factory/robot_model.py
@@ -105,4 +105,4 @@ def armature(self) -> npt.NDArray[np.float64]:
         Returns:
             npt.NDArray[np.float64]: Armature of the robot.
         """
-        return self._params.armature
\ No newline at end of file
+        return self._params.armature
diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 289bb5b0..c399666e 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -11,6 +11,7 @@
 from agimus_controller.ocp_param_base import OCPParamsCrocoBase
 from agimus_controller.factory.robot_model import RobotModelFactory
 
+
 class OCPCrocoBase(OCPBase):
     def __init__(
         self,
@@ -29,7 +30,7 @@ def __init__(
         self._rmodel = self._robot_model._rmodel
         self._cmodel = self._robot_model._complete_collision_model
         self._armature = self._robot_model._params.armature
-        
+
         # Stat and actuation model
         self._state = crocoddyl.StateMultibody(self._rmodel)
         self._actuation = crocoddyl.ActuationModelFull(self._state)
@@ -52,13 +53,13 @@ def dt(self) -> np.float64:
     def runningModelList(self) -> list[crocoddyl.ActionModelAbstract]:
         """List of running models."""
         ...
-            
+
     @abstractmethod
     @property
     def terminalModel(self) -> crocoddyl.ActionModelAbstract:
         """Terminal model."""
-        ...    
-    
+        ...
+
     def solve(
         self,
         x0: npt.NDArray[np.float64],
@@ -75,7 +76,6 @@ def solve(
         """
         ### Creation of the state and actuation models
 
-
         problem = crocoddyl.ShootingProblem(
             x0, self.runningModelList, self.terminalModel
         )
@@ -95,7 +95,7 @@ def solve(
 
         x_init = [x0] + x_warmstart
         u_init = u_warmstart
-        
+
         result = ocp.solve(x_init, u_init, OCPParamsCrocoBase.solver_iters)
 
         self.ocp_results = OCPResults(
diff --git a/agimus_controller/agimus_controller/ocp_param_base.py b/agimus_controller/agimus_controller/ocp_param_base.py
index 57eafdfc..04f147e5 100644
--- a/agimus_controller/agimus_controller/ocp_param_base.py
+++ b/agimus_controller/agimus_controller/ocp_param_base.py
@@ -2,6 +2,7 @@
 
 import numpy as np
 
+
 @dataclass
 class OCPParamsCrocoBase:
     """Input data structure of the OCP."""

From 58b8dc7d11a6bc1cb06b3e6357e28321878c595b Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Thu, 2 Jan 2025 11:48:03 +0000
Subject: [PATCH 28/95] changing OCPParam to ocp_param

---
 .../agimus_controller/ocp_base_croco.py              | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index c399666e..95841020 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -16,14 +16,14 @@ class OCPCrocoBase(OCPBase):
     def __init__(
         self,
         robot_model: RobotModelFactory,
-        OCPParams: OCPParamsCrocoBase,
+        ocp_params: OCPParamsCrocoBase,
     ) -> None:
         """Defines common behavior for all OCP using croccodyl. This is an abstract class with some helpers to design OCPs in a more friendly way.
 
         Args:
             rmodel (pin.Model): Robot model.
             cmodel (pin.GeometryModel): Collision Model of the robot.
-            OCPParams (OCPParamsBase): Input data structure of the OCP.
+            ocp_params (OCPParamsBase): Input data structure of the OCP.
         """
         # Setting the robot model
         self._robot_model = robot_model
@@ -36,17 +36,17 @@ def __init__(
         self._actuation = crocoddyl.ActuationModelFull(self._state)
 
         # Setting the OCP parameters
-        self._OCPParams = OCPParams
+        self._ocp_params = ocp_params
 
     @property
     def horizon_size(self) -> int:
         """Number of time steps in the horizon."""
-        return self._OCPParams.T
+        return self._ocp_params.T
 
     @property
     def dt(self) -> np.float64:
         """Integration step of the OCP."""
-        return self._OCPParams.dt
+        return self._ocp_params.dt
 
     @abstractmethod
     @property
@@ -83,7 +83,7 @@ def solve(
         ocp = mim_solvers.SolverCSQP(problem)
 
         # Merit function
-        ocp.use_filter_line_search = self._OCPParams.use_filter_line_search
+        ocp.use_filter_line_search = self._ocp_params.use_filter_line_search
 
         # Parameters of the solver
         ocp.termination_tolerance = OCPParamsCrocoBase.termination_tolerance

From 3faa492265afd8a22d15d4d9db8ac0cf085e4451 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Thu, 2 Jan 2025 11:56:02 +0000
Subject: [PATCH 29/95] adding few comments + changing the docstring for the
 init

---
 agimus_controller/agimus_controller/ocp_base_croco.py | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 95841020..75056b4b 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -21,8 +21,7 @@ def __init__(
         """Defines common behavior for all OCP using croccodyl. This is an abstract class with some helpers to design OCPs in a more friendly way.
 
         Args:
-            rmodel (pin.Model): Robot model.
-            cmodel (pin.GeometryModel): Collision Model of the robot.
+            robot_model (RobotModelFactory): All models of the robot.
             ocp_params (OCPParamsBase): Input data structure of the OCP.
         """
         # Setting the robot model
@@ -90,14 +89,16 @@ def solve(
         ocp.max_qp_iters = OCPParamsCrocoBase.qp_iters
         ocp.eps_abs = OCPParamsCrocoBase.eps_abs
         ocp.eps_rel = OCPParamsCrocoBase.eps_rel
-
         ocp.with_callbacks = OCPParamsCrocoBase.callbacks
 
+        # Creating the warmstart lists for the solver
         x_init = [x0] + x_warmstart
         u_init = u_warmstart
 
-        result = ocp.solve(x_init, u_init, OCPParamsCrocoBase.solver_iters)
+        # Solve the OCP
+        ocp.solve(x_init, u_init, OCPParamsCrocoBase.solver_iters)
 
+        # Store the results
         self.ocp_results = OCPResults(
             states=ocp.xs,
             ricatti_gains=ocp.K,

From 4e2373c08c93e7f93f0efe2186ca79676c4c3e24 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Thu, 2 Jan 2025 11:56:30 +0000
Subject: [PATCH 30/95] adding the filter linesearch data input in the
 dataclass ocp param croco

---
 agimus_controller/agimus_controller/ocp_param_base.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/agimus_controller/agimus_controller/ocp_param_base.py b/agimus_controller/agimus_controller/ocp_param_base.py
index 04f147e5..90d60e28 100644
--- a/agimus_controller/agimus_controller/ocp_param_base.py
+++ b/agimus_controller/agimus_controller/ocp_param_base.py
@@ -18,3 +18,4 @@ class OCPParamsCrocoBase:
     eps_abs: np.float64 = 1e-6  # Absolute tolerance of the solver
     eps_rel: np.float64 = 0  # Relative tolerance of the solver
     callbacks: bool = False  # Flag to enable/disable callbacks
+    use_filter_line_search = False # Flag to enable/disable the filter line searchs
\ No newline at end of file

From 9047baa9fb9f15e30d2921c50e0b6fdd682438a1 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Thu, 2 Jan 2025 15:00:35 +0000
Subject: [PATCH 31/95] unit testing the ocp param base dataclass

---
 .../tests/test_ocp_param_base.py              | 82 +++++++++++--------
 1 file changed, 48 insertions(+), 34 deletions(-)

diff --git a/agimus_controller/tests/test_ocp_param_base.py b/agimus_controller/tests/test_ocp_param_base.py
index 22299a24..ef273804 100644
--- a/agimus_controller/tests/test_ocp_param_base.py
+++ b/agimus_controller/tests/test_ocp_param_base.py
@@ -1,61 +1,75 @@
 import unittest
 import numpy as np
 
-<<<<<<< Updated upstream
-from agimus_controller.agimus_controller.ocp_param_base import OCPParamsCrocoBase
-from agimus_controller.agimus_controller.trajectory import (
-    WeightedTrajectoryPoint,
-    TrajectoryPoint,
-    TrajectoryPointWeights,
-)
-
-=======
 from agimus_controller.ocp_param_base import OCPParamsCrocoBase
-from agimus_controller.trajectory import WeightedTrajectoryPoint, TrajectoryPoint, TrajectoryPointWeights
->>>>>>> Stashed changes
 
 class TestOCPParamsCrocoBase(unittest.TestCase):
+    """
+    TestOCPParamsCrocoBase is a unit test class for testing the OCPParamsCrocoBase class.
+
+    Methods:
+        __init__(methodName="runTest"): Initializes the test case instance.
+        test_initialization(): Tests the initialization of the OCPParamsCrocoBase class with various parameters.
+    """
     """Test the OCPParamsCrocoBase class."""
 
     def __init__(self, methodName="runTest"):
         super().__init__(methodName)
 
     def test_initialization(self):
+        """
+        Test the initialization of the OCPParamsCrocoBase class.
+
+        This test verifies that the parameters passed to the OCPParamsCrocoBase
+        constructor are correctly assigned to the instance attributes.
+
+        Test Parameters:
+        - dt (float): Time step for the OCP solver.
+        - T (int): Total number of time steps.
+        - solver_iters (int): Number of solver iterations.
+        - qp_iters (int): Number of QP iterations.
+        - termination_tolerance (float): Tolerance for termination criteria.
+        - eps_abs (float): Absolute tolerance for the solver.
+        - eps_rel (float): Relative tolerance for the solver.
+        - callbacks (bool): Flag to enable or disable callbacks.
+
+        Assertions:
+        - Asserts that the instance attribute `dt` is equal to the input `dt`.
+        - Asserts that the instance attribute `T` is equal to the input `T`.
+        - Asserts that the instance attribute `qp_iters` is equal to the input `qp_iters`.
+        - Asserts that the instance attribute `solver_iters` is equal to the input `solver_iters`.
+        - Asserts that the instance attribute `termination_tolerance` is equal to the input `termination_tolerance`.
+        - Asserts that the instance attribute `eps_abs` is equal to the input `eps_abs`.
+        - Asserts that the instance attribute `eps_rel` is equal to the input `eps_rel`.
+        - Asserts that the instance attribute `callbacks` is False.
+        """
         dt = np.float64(0.01)
         T = 100
         solver_iters = 50
-        point = TrajectoryPoint(
-            robot_configuration=np.array([1.0, 2.0, 3.0], dtype=np.float64),
-        )
-        weights = TrajectoryPointWeights(
-            w_robot_configuration=np.array([1.0, 2.0, 3.0], dtype=np.float64),
-        )
-        weighted_trajectory_point = WeightedTrajectoryPoint(point, weights)
-        weighted_trajectory_points = [weighted_trajectory_point]
-        armature = np.array([1.0, 2.0, 3.0], dtype=np.float64)
-        ee_name = "end_effector"
-        p_target = np.array([0.0, 0.0, 0.0], dtype=np.float64)
-
+        qp_iters = 200
+        termination_tolerance = 1e-3
+        eps_abs = 1e-6
+        eps_rel = 0
+        callbacks = False
         params = OCPParamsCrocoBase(
             dt=dt,
             T=T,
             solver_iters=solver_iters,
-            WeightedTrajectoryPoints=weighted_trajectory_points,
-            armature=armature,
-            ee_name=ee_name,
+            qp_iters=qp_iters,
+            termination_tolerance= termination_tolerance,
+            eps_abs = eps_abs,
+            eps_rel = eps_rel,
+            callbacks = callbacks
         )
-
+        print("hello")
         self.assertEqual(params.dt, dt)
         self.assertEqual(params.T, T)
-        self.assertEqual(params.qp_iters, 200)
+        self.assertEqual(params.qp_iters, qp_iters)
         self.assertEqual(params.solver_iters, solver_iters)
-        self.assertEqual(params.termination_tolerance, 1e-3)
-        self.assertEqual(params.eps_abs, 1e-6)
-        self.assertEqual(params.eps_rel, 0)
+        self.assertEqual(params.termination_tolerance, termination_tolerance)
+        self.assertEqual(params.eps_abs, eps_abs)
+        self.assertEqual(params.eps_rel, eps_rel)
         self.assertFalse(params.callbacks)
-        self.assertEqual(params.WeightedTrajectoryPoints, weighted_trajectory_points)
-        self.assertTrue(np.array_equal(params.armature, armature))
-        self.assertEqual(params.ee_name, ee_name)
 
 
 if __name__ == "__main__":

From ebb28b79ee6ddfd400bddb2479e472fac96c813c Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Thu, 2 Jan 2025 15:18:50 +0000
Subject: [PATCH 32/95] deleting the ocp base unit test because that's only an
 abstract class

---
 agimus_controller/tests/test_ocp_base.py | 13 -------------
 1 file changed, 13 deletions(-)
 delete mode 100644 agimus_controller/tests/test_ocp_base.py

diff --git a/agimus_controller/tests/test_ocp_base.py b/agimus_controller/tests/test_ocp_base.py
deleted file mode 100644
index af02a0fa..00000000
--- a/agimus_controller/tests/test_ocp_base.py
+++ /dev/null
@@ -1,13 +0,0 @@
-import unittest
-
-from agimus_controller.ocp_base import OCPBase
-
-
-class TestOCPBase(unittest.TestCase):
-    def test_abstract_class_instantiation(self):
-        with self.assertRaises(TypeError):
-            OCPBase()
-
-
-if __name__ == "__main__":
-    unittest.main()

From 90afd0f93b7992b5c8e80c89d0f204f41a840657 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Thu, 2 Jan 2025 15:19:30 +0000
Subject: [PATCH 33/95] changing ocpparamcrocobase to ocpparambasecroco

---
 .../agimus_controller/ocp_base_croco.py       | 20 +++++++++----------
 .../agimus_controller/ocp_param_base.py       |  2 +-
 2 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 75056b4b..945ddde1 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -8,21 +8,21 @@
 
 from agimus_controller.mpc_data import OCPResults, OCPDebugData
 from agimus_controller.ocp_base import OCPBase
-from agimus_controller.ocp_param_base import OCPParamsCrocoBase
+from agimus_controller.ocp_param_base import OCPParamsBaseCroco
 from agimus_controller.factory.robot_model import RobotModelFactory
 
 
-class OCPCrocoBase(OCPBase):
+class OCPBaseCroco(OCPBase):
     def __init__(
         self,
         robot_model: RobotModelFactory,
-        ocp_params: OCPParamsCrocoBase,
+        ocp_params: OCPParamsBaseCroco,
     ) -> None:
         """Defines common behavior for all OCP using croccodyl. This is an abstract class with some helpers to design OCPs in a more friendly way.
 
         Args:
             robot_model (RobotModelFactory): All models of the robot.
-            ocp_params (OCPParamsBase): Input data structure of the OCP.
+            ocp_params (OCPParamsBaseCroco): Input data structure of the OCP.
         """
         # Setting the robot model
         self._robot_model = robot_model
@@ -85,18 +85,18 @@ def solve(
         ocp.use_filter_line_search = self._ocp_params.use_filter_line_search
 
         # Parameters of the solver
-        ocp.termination_tolerance = OCPParamsCrocoBase.termination_tolerance
-        ocp.max_qp_iters = OCPParamsCrocoBase.qp_iters
-        ocp.eps_abs = OCPParamsCrocoBase.eps_abs
-        ocp.eps_rel = OCPParamsCrocoBase.eps_rel
-        ocp.with_callbacks = OCPParamsCrocoBase.callbacks
+        ocp.termination_tolerance = self._ocp_params.termination_tolerance
+        ocp.max_qp_iters = self._ocp_params.qp_iters
+        ocp.eps_abs = self._ocp_params.eps_abs
+        ocp.eps_rel = self._ocp_params.eps_rel
+        ocp.with_callbacks = self._ocp_params.callbacks
 
         # Creating the warmstart lists for the solver
         x_init = [x0] + x_warmstart
         u_init = u_warmstart
 
         # Solve the OCP
-        ocp.solve(x_init, u_init, OCPParamsCrocoBase.solver_iters)
+        ocp.solve(x_init, u_init, self._ocp_params.solver_iters)
 
         # Store the results
         self.ocp_results = OCPResults(
diff --git a/agimus_controller/agimus_controller/ocp_param_base.py b/agimus_controller/agimus_controller/ocp_param_base.py
index 90d60e28..f0be5adc 100644
--- a/agimus_controller/agimus_controller/ocp_param_base.py
+++ b/agimus_controller/agimus_controller/ocp_param_base.py
@@ -4,7 +4,7 @@
 
 
 @dataclass
-class OCPParamsCrocoBase:
+class OCPParamsBaseCroco:
     """Input data structure of the OCP."""
 
     # Data relevant to solve the OCP

From 1fb5f380f8fd5ca58769581898f1d5f4e21ba07f Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Thu, 2 Jan 2025 15:23:27 +0000
Subject: [PATCH 34/95] wrong placement of problem solved

---
 agimus_controller/agimus_controller/mpc_data.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/agimus_controller/agimus_controller/mpc_data.py b/agimus_controller/agimus_controller/mpc_data.py
index a01e0a94..b08047f5 100644
--- a/agimus_controller/agimus_controller/mpc_data.py
+++ b/agimus_controller/agimus_controller/mpc_data.py
@@ -16,14 +16,14 @@ class OCPResults:
 
 @dataclass
 class OCPDebugData:
-    # Solver infos
-    problem_solved: bool = False
 
     # Debug data
     result: list[TrajectoryPoint]
     references: list[TrajectoryPoint]
     kkt_norms: list[np.float64]
     collision_distance_residuals: list[dict[np.float64]]
+    # Solver infos
+    problem_solved: bool = False
 
 
 @dataclass

From c544c12c338b9161951e37691b46acfd461afd8a Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Thu, 2 Jan 2025 15:24:33 +0000
Subject: [PATCH 35/95] changing ocpparamcrocobase to ocpparambasecroco

---
 agimus_controller/tests/test_ocp_param_base.py | 15 +++++++--------
 1 file changed, 7 insertions(+), 8 deletions(-)

diff --git a/agimus_controller/tests/test_ocp_param_base.py b/agimus_controller/tests/test_ocp_param_base.py
index ef273804..8aba83c4 100644
--- a/agimus_controller/tests/test_ocp_param_base.py
+++ b/agimus_controller/tests/test_ocp_param_base.py
@@ -1,26 +1,26 @@
 import unittest
 import numpy as np
 
-from agimus_controller.ocp_param_base import OCPParamsCrocoBase
+from agimus_controller.ocp_param_base import OCPParamsBaseCroco
 
 class TestOCPParamsCrocoBase(unittest.TestCase):
     """
-    TestOCPParamsCrocoBase is a unit test class for testing the OCPParamsCrocoBase class.
+    TestOCPParamsCrocoBase is a unit test class for testing the OCPParamsBaseCroco class.
 
     Methods:
         __init__(methodName="runTest"): Initializes the test case instance.
-        test_initialization(): Tests the initialization of the OCPParamsCrocoBase class with various parameters.
+        test_initialization(): Tests the initialization of the OCPParamsBaseCroco class with various parameters.
     """
-    """Test the OCPParamsCrocoBase class."""
+    """Test the OCPParamsBaseCroco class."""
 
     def __init__(self, methodName="runTest"):
         super().__init__(methodName)
 
     def test_initialization(self):
         """
-        Test the initialization of the OCPParamsCrocoBase class.
+        Test the initialization of the OCPParamsBaseCroco class.
 
-        This test verifies that the parameters passed to the OCPParamsCrocoBase
+        This test verifies that the parameters passed to the OCPParamsBaseCroco
         constructor are correctly assigned to the instance attributes.
 
         Test Parameters:
@@ -51,7 +51,7 @@ def test_initialization(self):
         eps_abs = 1e-6
         eps_rel = 0
         callbacks = False
-        params = OCPParamsCrocoBase(
+        params = OCPParamsBaseCroco(
             dt=dt,
             T=T,
             solver_iters=solver_iters,
@@ -61,7 +61,6 @@ def test_initialization(self):
             eps_rel = eps_rel,
             callbacks = callbacks
         )
-        print("hello")
         self.assertEqual(params.dt, dt)
         self.assertEqual(params.T, T)
         self.assertEqual(params.qp_iters, qp_iters)

From e422536978e1e0b17359c89faf0350a2b0cc8dd0 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Thu, 2 Jan 2025 15:30:46 +0000
Subject: [PATCH 36/95] switching from ... to pass for abstract class because
 it explodes otherwise

---
 .../agimus_controller/ocp_base.py             | 20 +++++++++----------
 .../agimus_controller/ocp_base_croco.py       |  6 +++---
 2 files changed, 13 insertions(+), 13 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base.py b/agimus_controller/agimus_controller/ocp_base.py
index 1f6e545f..7f920285 100644
--- a/agimus_controller/agimus_controller/ocp_base.py
+++ b/agimus_controller/agimus_controller/ocp_base.py
@@ -21,27 +21,27 @@ def set_reference_horizon(
         self, reference_trajectory: list[WeightedTrajectoryPoint]
     ) -> None:
         """Set the reference trajectory and the weights of the costs for the OCP solver. This method should be implemented by the derived class."""
-        ...
+        pass
 
-    @abstractmethod
     @property
+    @abstractmethod
     def horizon_size() -> int:
         """Returns the horizon size of the OCP.
 
         Returns:
             int: size of the horizon.
         """
-        ...
+        pass
 
-    @abstractmethod
     @property
+    @abstractmethod
     def dt() -> int:
         """Returns the time step of the OCP.
 
         Returns:
             int: time step of the OCP.
         """
-        ...
+        pass
 
     @abstractmethod
     def solve(
@@ -58,10 +58,10 @@ def solve(
             x_warmstart (list[npt.NDArray[np.float64]]): Warmstart values for the state. This doesn't include the current state.
             u_warmstart (list[npt.NDArray[np.float64]]): Warmstart values for the control inputs.
         """
-        ...
+        pass
 
-    @abstractmethod
     @property
+    @abstractmethod
     def ocp_results(self) -> OCPResults:
         """Returns the results of the OCP solver.
         The solve method should be called before calling this method.
@@ -69,14 +69,14 @@ def ocp_results(self) -> OCPResults:
         Returns:
             OCPResults: Class containing the results of the OCP solver.
         """
-        ...
+        pass
 
-    @abstractmethod
     @property
+    @abstractmethod
     def debug_data(self) -> OCPDebugData:
         """Returns the debug data of the OCP solver.
 
         Returns:
             OCPDebugData: Class containing the debug data of the OCP solver.
         """
-        ...
+        pass
diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 945ddde1..d36e2fee 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -51,13 +51,13 @@ def dt(self) -> np.float64:
     @property
     def runningModelList(self) -> list[crocoddyl.ActionModelAbstract]:
         """List of running models."""
-        ...
+        pass
 
     @abstractmethod
     @property
     def terminalModel(self) -> crocoddyl.ActionModelAbstract:
         """Terminal model."""
-        ...
+        pass
 
     def solve(
         self,
@@ -116,4 +116,4 @@ def ocp_results(self) -> OCPResults:
 
     @property
     def debug_data(self) -> OCPDebugData:
-        ...
+        pass

From fa3da6ea6c323543f706494630f3eacb773d645d Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Thu, 2 Jan 2025 15:38:31 +0000
Subject: [PATCH 37/95] fixing the union problem

---
 .../agimus_controller/factory/robot_model.py   | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/agimus_controller/agimus_controller/factory/robot_model.py b/agimus_controller/agimus_controller/factory/robot_model.py
index 43efd76b..22285525 100644
--- a/agimus_controller/agimus_controller/factory/robot_model.py
+++ b/agimus_controller/agimus_controller/factory/robot_model.py
@@ -1,4 +1,4 @@
-from dataclasses import dataclass
+from dataclasses import dataclass, field
 from copy import deepcopy
 from pathlib import Path
 from typing import Union
@@ -10,14 +10,14 @@
 
 @dataclass
 class RobotModelParameters:
-    q0_name = str()
-    free_flyer = False
-    locked_joint_names = []
-    urdf = Path() | str
-    srdf = Path() | str
-    collision_as_capsule = False
-    self_collision = False
-    armature = npt.NDArray[np.float64]
+    q0_name: str = ""
+    free_flyer: bool = False
+    locked_joint_names: list[str] = field(default_factory=list)  # Default empty list
+    urdf: Path | str = ""
+    srdf: Path | str = ""
+    collision_as_capsule: bool = False
+    self_collision: bool = False
+    armature: npt.NDArray[np.float64] = field(default_factory=lambda: np.array([]))  # Default empty NumPy array
 
 
 class RobotModelFactory:

From 5c8a53e39def376705ea8c4f79ca1582be573591 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Thu, 2 Jan 2025 15:38:52 +0000
Subject: [PATCH 38/95] fixing the property

---
 agimus_controller/agimus_controller/ocp_base_croco.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index d36e2fee..03146a4a 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -47,14 +47,14 @@ def dt(self) -> np.float64:
         """Integration step of the OCP."""
         return self._ocp_params.dt
 
-    @abstractmethod
     @property
+    @abstractmethod
     def runningModelList(self) -> list[crocoddyl.ActionModelAbstract]:
         """List of running models."""
         pass
 
-    @abstractmethod
     @property
+    @abstractmethod
     def terminalModel(self) -> crocoddyl.ActionModelAbstract:
         """Terminal model."""
         pass

From 038cd0fed31839f4cdcc95342312a95f43eac8d3 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Thu, 2 Jan 2025 16:03:21 +0000
Subject: [PATCH 39/95] first unit test passing, now working on the simple
 example as unit test

---
 .../agimus_controller/ocp_base_croco.py       |   1 -
 .../tests/test_ocp_croco_base.py              | 160 +++++++++++-------
 2 files changed, 99 insertions(+), 62 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 03146a4a..00479bc3 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -4,7 +4,6 @@
 import mim_solvers
 import numpy as np
 import numpy.typing as npt
-import pinocchio as pin
 
 from agimus_controller.mpc_data import OCPResults, OCPDebugData
 from agimus_controller.ocp_base import OCPBase
diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index f0395500..976b8527 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -1,75 +1,113 @@
 import unittest
+from unittest.mock import MagicMock, patch
 
-import numpy as np
 import example_robot_data as robex
+import numpy as np
 
-from agimus_controller.ocp_base_croco import OCPCrocoBase
-from agimus_controller.ocp_param_base import OCPParamsCrocoBase
+from agimus_controller.ocp_base_croco import OCPBaseCroco
+from agimus_controller.ocp_param_base import OCPParamsBaseCroco
+from agimus_controller.factory.robot_model import RobotModelFactory
 
+class TestOCPBaseCroco(unittest.TestCase):
+    def setUp(self):
+        
+        # Mock the RobotModelFactory and OCPParamsCrocoBase
+        self.mock_robot_model_factory = RobotModelFactory()
+        
+        mock_robot = robex.load('panda')
+        mock_robot_model = mock_robot.model
+        mock_collision_model = mock_robot.collision_model
+        mock_armature = np.array([])
+        
+        self.mock_robot_model_factory._rmodel = mock_robot_model
+        self.mock_robot_model_factory._complete_collision_model = mock_collision_model
+        self.mock_robot_model_factory._armature = mock_armature
+        
+        self.mock_ocp_params = MagicMock(spec=OCPParamsBaseCroco)
+        
+        # Set mock parameters
+        self.mock_ocp_params.T = 10
+        self.mock_ocp_params.dt = 0.1
+        self.mock_ocp_params.use_filter_line_search = True
+        self.mock_ocp_params.termination_tolerance = 1e-6
+        self.mock_ocp_params.qp_iters = 10
+        self.mock_ocp_params.eps_abs = 1e-8
+        self.mock_ocp_params.eps_rel = 1e-6
+        self.mock_ocp_params.callbacks = True
+        self.mock_ocp_params.solver_iters = 100
 
-class TestOCPCrocoBase(unittest.TestCase):
+        # Create a concrete implementation of OCPBaseCroco
+        class TestOCPCroco(OCPBaseCroco):
+            @property
+            def runningModelList(self):
+                return None
 
-    def __init__(self, *args, **kwargs):
-        pass
-
-    def setUp(self):
-        """Set up the test environment."""
-        # Load the robot
-        self.robot = robex.load('example_robot_description')
-        self.rmodel = self.robot.model
-        self.cmodel = self.robot.collision_model
-
-        # Set some fixed params for unit testing
-        self.dt = 0.01
-        self.T = 100
-        self.qp_iters = 200
-        self.solve_iters = 200
-        self.callbacks = True
-        self.x0 = np.zeros(self.rmodel.nq + self.rmodel.nv)
-        self.OCPParams = OCPParamsCrocoBase(
-            dt = self.dt,
-            T = self.T,
-            qp_iters=self.qp_iters,
-            solver_iters=self.solve_iters,
-            callbacks=self.callbacks,
-            ...,
-        )
-        return super().setUp()
-
-    def test_abstract_class_instantiation(self):
-        """Test the instantiation of the OCPCrocoBase class."""
-        with self.assertRaises(TypeError):
-            OCPCrocoBase(self.rmodel, self.cmodel, self.OCPParams)
+            @property
+            def terminalModel(self):
+                return None
+            
+            def set_reference_horizon(self, horizon_size):
+                return None
 
+        
+        self.ocp = TestOCPCroco(self.mock_robot_model_factory, self.mock_ocp_params)
+    
     def test_horizon_size(self):
-        """Test the horizon_size property of the OCPCrocoBase class."""
-        ocp = OCPCrocoBase(self.rmodel, self.cmodel, self.OCPParams)
-        self.assertEqual(ocp.horizon_size, self.T)
+        """Test the horizon_size property."""
+        self.assertEqual(self.ocp.horizon_size, self.mock_ocp_params.T)
 
     def test_dt(self):
-        """Test the dt property of the OCPCrocoBase class."""
-        ocp = OCPCrocoBase(self.rmodel, self.cmodel, self.OCPParams)
-        self.assertEqual(ocp.dt, self.dt)
-
-    def test_x0(self):
-        """Test the x0 property of the OCPCrocoBase class."""
-        ocp = OCPCrocoBase(self.rmodel, self.cmodel, self.OCPParams)
-        self.assertTrue(np.array_equal(ocp.x0, self.x0))
-
-    def test_x_init(self):
-        """Test the x_init method of the OCPCrocoBase class."""
-        ocp = OCPCrocoBase(self.rmodel, self.cmodel, self.OCPParams)
-        x_init = ocp.x_init()
-        self.assertEqual(len(x_init), self.T)
-        self.assertTrue(np.array_equal(x_init[0], self.x0))
-
-    def test_u_init(self):
-        """Test the u_init method of the OCPCrocoBase class."""
-        ocp = OCPCrocoBase(self.rmodel, self.cmodel, self.OCPParams)
-        u_init = ocp.u_init()
-        self.assertEqual(len(u_init), self.T - 1)
-
-    # def test_solve(self):
+        """Test the dt property."""
+        self.assertAlmostEqual(self.ocp.dt, self.mock_ocp_params.dt)
+
+
+class TestSimpleOCPCroco(unittest.TestCase):
+    def setUp(self):
+        
+        # Mock the RobotModelFactory and OCPParamsCrocoBase
+        self.robot_model_factory = RobotModelFactory()
+        
+        robot = robex.load('panda')
+        robot_model = robot.model
+        collision_model = robot.collision_model
+        armature = np.array([])
+        
+        self.robot_model_factory._rmodel = robot_model
+        self.robot_model_factory._complete_collision_model = collision_model
+        self.robot_model_factory.armature = armature
+        
+        self.ocp_params = OCPParamsBaseCroco()
+        
+        # Set mock parameters
+        self.ocp_params.T = 10
+        self.ocp_params.dt = 0.1
+        self.ocp_params.use_filter_line_search = True
+        self.ocp_params.termination_tolerance = 1e-6
+        self.ocp_params.qp_iters = 10
+        self.ocp_params.eps_abs = 1e-8
+        self.ocp_params.eps_rel = 1e-6
+        self.ocp_params.callbacks = True
+        self.ocp_params.solver_iters = 100
+
+
+        # Create a concrete implementation of OCPBaseCroco
+        class TestOCPCroco(OCPBaseCroco):
+            @property
+            def runningModelList(self):
+                return None
+
+            @property
+            def terminalModel(self):
+                return None
+            
+            def set_reference_horizon(self, horizon_size):
+                ### Not implemented in this OCP example.
+                return None
+
+        
+        self.ocp = TestOCPCroco(self.robot_model_factory, self.ocp_params)
+
+
 
 if __name__ == '__main__':
     unittest.main()

From 8ce22ad21468c754bc4c6d22c7e409cfcbf675c6 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Thu, 2 Jan 2025 16:03:40 +0000
Subject: [PATCH 40/95] [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
---
 .../agimus_controller/factory/robot_model.py  |  4 ++-
 .../agimus_controller/mpc_data.py             |  1 -
 .../agimus_controller/ocp_param_base.py       |  2 +-
 .../tests/test_ocp_croco_base.py              | 35 ++++++++-----------
 .../tests/test_ocp_param_base.py              | 10 +++---
 5 files changed, 25 insertions(+), 27 deletions(-)

diff --git a/agimus_controller/agimus_controller/factory/robot_model.py b/agimus_controller/agimus_controller/factory/robot_model.py
index 22285525..43a41124 100644
--- a/agimus_controller/agimus_controller/factory/robot_model.py
+++ b/agimus_controller/agimus_controller/factory/robot_model.py
@@ -17,7 +17,9 @@ class RobotModelParameters:
     srdf: Path | str = ""
     collision_as_capsule: bool = False
     self_collision: bool = False
-    armature: npt.NDArray[np.float64] = field(default_factory=lambda: np.array([]))  # Default empty NumPy array
+    armature: npt.NDArray[np.float64] = field(
+        default_factory=lambda: np.array([])
+    )  # Default empty NumPy array
 
 
 class RobotModelFactory:
diff --git a/agimus_controller/agimus_controller/mpc_data.py b/agimus_controller/agimus_controller/mpc_data.py
index b08047f5..a8ae345f 100644
--- a/agimus_controller/agimus_controller/mpc_data.py
+++ b/agimus_controller/agimus_controller/mpc_data.py
@@ -16,7 +16,6 @@ class OCPResults:
 
 @dataclass
 class OCPDebugData:
-
     # Debug data
     result: list[TrajectoryPoint]
     references: list[TrajectoryPoint]
diff --git a/agimus_controller/agimus_controller/ocp_param_base.py b/agimus_controller/agimus_controller/ocp_param_base.py
index f0be5adc..026bb567 100644
--- a/agimus_controller/agimus_controller/ocp_param_base.py
+++ b/agimus_controller/agimus_controller/ocp_param_base.py
@@ -18,4 +18,4 @@ class OCPParamsBaseCroco:
     eps_abs: np.float64 = 1e-6  # Absolute tolerance of the solver
     eps_rel: np.float64 = 0  # Relative tolerance of the solver
     callbacks: bool = False  # Flag to enable/disable callbacks
-    use_filter_line_search = False # Flag to enable/disable the filter line searchs
\ No newline at end of file
+    use_filter_line_search = False  # Flag to enable/disable the filter line searchs
diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index 976b8527..7fc9e358 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -8,23 +8,23 @@
 from agimus_controller.ocp_param_base import OCPParamsBaseCroco
 from agimus_controller.factory.robot_model import RobotModelFactory
 
+
 class TestOCPBaseCroco(unittest.TestCase):
     def setUp(self):
-        
         # Mock the RobotModelFactory and OCPParamsCrocoBase
         self.mock_robot_model_factory = RobotModelFactory()
-        
-        mock_robot = robex.load('panda')
+
+        mock_robot = robex.load("panda")
         mock_robot_model = mock_robot.model
         mock_collision_model = mock_robot.collision_model
         mock_armature = np.array([])
-        
+
         self.mock_robot_model_factory._rmodel = mock_robot_model
         self.mock_robot_model_factory._complete_collision_model = mock_collision_model
         self.mock_robot_model_factory._armature = mock_armature
-        
+
         self.mock_ocp_params = MagicMock(spec=OCPParamsBaseCroco)
-        
+
         # Set mock parameters
         self.mock_ocp_params.T = 10
         self.mock_ocp_params.dt = 0.1
@@ -45,13 +45,12 @@ def runningModelList(self):
             @property
             def terminalModel(self):
                 return None
-            
+
             def set_reference_horizon(self, horizon_size):
                 return None
 
-        
         self.ocp = TestOCPCroco(self.mock_robot_model_factory, self.mock_ocp_params)
-    
+
     def test_horizon_size(self):
         """Test the horizon_size property."""
         self.assertEqual(self.ocp.horizon_size, self.mock_ocp_params.T)
@@ -63,21 +62,20 @@ def test_dt(self):
 
 class TestSimpleOCPCroco(unittest.TestCase):
     def setUp(self):
-        
         # Mock the RobotModelFactory and OCPParamsCrocoBase
         self.robot_model_factory = RobotModelFactory()
-        
-        robot = robex.load('panda')
+
+        robot = robex.load("panda")
         robot_model = robot.model
         collision_model = robot.collision_model
         armature = np.array([])
-        
+
         self.robot_model_factory._rmodel = robot_model
         self.robot_model_factory._complete_collision_model = collision_model
         self.robot_model_factory.armature = armature
-        
+
         self.ocp_params = OCPParamsBaseCroco()
-        
+
         # Set mock parameters
         self.ocp_params.T = 10
         self.ocp_params.dt = 0.1
@@ -89,7 +87,6 @@ def setUp(self):
         self.ocp_params.callbacks = True
         self.ocp_params.solver_iters = 100
 
-
         # Create a concrete implementation of OCPBaseCroco
         class TestOCPCroco(OCPBaseCroco):
             @property
@@ -99,15 +96,13 @@ def runningModelList(self):
             @property
             def terminalModel(self):
                 return None
-            
+
             def set_reference_horizon(self, horizon_size):
                 ### Not implemented in this OCP example.
                 return None
 
-        
         self.ocp = TestOCPCroco(self.robot_model_factory, self.ocp_params)
 
 
-
-if __name__ == '__main__':
+if __name__ == "__main__":
     unittest.main()
diff --git a/agimus_controller/tests/test_ocp_param_base.py b/agimus_controller/tests/test_ocp_param_base.py
index 8aba83c4..bcb35b58 100644
--- a/agimus_controller/tests/test_ocp_param_base.py
+++ b/agimus_controller/tests/test_ocp_param_base.py
@@ -3,6 +3,7 @@
 
 from agimus_controller.ocp_param_base import OCPParamsBaseCroco
 
+
 class TestOCPParamsCrocoBase(unittest.TestCase):
     """
     TestOCPParamsCrocoBase is a unit test class for testing the OCPParamsBaseCroco class.
@@ -11,6 +12,7 @@ class TestOCPParamsCrocoBase(unittest.TestCase):
         __init__(methodName="runTest"): Initializes the test case instance.
         test_initialization(): Tests the initialization of the OCPParamsBaseCroco class with various parameters.
     """
+
     """Test the OCPParamsBaseCroco class."""
 
     def __init__(self, methodName="runTest"):
@@ -56,10 +58,10 @@ def test_initialization(self):
             T=T,
             solver_iters=solver_iters,
             qp_iters=qp_iters,
-            termination_tolerance= termination_tolerance,
-            eps_abs = eps_abs,
-            eps_rel = eps_rel,
-            callbacks = callbacks
+            termination_tolerance=termination_tolerance,
+            eps_abs=eps_abs,
+            eps_rel=eps_rel,
+            callbacks=callbacks,
         )
         self.assertEqual(params.dt, dt)
         self.assertEqual(params.T, T)

From 156afd8e544f4bb791ccfc6079309ae630076056 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Mon, 6 Jan 2025 12:04:38 +0000
Subject: [PATCH 41/95] addind setters for ocp base croco & robot model

---
 .../agimus_controller/factory/robot_model.py      | 10 ++++++++++
 .../agimus_controller/ocp_base_croco.py           | 15 +++++++++++++--
 2 files changed, 23 insertions(+), 2 deletions(-)

diff --git a/agimus_controller/agimus_controller/factory/robot_model.py b/agimus_controller/agimus_controller/factory/robot_model.py
index 43a41124..a02f3a4c 100644
--- a/agimus_controller/agimus_controller/factory/robot_model.py
+++ b/agimus_controller/agimus_controller/factory/robot_model.py
@@ -108,3 +108,13 @@ def armature(self) -> npt.NDArray[np.float64]:
             npt.NDArray[np.float64]: Armature of the robot.
         """
         return self._params.armature
+
+
+    @armature.setter
+    def armature(self, value: npt.NDArray[np.float64]) -> None:
+        """Set the armature of the robot.
+
+        Args:
+            value (npt.NDArray[np.float64]): New armature values to set.
+        """
+        self._params.armature = value
\ No newline at end of file
diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 00479bc3..4f2d1665 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -36,6 +36,8 @@ def __init__(
         # Setting the OCP parameters
         self._ocp_params = ocp_params
 
+        self._ocp_results = None
+
     @property
     def horizon_size(self) -> int:
         """Number of time steps in the horizon."""
@@ -109,9 +111,18 @@ def ocp_results(self) -> OCPResults:
         """Output data structure of the OCP.
 
         Returns:
-            OCPResults: Output data structure of the OCP. It contains the states, Ricatti gains and feed-forward terms.
+            OCPResults: Output data structure of the OCP. It contains the states, Ricatti gains, and feed-forward terms.
+        """
+        return self._ocp_results
+
+    @ocp_results.setter
+    def ocp_results(self, value: OCPResults) -> None:
+        """Set the output data structure of the OCP.
+
+        Args:
+            value (OCPResults): New output data structure of the OCP.
         """
-        return self.ocp_results
+        self._ocp_results = value
 
     @property
     def debug_data(self) -> OCPDebugData:

From f6c8efbe34cef726d5639e391e6d3cd963b7de7a Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Mon, 6 Jan 2025 12:29:12 +0000
Subject: [PATCH 42/95] adding the unit test for the simple ocp

---
 .../tests/test_ocp_croco_base.py              | 164 ++++++++++++++----
 1 file changed, 134 insertions(+), 30 deletions(-)

diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index 7fc9e358..1b304f51 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -1,8 +1,10 @@
 import unittest
-from unittest.mock import MagicMock, patch
+from unittest.mock import MagicMock
 
+import crocoddyl
 import example_robot_data as robex
 import numpy as np
+import pinocchio as pin
 
 from agimus_controller.ocp_base_croco import OCPBaseCroco
 from agimus_controller.ocp_param_base import OCPParamsBaseCroco
@@ -61,6 +63,103 @@ def test_dt(self):
 
 
 class TestSimpleOCPCroco(unittest.TestCase):
+    
+    class TestOCPCroco(OCPBaseCroco):
+        @property
+        def runningModelList(self):
+            # Running cost model
+            runningCostModel = crocoddyl.CostModelSum(self._state)
+
+            ### Creation of cost terms
+            # State Regularization cost
+            xResidual = crocoddyl.ResidualModelState(
+                self._state, np.concatenate((pin.neutral( self._rmodel), np.zeros( self._rmodel.nv))) 
+            )
+            xRegCost = crocoddyl.CostModelResidual(self._state, xResidual)
+
+            # Control Regularization cost
+            uResidual = crocoddyl.ResidualModelControl(self._state)
+            uRegCost = crocoddyl.CostModelResidual(self._state, uResidual)
+
+            # End effector frame cost
+            framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
+                self._state,
+                self._rmodel.getFrameId(
+                    "panda_hand_tcp"
+                ),
+                pin.SE3(np.eye(3),np.array([1.0, 1.0, 1.0])),
+            )
+
+            goalTrackingCost = crocoddyl.CostModelResidual(
+                self._state, framePlacementResidual
+            )
+            runningCostModel.addCost("stateReg", xRegCost, 0.1)
+            runningCostModel.addCost("ctrlRegGrav", uRegCost, 0.0001)
+            runningCostModel.addCost("gripperPoseRM", goalTrackingCost, 1.0)
+            # Create Differential Action Model (DAM), i.e. continuous dynamics and cost functions
+            running_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics(
+                self._state,
+                self._actuation,
+                runningCostModel,
+            )
+            runningModel = crocoddyl.IntegratedActionModelEuler(
+                running_DAM,
+            )
+            runningModel.differential.armature = (
+                self._robot_model.armature
+            )
+
+            runningModelList = [runningModel] * (self._ocp_params.T-1)
+            return runningModelList
+
+        @property
+        def terminalModel(self):
+
+            # Terminal cost models
+            terminalCostModel = crocoddyl.CostModelSum(self._state)
+
+            ### Creation of cost terms
+            # State Regularization cost
+            xResidual = crocoddyl.ResidualModelState(
+                self._state, np.concatenate((pin.neutral( self._rmodel), np.zeros( self._rmodel.nv))) 
+            )
+            xRegCost = crocoddyl.CostModelResidual(self._state, xResidual)
+
+            # End effector frame cost
+            framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
+                self._state,
+                self._rmodel.getFrameId(
+                    "panda_hand_tcp"
+                ),
+                pin.SE3(np.eye(3),np.array([1.0, 1.0, 1.0])),
+            )
+
+            goalTrackingCost = crocoddyl.CostModelResidual(
+                self._state, framePlacementResidual
+            )
+
+            terminalCostModel.addCost("stateReg", xRegCost, 0.1)
+            terminalCostModel.addCost("gripperPose", goalTrackingCost, 50)
+
+            # Create Differential Action Model (DAM), i.e. continuous dynamics and cost functions
+            terminal_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics(
+                self._state,
+                self._actuation,
+                terminalCostModel,
+            )
+
+            terminalModel = crocoddyl.IntegratedActionModelEuler(terminal_DAM, 0.0)
+            terminalModel.differential.armature = (
+                self._robot_model.armature
+            )
+
+            return terminalModel
+
+        def set_reference_horizon(self, horizon_size):
+            ### Not implemented in this OCP example.
+            return None
+
+    
     def setUp(self):
         # Mock the RobotModelFactory and OCPParamsCrocoBase
         self.robot_model_factory = RobotModelFactory()
@@ -68,41 +167,46 @@ def setUp(self):
         robot = robex.load("panda")
         robot_model = robot.model
         collision_model = robot.collision_model
-        armature = np.array([])
+        armature = np.full(robot_model.nq, 0.1)
 
         self.robot_model_factory._rmodel = robot_model
         self.robot_model_factory._complete_collision_model = collision_model
         self.robot_model_factory.armature = armature
 
-        self.ocp_params = OCPParamsBaseCroco()
-
         # Set mock parameters
-        self.ocp_params.T = 10
-        self.ocp_params.dt = 0.1
-        self.ocp_params.use_filter_line_search = True
-        self.ocp_params.termination_tolerance = 1e-6
-        self.ocp_params.qp_iters = 10
-        self.ocp_params.eps_abs = 1e-8
-        self.ocp_params.eps_rel = 1e-6
-        self.ocp_params.callbacks = True
-        self.ocp_params.solver_iters = 100
-
+        self.ocp_params = OCPParamsBaseCroco(dt=0.1, T=10, solver_iters=100, callbacks=True)
+        self.state_reg = np.concatenate((pin.neutral(robot_model), np.zeros(robot_model.nv))) 
+        self.state_warmstart = [np.zeros(robot_model.nq + robot_model.nv)] * (self.ocp_params.T - 1) # The first state is the current state
+        self.control_warmstart = [np.zeros(robot_model.nq)] * (self.ocp_params.T-1)
         # Create a concrete implementation of OCPBaseCroco
-        class TestOCPCroco(OCPBaseCroco):
-            @property
-            def runningModelList(self):
-                return None
-
-            @property
-            def terminalModel(self):
-                return None
-
-            def set_reference_horizon(self, horizon_size):
-                ### Not implemented in this OCP example.
-                return None
-
-        self.ocp = TestOCPCroco(self.robot_model_factory, self.ocp_params)
-
-
+        self.ocp = self.TestOCPCroco(self.robot_model_factory, self.ocp_params)
+        self.ocp.solve(self.state_reg, self.state_warmstart, self.control_warmstart)
+        # self.save_results()
+        
+    def save_results(self):
+        results = np.array(
+            [
+                self.ocp.ocp_results.states.tolist(),
+                self.ocp.ocp_results.ricatti_gains.tolist(),
+                self.ocp.ocp_results.feed_forward_terms.tolist(),
+            ],
+        dtype=object,  # Ensure the array is dtype=object before saving
+    )        
+        np.save("ressources/simple_ocp_croco_results.npy", results,)
+
+    def test_check_results(self):
+        results = np.load("ressources/simple_ocp_croco_results.npy", allow_pickle=True)
+        # Checking the states
+        for iter, state in enumerate(results[0]):
+            np.testing.assert_array_almost_equal(state, self.ocp.ocp_results.states.tolist()[iter])
+        
+        # Checking the ricatti gains
+        for iter, gain in enumerate(results[1]):
+            np.testing.assert_array_almost_equal(gain, self.ocp.ocp_results.ricatti_gains.tolist()[iter])
+
+        # Checking the feed forward terms
+        for iter, term in enumerate(results[2]):
+            np.testing.assert_array_almost_equal(term, self.ocp.ocp_results.feed_forward_terms.tolist()[iter])
+            
 if __name__ == "__main__":
     unittest.main()

From 982f9768097bf334d2849d45fe2644dcf17d7db4 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Mon, 6 Jan 2025 12:31:19 +0000
Subject: [PATCH 43/95] adding the npy results for the simple ocp used for the
 unit testing

---
 .../ressources/simple_ocp_croco_results.npy     | Bin 0 -> 15149 bytes
 1 file changed, 0 insertions(+), 0 deletions(-)
 create mode 100644 agimus_controller/tests/ressources/simple_ocp_croco_results.npy

diff --git a/agimus_controller/tests/ressources/simple_ocp_croco_results.npy b/agimus_controller/tests/ressources/simple_ocp_croco_results.npy
new file mode 100644
index 0000000000000000000000000000000000000000..1f976f0b0c7665feeeed50de40040caa08966b6a
GIT binary patch
literal 15149
zcmd7YX*5-TxWI8!$WSRmrZOaRhBC)~WS$~Ih|HxjY>rHkdCHVA(Z7;V8e}RZzn@T2
zQX!QR4JuKT3?<!a(^~iSeRIzX-{r9O^EvCR_3U-d-Z@)sEvy{q=?>6ENP2n&c?3#o
zibx)`l#~;Z^!5*=1-kj|@(=X%`t!7*+x{T0-_Hl_aSQPJ{kzmgIcYhONRj``gDslT
z!!Ou3AZ&w&f1uX}-{Ac;AGg3jw=lL{fnFZ|enGUrU=JD_Nk{z`o1Z83SCZb0E`emw
zqjx76lSn3Oa*31~y%}Qy$-MB7p0uz4FE)~82Ll})ouxX-nv$GEF104vq|6w1(Eon!
zAj!UU>sHfWzaM{pm=QFR!#%~B<dkwImr3oR+w*tC@;$$gzrvd2`g8o13FOK>)FHjq
z$yI+2$!$&YEFN+*$@{+#Nk=33+*9<jevkWq^&nT%NPhRlmniV(B??-TLJKcZcn{BS
zb99#ZjGMD9VjE~W&gdIYJa@5D%eN^;M;+g@X5amfu;bHo_fYmlYx*@<)Z6L_#jfPm
zexX{VR~e?hjpnmhLbxsbywZzF)aS{O9Q;KbN)IwspFFFD!{1jI-&?5z9oI%VRpL1C
zk3-9@9NlaOw#UYfZz#3F@NoUMf_~P;0=b4puKi~g`C}HfCdC%a;(yICFB%&+`;aKW
zM6k!tqnNn8jOWGY@72inip0KyM{0>(*TQ^dgX2-}yh?)P&-=tX^O*K8KRS@cUagAM
z2hocqRz`VrOk}fQ^B|#+mq-axUv-F?Ya3xE)zNA!W&+3E`%QIf`0>l|%*~_gJm6DE
zU;Ec2SKL=ZCl%+zy;vaE(MXAZ=JkKflGdcuf?4{nnR{M#R_$913hOzeAt!l`kV#a(
zp?wvjjSQ-?O2f@WE#GC;-E}!=+O;6}3d3{4N3CVob%7^neb$nu;)wLc65|mL400=1
zVgv19i9Jqgpjv5vDOcMZ+iXo(z7$!2DZ>Z(p*S%d|4C$uCvXplGHSh)x#o#iJ<70N
z+q8PIK+4ca*?(raKW2Gra>IgI;jeiH{StL8&p?*t3^QlzYlyg-Kvwr(w-M*_T9`Jz
zLsaibeWM?G5?$`+whR9GhPWvV%Qg-UA!qKZf@oJB+DVnD%D>@1b$2z+3RlirGNu7*
zb^F`i`kLeU*y)zur{*A+Jd$vzPa1bTSlZEIybtK)PK6#p-uS1|I=9^S{CJ)!kcu=?
z>7QBok9ngtxoN?y^4FZa;-kospaQgS^TT0=HP;Bu_}(df<NGM+dFds6mo8%COSxNm
z{Aba6&pLxgyrYEoHLvQ%>UZeu`UsKRRYj<RDsj4Q!}_GgwK#O@v1bjV23Y=53aU>t
z#%VDQVX7@gQ0sU30r^Z3v+%I9uDZSthCV8v^=b0Pp}z{Plg_WkVN`)srIBj?%<6y4
z8rG!df?4aYxx-S;>!R67#IQss{6Rb-CN57tl#73e;wJYgclA6ZoMW!u(FnYZ{B)|<
zSK3Sv^_*{;v+2fAy!60JeB$I{3D2+Gw?8};#TlOmS#tPQ;M^&mR;!FHSk$L$pWQ70
zPo57upNee6i8G8%aiu=s>zpFTan%z$<nhiY*YPYC$jvlT`=43okD0J0VZp5X*WA0#
z>Y`qJDY_zGQvBvpGhsi!@5@u}Zgk9|&1jEsHxY4Te)0`t{qJ|tpZn<SS7KgZG@0wa
zar9_%E+ew5WU+)2U5W%tqzK*+=eGUInhj99)L8HcO&$MQo;zm0Y!mc4vYldjs)W6g
zcF>zud4ff+#nsAWSG+A(Wc3$rE^JK|$SpKd@1I%!kJ-SQG+Zzn{WZ@W+{!*~Q;x!L
z{(;oWn}m4Bi{Z4l-RJ?!gOj7zyNMv0-6rLw4d{FJqc?F`--ylw0|FjH6Nq2tNzs^a
z>0*iVE8_A4r+Bc)h(Q1Rl{L^H^2Y8-m^9|r${Z+n5QqH+FQWDpN@5UTJ-Y7k4#;}8
zH~HCh8+<@ys6F3@2?tXJ(wIh?{4<;WF`HSF<_l(vzvi(^J>rZH%F(0s5k`k|ZW1l>
zC#5d@>P9p9`rAW3b`#9CrzWf<8c^O{#iG>WZ^S9gz_ojL0`*mJ{5m99x>(|=f6KwM
zrk{{I=Oo=G|91$P_Nqn4&7y<DA5Ju=vcOi}gZ)eg`q5Mdv!YNW3p9A*$gqO1sO#a-
zw;eOt#C@uW+)5)Y-MySitAFRtZGYg_)}+lJ_)@c#iRAX*^erov(9tyolZK%I32>xm
zg*hK*3f?_<l#%CG3h3@w6XeAb3fg_ojm^mz*cEPd!+P8jgrr(_8Yau*jsky?c%@DF
z2326uDjBF=qX~@jGOs0r=<p8LyK=o1Vo<j`Uocta8^Iz!FSs?C57<B2&u?w%CfH1P
z4qWdYMme%Czunv1fYPWEJf?z)mM>Hw_MP|H%jK5X>r9>W@?tBzYybNYjV%G76Ic3c
zZ~sc%%&4d?KB)s{W`zu=j@#gdM{LjPc~;@;RDpEjoO_bnMX-i(9&Ed;22#(2=YwmC
z35|Kx0IMA%gjr{;wu#MCf<As{TKD#PRJUm?@yKL1dg~t-xa4v;>ZeM$&zah$hvh&S
z?!5Fu!yk*&M_*jKqXc4_bagC-gK?3VP)Ku37K}Vnm7WXrg7~V8-t=ZSaO@IPZun|~
z7Yc4lyN4>CNJF%>02%hE8Lg1Orw(;#1G%2RxP|c>Vq{pUuKP>`T(!6BE!+5vsFtt4
zTmP{a#U?Z;socM}SYlI4;8=%;2YgJm=o7I^!nPR-`U3}j@cjoyr{{h<b)37I)ZQq8
zN7r7xd7Vhu_+--VWw$k)+IRW=polR3k1FhL-FLakqmo!XNd~;ze2*YwFY~PHd57+A
ztYa6g)&S=6iC3zIpNWXzgp;xg-wE^1qI?dQhs5KLYtXJE@hF`tF?j32?ly}&c&1_4
zs(92Bv*-F1l46D6#WCZNdsPneEt|0B)H(voOFEqjx%b2AN3Zi|rw`!k8fJB=HCq-7
z#hs2Go&92<vUBqiHRV;f<LU8zgNMXHS#b5cQ>&IiXkN7QjwBU${Y|gjv9JsMmm2a*
z+>{Q7&6&+_>}*2|C49u_SB5qRL8W1j@-@yJT(35(>v%R0h7)t&WE{?cy?39Ny!P`4
z^-{KJ<KRRvwH1(yNZAWl!cWf7MQh<3RDt2XTTQ0=AklfW&CJMC0Ury+I)czo#Js&$
zxz<Mv^ywQ#{A{ihdVWtQh6PmM6vGWo1&?9knDH5|d!o09T&jq4n`ZnqvmCB&5b7~1
z$i=5suD^M_#|H*qYK*dOjf1$&5i9Sm4aMru`7_M41Hj!V(ab>o6tJ#z=S=X~4D3|#
z8xJWt(>p^bAWhf9t!o`N6$y<CI(mulcz&(0S9%pV^180_CVwG;N$k;HH%v6gIJQoF
zQvvP6MGWc{7YGBYNPqXu-ujc$_>T9pXQ<T>oa4A|s4ME>PoH`C_xJ;@x3K+MnIn${
zBt~}$4H;o`{<n@beafIU?ml2Vn*c6yYM1Ga+X)l%4?9usZKSOdnrR$yn~*MZ6#4n}
z0h(Shw4oE<ME@;Gy->h%8@*>gSzOD1kC17v4|ZB{nXuYt{AG)ZGF+1A%{$~r5(MRe
zX3B$|C=a}$JaC!v01xGX1rPRHaBN<1ZJ|7bBIQ9+l!tgtd5Ghbhj>nTkSygvi;hJX
zX-gyR{%w5wKZ|pRHR<qgaXKz8PVLZyrq*9^(3jPebZoE)8~C3PP`efl-qxq2G#xVG
z&SVZ~WXHkU43nZgCtUEmJ?ZMJ-KUY)oeeC<BAz3=+kZ<8CXgIOtefFo=$CUYToXiT
z?{-mx0dcq_qH(<CSr6LsaYaFu5Fco3?(kCiy%^Z4_|{Zbe?aRZZ@)|rZ$eU33EAYT
zOI1UPa8<RhTu{IPZ;PICdeFHIh9W-8N6eAH_@%aRR7)Ie?`FF*rCVcvn=^GbvetOe
zMS`O)oE^WW3Qu|;xF<a8CT70pcb%(Ihttw`Rwi~dp#dAClSBV~A=ZnFt$X|Q1>v!z
zCEABA6AAQfb()QQh;$}nYiLoiNR}!g6}md6(l#6Ro^?wa#vb^4`mQ(s$((@Iqb|?c
zKL>+}$hPr~Q%B%q{k^EypB(URj@WW~4^J3qPLqhx(!>h|51-7wt(8(R>pB{8^hzH>
zrw9KV(bs|aardkfR`mGF9veH4hikxGboSn-%6=koTd%W2-%Hducb>KG-92=bDseHq
zGn3Q69a7GPSbV^VSY9!%czl^Y?A3eB*7hO?UcJ8gt1!S2d*0Hk+qNMVn#<3;N9Szd
zaRjz2s;9#bs6zUSUY?oQW>oTQdq{HGQrPk+FepuZ0{!sNbkIxFfZN3ueINY45?=#W
zvY)*Ci5RqIc`%rLmyjH2ug&w1Mk6o&mT3F9VY3rkE(lIC^(`y%$BHi$iu_;YfY)eV
zk3>c|XxO&zihFkiboQS%XN=;*BU6p$6Z>iS%;e>Z2el+{8C58rP3!q!C<e`G^hr!U
zT-Zw?@a|_nai}cen9ZZH!-nxFj$S)eIOLG7dYtna(iwD{E~uPGy$^RQzAL@6Si-A-
zjqi9G31Yh{UKG4Kf-S7anCV=7p?19m2gidfc(5$Rqs+$xTp9;<zJ7if^j5vvthm`1
z?CKhM!+E7)8&zB=eb+m}uM7z(7FK+Wk;5*E>z^FI-HfWlC)POZ5P?kzYUYfQ&rn8P
zZ=k)DDlE~=;wanricrxj5V`B#L2ROmfGuzLXa)-qpXFBwyXE5U`>tzF_IksszW$-z
zHt}#!cDj^qXDF`uaboI9ZZO<Ryy-GfeiE+Tu1K6bB@2iurZHwOU-kVhQc(4;A_GKm
zhq(L(?Wk51!Ngia_kKADeQ*6tr}h)wk_!Fx)Z`^`DA1*Ceq05vzc_cTv#Of7NfqgS
zH5OD<mB1=v^QFEI^`L59!Ks~VwK1K-G9#5+`dHv@@oa~fGPVisjPA<S#X-(2Y4=rx
z;Dt|m+w4d(ycnv~Z2s_$@clZadp@!SIc&9I+vs(bDD6%Su;%YXb@Tq-7Pd9WCM9C<
zntBVWO^vXa>+T`C9nZwt*47eHdr11X8#lt;G?k1c6*NMJ@_;<$!L^hJrZ@ccfFR`o
zOUeV`ln1|Da11QC_ER20m-3*Kl!wTtJcJYFAudrK^qBIX-UUa@f{S#bkvsn_PUk<1
z)5V(H^>1;yE-p@ci`|E;ZDXNCp7ZEFl@iRTYA?#fwI6=zrp|pz$b-y>U4mi{<AKSl
z@qOx#AS}>s673SX5#Z8w+r8)4gDq7|v^8!Q<sx9`kAxvELvA=?yk*R?R{}&?k=3UQ
z^JrhlJGH`y)o}ZgnXB530piq~H=WyWjG>tI9BxypH&G;2qB$Qusop074fo20<6PWu
zPViuA`&nCXH{q*1a*_tRgQI)SnX16W%PG^gS#Ef{<)#d`2`gMD^F_%2?lN3K74Vo=
zkXp_l5f`<&#Nnp~oY^$@%8cwq;@Vl)Zf^TYr0rd~JVWI*;TTp9QR<b5+o68#=$=k=
zFQ_4-(=HX=ph~FszW;7on*|pI)m8*IY{t1(n;7^~k3rIs-ouAmV!+{hWA-WDY@oAr
zJQS!JjD7b-c6t=;21CKYpcFb8tWOn`C2#PBSWCfL+E~OF?eC~-)BV<fbp$kzL@7A?
zvf%S&{%-q(h2gq)d&_#QXT)^DD=w1*!-(B=>VxU^yNe}W8kaxpOLT$R)%nKZu?aY(
z#Ck5##tL*v@n%bdTnI3!`21XD2Tt_J+Rd>k0os}T0?d0YA<to#a6{cZF-aBaJbiSY
z&9WCgxHUBzWhVgFzjcOhI`#v(TU`8cajPb<KQK$tmYgAsxWcm7`<e;j#VYZH@D^g|
zWc85QsT@>Kl{mku-dlHXHgtScYxPJ7#I$-3<~M$6!1I3YQWS48bnrB`Uk*P4%-xK3
z`u+Q`7<b5u+^PU<ArfQE7q<!wsN$1uiS*nz#NbwYanPfy9GExIPN8^M9L`AGZ5iCc
z2HOt#ZReC$hn&7|&dKgWX!}ZG*0`jfD2icb$5HnC=nz$c!(@B7UY{>8i8xF;X=P#A
zV$V;E72e=#BIBCZnhN~e4ONU-w*g<W%YGiI3|N^H!?>2*59#V<I<alWaF{CI7$CbR
ztMD~i85~dOzm&$=K}EM|e#2;sL3g>EqA=*VaoqVNIf|BcyyCQZtOj}i31sF=JR=mu
zn2Um{y9i0DXmlVz+$MCEc>Co>JMWGhe1hLN;o)^J=(8Ko3owa;lpT+Pg>yo1<I0{0
z&I2LfSoLvIWbQu@mx<13(U$^gs`&mFM)ew#kBC$6(1rGGf>^0s{MrrP=SayvLC?yC
z6~-@;Zm$phKuuhnGEbLw6QiH*-Y#CJ3ab+i^A<Z^BLt|Teb<6hZSRQUn=IX91;GS3
zTS(^hYN+6dy5`&s@rL-D(7N4gKW)P6lAA~8&Z=RJN1MBS9k?OhD4=wx{xEz^56~=O
z{!Dl+^$|X~t`$A9l9PU#bqcZgye-{AyNmjz?kCZ&N<(s^4|!8%TT!J2Uj=UOCt{y`
zVU6LwK^TtBtTYSQ1aZ<r%9qN6iM^Bu>L?F(qC5~pd0>|EfH36&1ImNv796PyE;q_U
z$ZY%TAs8tS(INTQLnu%lV!?wFC=W_raF{N*NH-ek{%>)5{8^ly)}+_J#p%7cIK9|c
zU(8I8hO%qyK9So_;1Wizur2Ct*hi;D?NLiET)WvP(#4Pf>Rq>dCQ&3_y>!L+n@y&$
zH>pmD#ccybQ^i>q3mS@Bbb+a7%fl{XQP2qqnZHyo1<GG68~PMjF}r7Vx#6wfA9j4$
zHHQ?Q5r#*+L~f3cqwW4ik@Z@wXe(91uQBQ72+I$`+1=VN-^&x{gmfrQt#H8jglq1|
zKqw3^Pqlj8qY0JUZ!DJ^bj5Ghikmn(T44ibjUX9DUTjDeKI`uM^u=_PVAwa`%XLBv
z#QF+W>Dvz=eaFayH^y0^h(6r)iRdU16jqy@-qns2S(dceZD>b{4-8F=1qx6yRl+$W
z_K3DYI^4jk$Dilg!S0QLrzI~Hz#g5gWqy;+Fv7Bx?TOP7*lp{`vfeQo_s~WMogIIF
znd#}TTYgajFBHyci_cXblL9q0KG_xRocMZA4a+`XfQ{0sgEx!Vaq6(AmO_mn969jj
zh~TZqM1hTa59i1zik&ev=S;he4pAi>vX+Ym2H8V#NZtjDE%A6rN2|nY8Ho)fW;N2{
z^Pu?Yxv=~5ZkQu|?r6lz6xh!a-1_9K4)jPJu8q`N4ezMp9hc5ZpvuQ+wA6O@nOEyT
z?67w0)+Y>jMAH1n#-h!@c>I3fZdp3Gr`ND4`u+$SlK*~Ix2~SBedD`wmvkl4qe?7Y
z)^tDk_g5A79YuE{yFlF2Aaz4@$syoIS5Uis77TTENm=G)LQik}`s+g>n9t=@V~pNz
zEZH0J%KP+s5TS~n)ZaV0VbeNbJjPsO61@Zub$pRa7+VLJyz+G0y`>;?rBURzmKJ1(
zd(7Q+9!6e_7L#`cexfIbM|WE8d$3qSe=m2YQ1%{Z(ixcL`<#XkJe`YV*zO8D%MI6R
zm&d_Lqt_QQk8<PJG?yAH@-Up&3)^*f)oys!_T%IV+3nCz6}z17@~l!CM{}}=ukG5i
z9<vD-&b);$$YE~cyLJgb+^b;ItyuFFv3fsJ4R_Oo?FW-Je0JX_1kQbD;qvY#R#HU_
zj2BYFH_>CyHj|>LeMj&C*0G~zm%ZTh-h&q=xufCy)ppUXUBQ^k#DZ~<77R*#jvw||
z7Qr=R7L`6#b!eoD2YbuytF_0G(C7YhD>_zT_xz70X&;ACNNTI;p>#SB`EZ?mWAS&y
zC2Z!?E!<2bSt^d68B&F}oW^HL+cEK;Dtc<KA5f997F+nF2v&!v!n|oO->jl6Uaozi
zaraw8d|2%EHyIUGJiFqE^)_!MJYDv7ViUhCIE`N~+)AGgj|ab<K2LlkvdU)9+}PBH
zl6=Nz`d_xA`MHCSD#h=hUl-C@PE?l?O*w14=xy3i-OUwFi7-SkJ4{D*&EF)XL&%iF
z(yGv-Sz+G3ghq5y9@tEI@OR1sOq2%}JTOjq;3wt5FBTjw3$9ekLu6APg8t`U4?$e~
z>p=@1qK5Jy4$6a`FF52DT;y&Vx#!>F^!c+m_ga(t{w>b^i;MFL+3jNO5edxjsBo-5
z5m#nws9vQ-<EgH$?a#|oAR%L5GmA_-ER%g1%B~fSXKj6C&cxV&{^Na}+C$1<K@}@B
z2==E9>qC~vcuI1p1YE%Ww+b!fAnlQS%{~V<oN`YtXVP2*PGGYwAADMfl53jm8_d6;
zyQ@@lKbf|oJ5&jSyo+Mb-wzW43a_tDU-ra|52J{0$D*)ifmKgBXB5~*M>y8!%%ZTR
zpC7ZQt7ErbkqQerD}3>5fU~v4DqKeu`a_(fbWNrRz1{Es(>kFI4GI!xZiWt^huQ09
zTc&y7cA~R+-q~+N*jP!NSlDA^S2+GPDz+8*R)nO>*`HY~A*rH6=N6d^sa9*_6y;sv
zSm2Mhx$y;XvyIg-=JsLy-V>dj(LDm@M$!E5TjFtFPlCZ#vHc)F^h5KtzciMl3Mc-1
zyH<^qhCa>ZM(+gquyoy+@Uf*^faUy!(pH^iIKAesNkjraWchiOREOOoke}j(&X95R
z!F{LOnipuXgpP;eJ*lc~Aa!S+qiHS<uaRBxsOx+N78c-MZPri#C$DEZT({qk&xssq
zO1YB-j{8nN75>DB8Mf-*n*2`=W-a~}yIRSw)CjwZZY{HM!m0A`We;DLp#w9HbGvkz
zy$&F+>Xz--7G{{s6uT7=`V+~S-O2p+IFGpe;@RGKz8k2LD$)9xoPJ%F2K39GXDBoM
z{zO6~@6w*Pg$=@OCCkflK;nY*lH!IGIK;9G|9l*dH`PjO2tRei%jpVu*Yqj_Csiyi
z7QH)0Z#^(IZr*ayU>+&ZtMC;vtcPMfq5D7Pmx7CE&e@ty0*+ljKO3MjihQqT&%S&%
zhwQd^XkS_J0NGO|h?urr_=*R-9eE^S?VN(kn-VyuAL~M9tIp-*u5jr88Z+$HX^Vr7
zToJs~84KoFf>-IRoZ;k?k7@6J+QWZTak|~aM32)1n#ig^jE17vt#i5VWzi{A!ffzi
z)lnX>Z{f?zSUQQiSk_j2R?vZzo>-N`<JH80vdb>p%AXK^R8js!ykh$UF)XviC0oWV
z9iL9V=Q4fR3!XT|xTwg6fUDXzBSXF*?8}ut_E91L9{ze3eKR^At}TzG{ikgRja2cw
z{pJz;AHJaM1dp#GTx{6Z)8bO{n$O7XnsTkh<QGC@D6?YC$agduuw+xRO%Nd)-Sk|o
zOdW2!Y<kyhbd$J46(yWu*`Iw^2-}HG)f9NiLF4_G_rgU)Ft6qQ3u?Uv*iPL~Y3j`;
z?4SK^S8k{>_WSH{Gj_#hP`=pa9@usms-xz=KfXFd^u<Jniau#Z$tR|tdJOcSHwuBQ
zob@-+X=9f)n@`pdrS<AfVoPtLtX~^9@3VYEY}sb9j_LPzzxN6q=-QvCgZD^>(;-HZ
z2%|ipNqKNR<pD0r0}hl2yeJRQC=YI3aHuZ0x+o8kL3xN*l!s`eJm@s#Ar4a>q)&NJ
z;DY1ff{XN}k$(Rcr~jYD8DLG4{}yN9;^N#f*X|OvHw-kU1fJ*BWWgDk4GA_JhjH{b
zj@yI$8$rq=Qkh;O7E19^>Hj1S;)&C{W-irkgP-jcdtzmEV9A}o#l<gw+GJRo{QihM
zu;BskdZ-iLw&bROBK$N-^gHCZ1b^`A<$TW~4xIhulyE{D>ZT2^<#PFgWQI154<<IF
zbySIz;pr#&JD0=7YXcwD^SyBA8nzT4r40P~Y8!{EK{P}W-<y95+TkOcoxjM4>;_4a
z!$p$Y5;rw|2srhD0j^WUUCvteZ3=Yo{GydY1PcKFj^)owW}l+73LF7ZQsThNa6{iL
z{uj}k`0_lTb}#Y>VN+}_zKITx1lIMvtwL#33070vQdgfiF!L&ru@tlgu8gtFx6wx-
zb8r6^xveMheuXAyE1nz>`DDetzd0Fe2#7`6M0vxrIDOXB!*Y0tDg=o>8^1*_3&NfX
zVV&cGILY|l{pEB9;5_GdOIKqB&RD*#f1fxnynQrhd-r+`+9=!dUq$pdvVZ48-&B8Y
zvBXnHy{2GIGiZII`BGjy5eHsZ9^#)a#`}G%y+247!gedw!P+|^_??zqxI1e;L<Dl&
z8Z*$w3E{Cpo|Xh8QpL^m+5WdTJR`1fsd%Se+6c;9WrXq%EyeSOgR?U;`jBpXL(_ci
za+u$mE_r>F2_Mwuv`UITi$eJ2Ci1M?Q7BcScJ*vg=9whuV2a0DAz`>x>EO8?dHZqZ
z3l>=;!F*skbbr*TJO%=KUh>J#hv10s51E$KZ^dFFLwRDZx*$Roza$5O25qFkZl{T<
zf!ic<Gp#Re_FWI8^$hoD1RIEd(*Inipa(Ob)Yr+$kE6YE_Olzd%%R!m{=LVGIu=Vj
zUi0&Pt-cGiWe#Vt*(Bht(L&B1w_NeHi|Wx*?Eb*6aN;|E>0Uf==Fz*uYxcket#c6v
z$Bf{Oz#fIXU3Tz_D)!Pf4Qwg-h_q<0J)8V^u*|gM#UH<>(N$B&44*tMDCZGoU-$dO
zroHOI75CpKK!zQ>;fY3<P?OTDIH8S$#5Jm@`?$(ik)29-^(<<YzxDh5%GbA+z3Blb
zF6d@sVn3KSa9cXC`eR?7WAdhiJIIe{TJg_j0ej!|-<Nc@0S8q~_iRO)g!eb3+(dtF
z=EhINToZVXz3~gO&20-P+xU`bRGVHQ$U28=j%^%{eO-h2>++xUY}pK_oAbAx-HwS;
zs>s;yUU``cKYnvYd-;U`ArL+PdiIH-2&OBF=IKw^g3D6h?Q6<W!luv~Y2U1dO~S6c
zVB#}`<LhRkuY68|*B4`Mm{oNXvudB0%3i8Md|l_c2YUOEW6IU5#{;WSOd$6eZPt3i
zWXHg*K;J4fq*7YE72Xj?b}}{nkiSi6C9Ar(?9c|O+l+0--}e8$xc1irEtCffQ6Bhp
z?5_tZC=VD>9<ZW3nCTMbIJw~ZN_hw&%0t*u9wLqMAXCaiyrMiPm+~Op1;>^J7a2q&
zY5x{y@Snwbz?uyCw>U!=7bkmw)fe}98dN-pmUZGRgBKrJs}KCj!s-!W4=<fc!8}X4
zUFpUm!Tj1grjvoe`1)5#W8U1~_h4;u`_Lz04EL$xZt3~2o*ym2x^`>svNmyuXJtxi
z6H|tcJR!c*Qd4N`>hsX~ixS|pGH`FYQy<!SqSXBZ{REO05IQ~5T8~7j5}MJi!sYI3
zL5F;$6WhEO7tMT*&$G_Mrz10rj$McZLx~|~#k+fPaooW|<JaME`t*8opvw%ued7H6
z@*xFSC|12S_^9?UGx+XGRVz8F3wmGf?XXYkMow+s?}j%hLUPp3r4q?Z&?utpxmFz$
zOxK7=W0q#r?x5i|7E*^gs1hk_ha1L^9fW1!6-CY$c`!e(i%!tGqtGg!e9J$f0=umK
zQL`Z@A9Rh4@UT=G=4;#Ed4|~@c#3$sxMzM}Ri+B>RlGLXX3GO9er0<mzYwM!TNQfL
z#}tw)j_a)+TY()c-*j}2aKXBjk7;=;UZXk~?J{3BhFU@nOv<pv5M@+R-I>ik?m`B@
zdTQd-o$sl*<V|PUX7y4`?;de20**pn+8b%Pmod0d_eH3UY7sbUzVQ95rHiBTpUn<e
znZZKwgqU5s=B;t!Yp}FvNxKHft>v<nmt2OcnPk*U2F>95Pwk7onX6zb!y@-#2n!~e
zgJMKBH6rVl>9s~@9xRp+JD_+g=UEIiN>9y{=)~czckRtDw}<2N$HxO}R~`imSw<zC
z;|Hcr;TrGdXm}#k@U`z(Q#?^DzFwoi9LA{PobAAyD<uQx#F%-Cn1|5`Z$Wp#mlBZZ
zo+Dgn%mxXQyxq1&#t<Bk=ziN^9KEpl5N_l31JUg2SG7JwXb)8)&`IPf+Y(#2VOSz&
z|0x=)@vhh&tr3h@ioD_*KVc5jy_FmL5<>CAjUYLd;*S+HYBED^uf?M^Z{7=ATEPdZ
zcqsc@?AeS#w5;U8mB#j;=sx53>y};LkpzFe+FBk?a4x=HcL~2nzN8Xk=TAfETU}7L
z?Ac9ZMWo6k^}QubsUnXa;fYWs6)gVFtJU&r6mDI%R&q#V7mya*mzG*MfMO-f>Sw{;
z_>-xzzDfiEhcrL5&M79sZN@?lv}?y=u~+V<gTFrqqF49C%TDloK<x=%t(4d%(G!9!
zBXkRhHL>o}!JB>}o)h#=pEMiLl#)o3#${bl-96}Wrl^)!C^FugWgPr`B|h}oo%Q*h
zVdTF~_^Vl~3_d7#al?B?O`P#Ivz&XGJRaA#{^=yN8K)236Il7h0whYleYG-+h26y;
zJ%!hG5b-0NJNB$OLp%<u$~>~R7csWJDSDW2l(6rq>|MV3GEwpH^PXCli^RzzGY(2W
zM~JfFoad*mv=Q4rNAMn~-vS@FbT`WJ`x09y55V|e58+FBAe{2R5y}ITlm`kZ5AIxW
z$Sk<3DGwT;JcK0WL6(%~_q|7dJ?I4GK^2q-(XXT&4ht?aj7En4TbvPp7Uw~0GV<Tz
zj9OfrG;@=?N1ywH;MZlfHoR2;dKpJQ^JU|}2mAK-oy^DM9m_eKm1t1qWmaD)9)Q0a
zCYG5`DdP9c(n9@KR`8B0=3qOw-L}vcxTiP^rQOzonE0IwE0%A9fckpNy#}R(JHOj(
z&Wt4Zuspc2(Xa>2rVAC4IiqO)*T-VmeFRNXB}DecOlw+*gD<a>5}O8z-6!N(DwZ6@
z>%9sRUM2;D!hx*0;fNsoX!&((EEos*`9BPn?K1p*b6kE&8jCjkq>4RS4lqw}F9)5y
zJI#ZJ^njU3Lw{mjD)MO%J}~!K6EYq$$0$m(K$>R5Q@?NDi1@Da#Y~ddkeGpO8yu-a
z>!=bI13}_4>4CsD%NWOV!3#Ue%_ywcp9e|ecjRo?FJk+1>&`ZE9s@u8@$lNRbR7RW
z<<me40~U3R^pZ1{z+zNk_O{aN2v0?ryUCO7sKtlbq|+}Ok=F1|@5PtA)D`$%e%X|{
z6C3EX7qpEld_(iJsgnJPZ_!OQuXj<lB}5ff<jsNf+CvCPSrXxP?`I}fz=gR2ZDsg5
zZ;P8$Xf6Z?*M4n2lYsjpMi`d%90!xc`=UnrTKH_`ES*k^H7pcIGS|yQJ)I#uQc5~*
zZPx`)`VZ35x+`&J|C9UoqHLg4`5Zgy;Q{$72aW13W-QvV-?HIsGx}WFcx-iR_hJc>
zFNlMF{{dLe6;jaIei)l?$!?ea7>!$OhMk7!kHN(0)Xl|PcHzU{vzWz81M$--`owKf
zmN?KTuj>fwcA!zkhgg{($~4PDf8LubIlXy=jQBZAIbt37p|f{wma#&i(Xt4$K`R)j
zSNb~kWfa*K`R5;1{egNSa9%-~H*xOJ-=eq1At5ks2+dWeJ`)c^v3}&G2nM%9c<kMt
z-ppJNygkTccui<5exX}ECEgs5_m14CPCjmk4c|Xz98m`_po-t6rt&4PZbj<t?<E*5
zripm<g9*Ei(&2-bSMTE-W`!peWvNanw~)&kvis_GD_FUE@Q%(%8%q5yIvalG6A?od
z-L~sIE?%pIgEE!rrj-0~hxa*`FC#XfE%_-+(25TysTijCSvX*Z;fS3l`?g{F#JUNI
zNE)~T7ayIP>tgZg=bKhDa84moo=k{beG$b5vPQJme?f^=v&%KEcAx_7t-hlt=8-u6
zwQTwK6==FQ>zwyC6A)6%wEo2sM9i!HExM3Y*&$%Vfa&>7W|`4?EKwb<7(T6xW#n80
z)+9^eIvo7X`m78tYQ80J?XnrKi)*{KIn5e$)2f$^;}AGqw)_5n!(GG!?N_T$2DT8x
zk2>Gzvvr|!Ym1mK;5ve(?MY;bd=8<-V%N&F?;a7BwEAQ1-Em^okKkQm8O;RU9*?Z~
zZ>Dg%=z3U+;sK(B^1yP+12U8c)=?g~N_k+x1G1C{4=*^37hG>B5BfxTh^v$bF;E`l
zKzUFE<w4w(2Q@A@SQcF5AsQL|Z*j)_S)8%fWZb{S8NawVAM<ZodPdk2?6euLCMH~l
z!+rfMk_R&J`4T}v;lKiH81uQ5v&tU)!UvzN@AJZMq=H8Jh4paxxo_LbJguOND$ag+
z^*EYv1*v<X8LKUL!M*Y86?SQ5DAjcujr{Tm4PSJbpWM10K6^)bD=4%O`Skj-^rEj&
zXIO^t$k}s*2UYa+*vLW4TWev*PPKmixIoOZ_5Pt*nOr=-#fOv3afONC%(1RLB!2Qe
zjIlE)7Mw)6^v`i=WB#fM4$l`luu$BsZJG4LiyPF081#$gj9`^yrdRvu3{fJ=necSR
z5PtBkTWXX@2i^KtPnu7!fMZi32ObW&5|VcFXZD0wAqA?$<yhXcGmdVcFL_U<R5=zq
zIcEvyY|8@YXlWH)fs0t{3$1y0%Q48s&Urchsrcd=&L(DaE4(B6sPwh(Ja{KnaIBwx
zYAhrVHXPeW55_Ry!Cf7(mXf=m^I_iY>&KX}SVM2t`OV`*&CZ904SVQt$%vhc{mw_I
zC0al)GBcWRqKa^~**n+YC#0`+bkk-znfQK=!&@$`(|EV`juQe2zrT}qddO6LCkBt(
z*FJS^EreyG<!7>@<uNmF;rYS~+hC!%;rkVPDfTBsiT?6A<~=4bp>}I5W-}M&R`{}Q
zNsKcL3<vKv<KTj`_xkQ1NaDo58XPOXan_-+U7J7VZ+eVIs1jK=dGiCkyP&_!w&+7k
zHs)%sV3j3eaL*0vuaYdqP(E?zlkSr!T#;;$Q};R$`!>p!t_`rjJ6@cc>fdb-_EfRZ
z_q^d<CDKq>vZ?pBW;gNVcv70ssx@#=rfGEU$uu!>?9{4Yb7#<DZK!%{@*bJ4Z@eHn
z`x713(mPvW-$4XXMZ)RgQ!lMWpel-0$r<m*7p>2!oGFUN6U!Bo&l`o}c$-h|p>~OQ
z4a2V)ji(vdIm-4>*iI{~ba(xe;els_Emd@d>$s9pT_3^GSyK3Ha20sRJ7ikBFyZ?@
z*f+jN`9^5Ae;hFJtR$K;!_{(CongCvqehA)Mqi?K*aqlN6Q8IeLxV2E2ls@2U&Pem
zw@+G*pYDxas>}<JFvC`LsBH%xvO2Qsu%jZL)ie!%9O8kiav15fYYi|Pi$tF9um>y@
zKe=sX=-@hmQmngfSU%_`(k(JwYa&O`K^Zz}W}7>xqD(u9SVfOV8lnwsJ1-Kt!!Lf6
z*4V>6lcpz2KGmRhs>GqC#kykx?+DjPUiMdh>R2>sF!#UX8*%<zL)_>T6Ub!nt#Y)L
z!g)CaPrr~V{$%ia_qWOIaI`4U_)?KKM8Q<3sNr+squ9v#ZRhV2tp`3Hds*9p1XHhS
zPO()JPO%THws_Pc0j_zP`m=rlZL74`e>zRv5^S0rIdz3#nqu0L%I5$tj|^~6Nn{b>
zlm|Ie9uT2CkVJX#f(LRa4~$YCd}zT@u;6+_c@U;N1TW=54wQ#jMtM*(<w0$f2Q512
z7hGfljZFM=7ktvcyWo@mY|j*HGWGBFOdt>c-ko~JYPZqK>gNCO8m|tI^n=Hs*Xoql
zSj%PXOA4@UJl_B<|Ai;S$tU8X)jfPSR(62t`Bj5D8!+Dg%auumaW~#a705Iinf`B6
zXZ)F!X-#G=%sTRSmaWJ66DOY*;D#uZY2!=T@JmC#aYuOteoNzw`6W~hc1Oh+>c?U*
zTm15-voCJK)Q?XMY0K*I&v&)m5eyD^jw+DZG&1MktlU4d@~p}Hg;@oEXE{X%qCVLy
z+@CW4^=^J56w3D)%E<hOwaipnH*6|{QInI8>xLrmdM#(=8#Py<m*-WBR#YX<GIXun
z5oLrkr~-MEMi%~?b?ncqB5SgEVb<}#v$pwM%W`yz#;tW6m)3Lchlf{bKaNW!W8v`1
zV5{TVP%&`DtVxZAdzvq~bbhXYU;j1VydQiF%OAQerpK&^<){MrAB{ZmZ`R2_vrbu)
zB@44o|D83pQc>bci6GX`UJ(^}BJlU?i<xfWromOVd(3`?0l3Jy{E@P`J3e5gy8l9w
z3)tLXPPnj@2fwEsRNxc&fM`?+vXn-i`8TWV&#ZE5vSMM@*}t<=H8_J6jiTX{wM(qR
zt`w}g@&@_kP%gMlIntk9QSkd-%hax}E_aA{u|9G<y&SvCJTjyGK8ZLTzZto;R1h4f
zV)7h~to%3Y{GVADtjUWDvo8IeWtQ*WxTZb}X5=alCn@D(11U*m_VuU0I_G=DnboH;
zPwu>VieoIeYjx-6v|q(z9y+hv9V?-{D98Kq3MHtbipeS(dHLV0>OZq;tjXGiS#^JB
zZBgL~Tb5Y}y~^lUnqm>Y#`Vfw`D8idPT1J1n4QG|duHCp3?G5hJ*sj&2`yOCPJbrm
zOC7xPtL^;$Qg5-Ctf!Gz{>{4jXI6tXd2M0V^}n+?jMP2~Z94V)V(Gzz)4Zp!(4$z!
z^pK12*g%cXa``2E*_(53{`Vp<x^exIY|34%!%_7$dliP5eZK^Y<ZR&(RZKS0$Q$ln
LUNo}FeZ&6%LTcSd

literal 0
HcmV?d00001


From 31c06ff2e94cb8fd67e91d139210e1ce63f7fa57 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Mon, 6 Jan 2025 12:34:04 +0000
Subject: [PATCH 44/95] [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
---
 .../agimus_controller/factory/robot_model.py  |  3 +-
 .../tests/test_ocp_croco_base.py              | 73 ++++++++++---------
 2 files changed, 41 insertions(+), 35 deletions(-)

diff --git a/agimus_controller/agimus_controller/factory/robot_model.py b/agimus_controller/agimus_controller/factory/robot_model.py
index a02f3a4c..d78fd7ce 100644
--- a/agimus_controller/agimus_controller/factory/robot_model.py
+++ b/agimus_controller/agimus_controller/factory/robot_model.py
@@ -109,7 +109,6 @@ def armature(self) -> npt.NDArray[np.float64]:
         """
         return self._params.armature
 
-
     @armature.setter
     def armature(self, value: npt.NDArray[np.float64]) -> None:
         """Set the armature of the robot.
@@ -117,4 +116,4 @@ def armature(self, value: npt.NDArray[np.float64]) -> None:
         Args:
             value (npt.NDArray[np.float64]): New armature values to set.
         """
-        self._params.armature = value
\ No newline at end of file
+        self._params.armature = value
diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index 1b304f51..04ae7f78 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -63,7 +63,6 @@ def test_dt(self):
 
 
 class TestSimpleOCPCroco(unittest.TestCase):
-    
     class TestOCPCroco(OCPBaseCroco):
         @property
         def runningModelList(self):
@@ -73,7 +72,8 @@ def runningModelList(self):
             ### Creation of cost terms
             # State Regularization cost
             xResidual = crocoddyl.ResidualModelState(
-                self._state, np.concatenate((pin.neutral( self._rmodel), np.zeros( self._rmodel.nv))) 
+                self._state,
+                np.concatenate((pin.neutral(self._rmodel), np.zeros(self._rmodel.nv))),
             )
             xRegCost = crocoddyl.CostModelResidual(self._state, xResidual)
 
@@ -84,10 +84,8 @@ def runningModelList(self):
             # End effector frame cost
             framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
                 self._state,
-                self._rmodel.getFrameId(
-                    "panda_hand_tcp"
-                ),
-                pin.SE3(np.eye(3),np.array([1.0, 1.0, 1.0])),
+                self._rmodel.getFrameId("panda_hand_tcp"),
+                pin.SE3(np.eye(3), np.array([1.0, 1.0, 1.0])),
             )
 
             goalTrackingCost = crocoddyl.CostModelResidual(
@@ -105,33 +103,29 @@ def runningModelList(self):
             runningModel = crocoddyl.IntegratedActionModelEuler(
                 running_DAM,
             )
-            runningModel.differential.armature = (
-                self._robot_model.armature
-            )
+            runningModel.differential.armature = self._robot_model.armature
 
-            runningModelList = [runningModel] * (self._ocp_params.T-1)
+            runningModelList = [runningModel] * (self._ocp_params.T - 1)
             return runningModelList
 
         @property
         def terminalModel(self):
-
             # Terminal cost models
             terminalCostModel = crocoddyl.CostModelSum(self._state)
 
             ### Creation of cost terms
             # State Regularization cost
             xResidual = crocoddyl.ResidualModelState(
-                self._state, np.concatenate((pin.neutral( self._rmodel), np.zeros( self._rmodel.nv))) 
+                self._state,
+                np.concatenate((pin.neutral(self._rmodel), np.zeros(self._rmodel.nv))),
             )
             xRegCost = crocoddyl.CostModelResidual(self._state, xResidual)
 
             # End effector frame cost
             framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
                 self._state,
-                self._rmodel.getFrameId(
-                    "panda_hand_tcp"
-                ),
-                pin.SE3(np.eye(3),np.array([1.0, 1.0, 1.0])),
+                self._rmodel.getFrameId("panda_hand_tcp"),
+                pin.SE3(np.eye(3), np.array([1.0, 1.0, 1.0])),
             )
 
             goalTrackingCost = crocoddyl.CostModelResidual(
@@ -149,9 +143,7 @@ def terminalModel(self):
             )
 
             terminalModel = crocoddyl.IntegratedActionModelEuler(terminal_DAM, 0.0)
-            terminalModel.differential.armature = (
-                self._robot_model.armature
-            )
+            terminalModel.differential.armature = self._robot_model.armature
 
             return terminalModel
 
@@ -159,7 +151,6 @@ def set_reference_horizon(self, horizon_size):
             ### Not implemented in this OCP example.
             return None
 
-    
     def setUp(self):
         # Mock the RobotModelFactory and OCPParamsCrocoBase
         self.robot_model_factory = RobotModelFactory()
@@ -174,15 +165,21 @@ def setUp(self):
         self.robot_model_factory.armature = armature
 
         # Set mock parameters
-        self.ocp_params = OCPParamsBaseCroco(dt=0.1, T=10, solver_iters=100, callbacks=True)
-        self.state_reg = np.concatenate((pin.neutral(robot_model), np.zeros(robot_model.nv))) 
-        self.state_warmstart = [np.zeros(robot_model.nq + robot_model.nv)] * (self.ocp_params.T - 1) # The first state is the current state
-        self.control_warmstart = [np.zeros(robot_model.nq)] * (self.ocp_params.T-1)
+        self.ocp_params = OCPParamsBaseCroco(
+            dt=0.1, T=10, solver_iters=100, callbacks=True
+        )
+        self.state_reg = np.concatenate(
+            (pin.neutral(robot_model), np.zeros(robot_model.nv))
+        )
+        self.state_warmstart = [np.zeros(robot_model.nq + robot_model.nv)] * (
+            self.ocp_params.T - 1
+        )  # The first state is the current state
+        self.control_warmstart = [np.zeros(robot_model.nq)] * (self.ocp_params.T - 1)
         # Create a concrete implementation of OCPBaseCroco
         self.ocp = self.TestOCPCroco(self.robot_model_factory, self.ocp_params)
         self.ocp.solve(self.state_reg, self.state_warmstart, self.control_warmstart)
         # self.save_results()
-        
+
     def save_results(self):
         results = np.array(
             [
@@ -190,23 +187,33 @@ def save_results(self):
                 self.ocp.ocp_results.ricatti_gains.tolist(),
                 self.ocp.ocp_results.feed_forward_terms.tolist(),
             ],
-        dtype=object,  # Ensure the array is dtype=object before saving
-    )        
-        np.save("ressources/simple_ocp_croco_results.npy", results,)
+            dtype=object,  # Ensure the array is dtype=object before saving
+        )
+        np.save(
+            "ressources/simple_ocp_croco_results.npy",
+            results,
+        )
 
     def test_check_results(self):
         results = np.load("ressources/simple_ocp_croco_results.npy", allow_pickle=True)
         # Checking the states
         for iter, state in enumerate(results[0]):
-            np.testing.assert_array_almost_equal(state, self.ocp.ocp_results.states.tolist()[iter])
-        
+            np.testing.assert_array_almost_equal(
+                state, self.ocp.ocp_results.states.tolist()[iter]
+            )
+
         # Checking the ricatti gains
         for iter, gain in enumerate(results[1]):
-            np.testing.assert_array_almost_equal(gain, self.ocp.ocp_results.ricatti_gains.tolist()[iter])
+            np.testing.assert_array_almost_equal(
+                gain, self.ocp.ocp_results.ricatti_gains.tolist()[iter]
+            )
 
         # Checking the feed forward terms
         for iter, term in enumerate(results[2]):
-            np.testing.assert_array_almost_equal(term, self.ocp.ocp_results.feed_forward_terms.tolist()[iter])
-            
+            np.testing.assert_array_almost_equal(
+                term, self.ocp.ocp_results.feed_forward_terms.tolist()[iter]
+            )
+
+
 if __name__ == "__main__":
     unittest.main()

From e011080353650095f7c80fd8f87c3e1140a5d608 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Mon, 6 Jan 2025 13:36:23 +0100
Subject: [PATCH 45/95] Update
 agimus_controller/agimus_controller/ocp_base_croco.py

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 agimus_controller/agimus_controller/ocp_base_croco.py | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 4f2d1665..113fd564 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -93,11 +93,8 @@ def solve(
         ocp.with_callbacks = self._ocp_params.callbacks
 
         # Creating the warmstart lists for the solver
-        x_init = [x0] + x_warmstart
-        u_init = u_warmstart
-
         # Solve the OCP
-        ocp.solve(x_init, u_init, self._ocp_params.solver_iters)
+        ocp.solve([x0] + x_warmstart, u_warmstart, self._ocp_params.solver_iters)
 
         # Store the results
         self.ocp_results = OCPResults(

From bc1f31dd4459e733002c40ed03713af62558a2fa Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Mon, 6 Jan 2025 13:36:56 +0100
Subject: [PATCH 46/95] Update
 agimus_controller/agimus_controller/ocp_base_croco.py

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 agimus_controller/agimus_controller/ocp_base_croco.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 113fd564..16ba67c1 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -44,7 +44,7 @@ def horizon_size(self) -> int:
         return self._ocp_params.T
 
     @property
-    def dt(self) -> np.float64:
+    def dt(self) -> float:
         """Integration step of the OCP."""
         return self._ocp_params.dt
 

From 255c1f0bc4edb667ed20063eebaf1c8574a4df56 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Mon, 6 Jan 2025 13:37:28 +0100
Subject: [PATCH 47/95] Update agimus_controller/agimus_controller/ocp_base.py

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 agimus_controller/agimus_controller/ocp_base.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/agimus_controller/agimus_controller/ocp_base.py b/agimus_controller/agimus_controller/ocp_base.py
index 7f920285..55100c33 100644
--- a/agimus_controller/agimus_controller/ocp_base.py
+++ b/agimus_controller/agimus_controller/ocp_base.py
@@ -35,7 +35,7 @@ def horizon_size() -> int:
 
     @property
     @abstractmethod
-    def dt() -> int:
+    def dt() -> float:
         """Returns the time step of the OCP.
 
         Returns:

From 417d9bb12135e14937a919642e7afdd2deb254c0 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Mon, 6 Jan 2025 13:37:53 +0100
Subject: [PATCH 48/95] Update agimus_controller/agimus_controller/ocp_base.py

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 agimus_controller/agimus_controller/ocp_base.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/agimus_controller/agimus_controller/ocp_base.py b/agimus_controller/agimus_controller/ocp_base.py
index 55100c33..f7d36bd1 100644
--- a/agimus_controller/agimus_controller/ocp_base.py
+++ b/agimus_controller/agimus_controller/ocp_base.py
@@ -36,7 +36,7 @@ def horizon_size() -> int:
     @property
     @abstractmethod
     def dt() -> float:
-        """Returns the time step of the OCP.
+        """Returns the time step of the OCP in seconds.
 
         Returns:
             int: time step of the OCP.

From bafd67ce5238c9abbdeb7b3e41bcfba71c8ffe9c Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Mon, 6 Jan 2025 13:38:29 +0100
Subject: [PATCH 49/95] Update
 agimus_controller/agimus_controller/ocp_base_croco.py

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 agimus_controller/agimus_controller/ocp_base_croco.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 16ba67c1..819051a3 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -65,7 +65,7 @@ def solve(
         x0: npt.NDArray[np.float64],
         x_warmstart: list[npt.NDArray[np.float64]],
         u_warmstart: list[npt.NDArray[np.float64]],
-    ) -> bool:
+    ) -> None:
         """Solves the OCP. Returns True if the OCP was solved successfully, False otherwise.
         The results can be accessed through the ocp_results property.
 

From 54ddd9b6e741e8b3a03efa4f976842b9831e8bf9 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Mon, 6 Jan 2025 13:40:14 +0100
Subject: [PATCH 50/95] Update
 agimus_controller/agimus_controller/ocp_base_croco.py

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 agimus_controller/agimus_controller/ocp_base_croco.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 819051a3..31d203fc 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -60,6 +60,7 @@ def terminalModel(self) -> crocoddyl.ActionModelAbstract:
         """Terminal model."""
         pass
 
+    @override
     def solve(
         self,
         x0: npt.NDArray[np.float64],

From adb3cedd9389b0c7dda4ada016497fe33a741dfb Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Mon, 6 Jan 2025 13:40:49 +0100
Subject: [PATCH 51/95] Update
 agimus_controller/agimus_controller/ocp_base_croco.py

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 agimus_controller/agimus_controller/ocp_base_croco.py | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 31d203fc..bb93a9d2 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -69,11 +69,6 @@ def solve(
     ) -> None:
         """Solves the OCP. Returns True if the OCP was solved successfully, False otherwise.
         The results can be accessed through the ocp_results property.
-
-        Args:
-            x0 (npt.NDArray[np.float64]): Current state of the robot.
-            x_warmstart (list[npt.NDArray[np.float64]]): Warmstart values for the state. This doesn't include the current state.
-            u_warmstart (list[npt.NDArray[np.float64]]): Warmstart values for the control inputs.
         """
         ### Creation of the state and actuation models
 

From 861f7c2387e481c26a11332d3b2c342f1204a775 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Mon, 6 Jan 2025 13:41:13 +0100
Subject: [PATCH 52/95] Update
 agimus_controller/agimus_controller/ocp_base_croco.py

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 agimus_controller/agimus_controller/ocp_base_croco.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index bb93a9d2..95204048 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -43,6 +43,7 @@ def horizon_size(self) -> int:
         """Number of time steps in the horizon."""
         return self._ocp_params.T
 
+    @override
     @property
     def dt(self) -> float:
         """Integration step of the OCP."""

From db440c7f226143d7abf70d1d47787c9ba35110c3 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Mon, 6 Jan 2025 13:42:36 +0100
Subject: [PATCH 53/95] Apply suggestions from code review

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 agimus_controller/agimus_controller/ocp_base_croco.py | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 95204048..5e0444da 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -38,6 +38,7 @@ def __init__(
 
         self._ocp_results = None
 
+    @override
     @property
     def horizon_size(self) -> int:
         """Number of time steps in the horizon."""
@@ -100,6 +101,7 @@ def solve(
             feed_forward_terms=ocp.us,
         )
 
+    @override
     @property
     def ocp_results(self) -> OCPResults:
         """Output data structure of the OCP.
@@ -118,6 +120,7 @@ def ocp_results(self, value: OCPResults) -> None:
         """
         self._ocp_results = value
 
+    @override
     @property
     def debug_data(self) -> OCPDebugData:
         pass

From b2f99bb74fc42cafa475c71c7766375606bf6f97 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Mon, 6 Jan 2025 13:43:02 +0100
Subject: [PATCH 54/95] Update
 agimus_controller/agimus_controller/ocp_param_base.py

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 agimus_controller/agimus_controller/ocp_param_base.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/agimus_controller/agimus_controller/ocp_param_base.py b/agimus_controller/agimus_controller/ocp_param_base.py
index 026bb567..fd36f727 100644
--- a/agimus_controller/agimus_controller/ocp_param_base.py
+++ b/agimus_controller/agimus_controller/ocp_param_base.py
@@ -16,6 +16,6 @@ class OCPParamsBaseCroco:
         1e-3  # Termination tolerance (norm of the KKT conditions)
     )
     eps_abs: np.float64 = 1e-6  # Absolute tolerance of the solver
-    eps_rel: np.float64 = 0  # Relative tolerance of the solver
+    eps_rel: np.float64 = 0.0  # Relative tolerance of the solver
     callbacks: bool = False  # Flag to enable/disable callbacks
     use_filter_line_search = False  # Flag to enable/disable the filter line searchs

From ee4181ce86fd00436da16ff5c1bcaa13a84b8898 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Mon, 6 Jan 2025 12:53:34 +0000
Subject: [PATCH 55/95] taking reviews in to account

---
 .../agimus_controller/ocp_base.py             |  9 +++++++++
 .../agimus_controller/ocp_base_croco.py       | 20 +++++++++++--------
 2 files changed, 21 insertions(+), 8 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base.py b/agimus_controller/agimus_controller/ocp_base.py
index f7d36bd1..255ab875 100644
--- a/agimus_controller/agimus_controller/ocp_base.py
+++ b/agimus_controller/agimus_controller/ocp_base.py
@@ -70,6 +70,15 @@ def ocp_results(self) -> OCPResults:
             OCPResults: Class containing the results of the OCP solver.
         """
         pass
+    
+    @ocp_results.setter
+    def ocp_results(self, value: OCPResults) -> None:
+        """Set the output data structure of the OCP.
+
+        Args:
+            value (OCPResults): New output data structure of the OCP.
+        """
+        pass
 
     @property
     @abstractmethod
diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 5e0444da..83864378 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -38,13 +38,11 @@ def __init__(
 
         self._ocp_results = None
 
-    @override
     @property
     def horizon_size(self) -> int:
         """Number of time steps in the horizon."""
         return self._ocp_params.T
 
-    @override
     @property
     def dt(self) -> float:
         """Integration step of the OCP."""
@@ -52,25 +50,29 @@ def dt(self) -> float:
 
     @property
     @abstractmethod
-    def runningModelList(self) -> list[crocoddyl.ActionModelAbstract]:
+    def running_model_list(self) -> list[crocoddyl.ActionModelAbstract]:
         """List of running models."""
         pass
 
     @property
     @abstractmethod
-    def terminalModel(self) -> crocoddyl.ActionModelAbstract:
+    def terminal_model(self) -> crocoddyl.ActionModelAbstract:
         """Terminal model."""
         pass
 
-    @override
     def solve(
         self,
         x0: npt.NDArray[np.float64],
         x_warmstart: list[npt.NDArray[np.float64]],
         u_warmstart: list[npt.NDArray[np.float64]],
     ) -> None:
-        """Solves the OCP. Returns True if the OCP was solved successfully, False otherwise.
+        """ Solves the OCP.
         The results can be accessed through the ocp_results property.
+
+        Args:
+            x0 (npt.NDArray[np.float64]): Current state of the robot.
+            x_warmstart (list[npt.NDArray[np.float64]]): Predicted states for the OCP.
+            u_warmstart (list[npt.NDArray[np.float64]]): Predicted control inputs for the OCP.
         """
         ### Creation of the state and actuation models
 
@@ -101,7 +103,6 @@ def solve(
             feed_forward_terms=ocp.us,
         )
 
-    @override
     @property
     def ocp_results(self) -> OCPResults:
         """Output data structure of the OCP.
@@ -120,7 +121,10 @@ def ocp_results(self, value: OCPResults) -> None:
         """
         self._ocp_results = value
 
-    @override
     @property
     def debug_data(self) -> OCPDebugData:
         pass
+    
+    @debug_data.setter
+    def debug_data(self, value: OCPDebugData) -> None:
+        pass
\ No newline at end of file

From 841493eee3f56c775b0de6546decdf9420990504 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Mon, 6 Jan 2025 12:53:45 +0000
Subject: [PATCH 56/95] [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
---
 agimus_controller/agimus_controller/ocp_base.py       | 2 +-
 agimus_controller/agimus_controller/ocp_base_croco.py | 6 +++---
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base.py b/agimus_controller/agimus_controller/ocp_base.py
index 255ab875..c0f8b96b 100644
--- a/agimus_controller/agimus_controller/ocp_base.py
+++ b/agimus_controller/agimus_controller/ocp_base.py
@@ -70,7 +70,7 @@ def ocp_results(self) -> OCPResults:
             OCPResults: Class containing the results of the OCP solver.
         """
         pass
-    
+
     @ocp_results.setter
     def ocp_results(self, value: OCPResults) -> None:
         """Set the output data structure of the OCP.
diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 83864378..f34ad3fd 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -66,7 +66,7 @@ def solve(
         x_warmstart: list[npt.NDArray[np.float64]],
         u_warmstart: list[npt.NDArray[np.float64]],
     ) -> None:
-        """ Solves the OCP.
+        """Solves the OCP.
         The results can be accessed through the ocp_results property.
 
         Args:
@@ -124,7 +124,7 @@ def ocp_results(self, value: OCPResults) -> None:
     @property
     def debug_data(self) -> OCPDebugData:
         pass
-    
+
     @debug_data.setter
     def debug_data(self, value: OCPDebugData) -> None:
-        pass
\ No newline at end of file
+        pass

From 402968b1d7f763ddc7191ded17cddcf0f4a2c03f Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Mon, 6 Jan 2025 13:01:49 +0000
Subject: [PATCH 57/95] switching to snake case

---
 .../tests/test_ocp_croco_base.py              | 70 +++++++++----------
 1 file changed, 35 insertions(+), 35 deletions(-)

diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index 04ae7f78..96141653 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -41,11 +41,11 @@ def setUp(self):
         # Create a concrete implementation of OCPBaseCroco
         class TestOCPCroco(OCPBaseCroco):
             @property
-            def runningModelList(self):
+            def running_model_list(self):
                 return None
 
             @property
-            def terminalModel(self):
+            def terminal_model(self):
                 return None
 
             def set_reference_horizon(self, horizon_size):
@@ -65,87 +65,87 @@ def test_dt(self):
 class TestSimpleOCPCroco(unittest.TestCase):
     class TestOCPCroco(OCPBaseCroco):
         @property
-        def runningModelList(self):
+        def running_model_list(self):
             # Running cost model
-            runningCostModel = crocoddyl.CostModelSum(self._state)
+            running_cost_model = crocoddyl.CostModelSum(self._state)
 
             ### Creation of cost terms
             # State Regularization cost
-            xResidual = crocoddyl.ResidualModelState(
+            x_residual = crocoddyl.ResidualModelState(
                 self._state,
                 np.concatenate((pin.neutral(self._rmodel), np.zeros(self._rmodel.nv))),
             )
-            xRegCost = crocoddyl.CostModelResidual(self._state, xResidual)
+            x_reg_cost = crocoddyl.CostModelResidual(self._state, x_residual)
 
             # Control Regularization cost
-            uResidual = crocoddyl.ResidualModelControl(self._state)
-            uRegCost = crocoddyl.CostModelResidual(self._state, uResidual)
+            u_residual = crocoddyl.ResidualModelControl(self._state)
+            u_reg_cost = crocoddyl.CostModelResidual(self._state, u_residual)
 
             # End effector frame cost
-            framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
+            frame_placement_residual = crocoddyl.ResidualModelFramePlacement(
                 self._state,
                 self._rmodel.getFrameId("panda_hand_tcp"),
                 pin.SE3(np.eye(3), np.array([1.0, 1.0, 1.0])),
             )
 
-            goalTrackingCost = crocoddyl.CostModelResidual(
-                self._state, framePlacementResidual
+            goal_tracking_cost = crocoddyl.CostModelResidual(
+                self._state, frame_placement_residual
             )
-            runningCostModel.addCost("stateReg", xRegCost, 0.1)
-            runningCostModel.addCost("ctrlRegGrav", uRegCost, 0.0001)
-            runningCostModel.addCost("gripperPoseRM", goalTrackingCost, 1.0)
+            running_cost_model.addCost("stateReg", x_reg_cost, 0.1)
+            running_cost_model.addCost("ctrlRegGrav", u_reg_cost, 0.0001)
+            running_cost_model.addCost("gripperPoseRM", goal_tracking_cost, 1.0)
             # Create Differential Action Model (DAM), i.e. continuous dynamics and cost functions
             running_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics(
                 self._state,
                 self._actuation,
-                runningCostModel,
+                running_cost_model,
             )
-            runningModel = crocoddyl.IntegratedActionModelEuler(
+            running_model = crocoddyl.IntegratedActionModelEuler(
                 running_DAM,
             )
-            runningModel.differential.armature = self._robot_model.armature
+            running_model.differential.armature = self._robot_model.armature
 
-            runningModelList = [runningModel] * (self._ocp_params.T - 1)
-            return runningModelList
+            running_model_list = [running_model] * (self._ocp_params.T - 1)
+            return running_model_list
 
         @property
         def terminalModel(self):
             # Terminal cost models
-            terminalCostModel = crocoddyl.CostModelSum(self._state)
+            terminal_cost_model = crocoddyl.CostModelSum(self._state)
 
             ### Creation of cost terms
+           ### Creation of cost terms
             # State Regularization cost
-            xResidual = crocoddyl.ResidualModelState(
+            x_residual = crocoddyl.ResidualModelState(
                 self._state,
                 np.concatenate((pin.neutral(self._rmodel), np.zeros(self._rmodel.nv))),
             )
-            xRegCost = crocoddyl.CostModelResidual(self._state, xResidual)
+            x_reg_cost = crocoddyl.CostModelResidual(self._state, x_residual)
 
             # End effector frame cost
-            framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
+            frame_placement_residual = crocoddyl.ResidualModelFramePlacement(
                 self._state,
                 self._rmodel.getFrameId("panda_hand_tcp"),
                 pin.SE3(np.eye(3), np.array([1.0, 1.0, 1.0])),
             )
 
-            goalTrackingCost = crocoddyl.CostModelResidual(
-                self._state, framePlacementResidual
+            goal_tracking_cost = crocoddyl.CostModelResidual(
+                self._state, frame_placement_residual
             )
-
-            terminalCostModel.addCost("stateReg", xRegCost, 0.1)
-            terminalCostModel.addCost("gripperPose", goalTrackingCost, 50)
+            terminal_cost_model.addCost("stateReg", x_reg_cost, 0.1)
+            terminal_cost_model.addCost("gripperPose", goal_tracking_cost, 50)
 
             # Create Differential Action Model (DAM), i.e. continuous dynamics and cost functions
             terminal_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics(
                 self._state,
                 self._actuation,
-                terminalCostModel,
+                terminal_cost_model,
             )
 
-            terminalModel = crocoddyl.IntegratedActionModelEuler(terminal_DAM, 0.0)
-            terminalModel.differential.armature = self._robot_model.armature
+            terminal_model = crocoddyl.IntegratedActionModelEuler(terminal_DAM, 0.0)
+            terminal_model.differential.armature = self._robot_model.armature
 
-            return terminalModel
+            return terminal_model
 
         def set_reference_horizon(self, horizon_size):
             ### Not implemented in this OCP example.
@@ -199,19 +199,19 @@ def test_check_results(self):
         # Checking the states
         for iter, state in enumerate(results[0]):
             np.testing.assert_array_almost_equal(
-                state, self.ocp.ocp_results.states.tolist()[iter]
+                state, self.ocp.ocp_results.states.tolist()[iter], err_msg="States are not equal"
             )
 
         # Checking the ricatti gains
         for iter, gain in enumerate(results[1]):
             np.testing.assert_array_almost_equal(
-                gain, self.ocp.ocp_results.ricatti_gains.tolist()[iter]
+                gain, self.ocp.ocp_results.ricatti_gains.tolist()[iter], err_msg="Ricatti gains are not equal"
             )
 
         # Checking the feed forward terms
         for iter, term in enumerate(results[2]):
             np.testing.assert_array_almost_equal(
-                term, self.ocp.ocp_results.feed_forward_terms.tolist()[iter]
+                term, self.ocp.ocp_results.feed_forward_terms.tolist()[iter], err_msg="Feed forward term are not equal"
             )
 
 

From d6ca98bc8c954432fd5f957e6d70dd60c7bd255d Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Mon, 6 Jan 2025 13:04:07 +0000
Subject: [PATCH 58/95] fixing typo

---
 agimus_controller/agimus_controller/ocp_base_croco.py | 2 +-
 agimus_controller/tests/test_ocp_croco_base.py        | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 83864378..05f971b8 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -77,7 +77,7 @@ def solve(
         ### Creation of the state and actuation models
 
         problem = crocoddyl.ShootingProblem(
-            x0, self.runningModelList, self.terminalModel
+            x0, self.running_model_list, self.terminal_model
         )
         # Create solver + callbacks
         ocp = mim_solvers.SolverCSQP(problem)
diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index 96141653..64a9c53f 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -109,7 +109,7 @@ def running_model_list(self):
             return running_model_list
 
         @property
-        def terminalModel(self):
+        def terminal_model(self):
             # Terminal cost models
             terminal_cost_model = crocoddyl.CostModelSum(self._state)
 

From 55c309566e19b0bcdacfd68ce4c0c69d8a3d9a90 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Mon, 6 Jan 2025 13:08:29 +0000
Subject: [PATCH 59/95] change t to horizon size

---
 .../agimus_controller/ocp_base_croco.py              |  2 +-
 .../agimus_controller/ocp_param_base.py              |  2 +-
 agimus_controller/tests/test_ocp_croco_base.py       | 12 ++++++------
 agimus_controller/tests/test_ocp_param_base.py       | 10 +++++-----
 4 files changed, 13 insertions(+), 13 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 05f971b8..bb77b1bf 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -41,7 +41,7 @@ def __init__(
     @property
     def horizon_size(self) -> int:
         """Number of time steps in the horizon."""
-        return self._ocp_params.T
+        return self._ocp_params.horizon_size
 
     @property
     def dt(self) -> float:
diff --git a/agimus_controller/agimus_controller/ocp_param_base.py b/agimus_controller/agimus_controller/ocp_param_base.py
index fd36f727..81735d59 100644
--- a/agimus_controller/agimus_controller/ocp_param_base.py
+++ b/agimus_controller/agimus_controller/ocp_param_base.py
@@ -9,7 +9,7 @@ class OCPParamsBaseCroco:
 
     # Data relevant to solve the OCP
     dt: np.float64  # Integration step of the OCP
-    T: int  # Number of time steps in the horizon
+    horizon_size: int  # Number of time steps in the horizon
     solver_iters: int  # Number of solver iterations
     qp_iters: int = 200  # Number of QP iterations (must be a multiple of 25).
     termination_tolerance: np.float64 = (
diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index 64a9c53f..7cdf4f84 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -28,7 +28,7 @@ def setUp(self):
         self.mock_ocp_params = MagicMock(spec=OCPParamsBaseCroco)
 
         # Set mock parameters
-        self.mock_ocp_params.T = 10
+        self.mock_ocp_params.horizon_size = 10
         self.mock_ocp_params.dt = 0.1
         self.mock_ocp_params.use_filter_line_search = True
         self.mock_ocp_params.termination_tolerance = 1e-6
@@ -55,7 +55,7 @@ def set_reference_horizon(self, horizon_size):
 
     def test_horizon_size(self):
         """Test the horizon_size property."""
-        self.assertEqual(self.ocp.horizon_size, self.mock_ocp_params.T)
+        self.assertEqual(self.ocp.horizon_size, self.mock_ocp_params.horizon_size)
 
     def test_dt(self):
         """Test the dt property."""
@@ -105,7 +105,7 @@ def running_model_list(self):
             )
             running_model.differential.armature = self._robot_model.armature
 
-            running_model_list = [running_model] * (self._ocp_params.T - 1)
+            running_model_list = [running_model] * (self._ocp_params.horizon_size - 1)
             return running_model_list
 
         @property
@@ -166,15 +166,15 @@ def setUp(self):
 
         # Set mock parameters
         self.ocp_params = OCPParamsBaseCroco(
-            dt=0.1, T=10, solver_iters=100, callbacks=True
+            dt=0.1, horizon_size=10, solver_iters=100, callbacks=True
         )
         self.state_reg = np.concatenate(
             (pin.neutral(robot_model), np.zeros(robot_model.nv))
         )
         self.state_warmstart = [np.zeros(robot_model.nq + robot_model.nv)] * (
-            self.ocp_params.T - 1
+            self.ocp_params.horizon_size - 1
         )  # The first state is the current state
-        self.control_warmstart = [np.zeros(robot_model.nq)] * (self.ocp_params.T - 1)
+        self.control_warmstart = [np.zeros(robot_model.nq)] * (self.ocp_params.horizon_size - 1)
         # Create a concrete implementation of OCPBaseCroco
         self.ocp = self.TestOCPCroco(self.robot_model_factory, self.ocp_params)
         self.ocp.solve(self.state_reg, self.state_warmstart, self.control_warmstart)
diff --git a/agimus_controller/tests/test_ocp_param_base.py b/agimus_controller/tests/test_ocp_param_base.py
index bcb35b58..98108f21 100644
--- a/agimus_controller/tests/test_ocp_param_base.py
+++ b/agimus_controller/tests/test_ocp_param_base.py
@@ -27,7 +27,7 @@ def test_initialization(self):
 
         Test Parameters:
         - dt (float): Time step for the OCP solver.
-        - T (int): Total number of time steps.
+        - horizon_size (int): Total number of time steps.
         - solver_iters (int): Number of solver iterations.
         - qp_iters (int): Number of QP iterations.
         - termination_tolerance (float): Tolerance for termination criteria.
@@ -37,7 +37,7 @@ def test_initialization(self):
 
         Assertions:
         - Asserts that the instance attribute `dt` is equal to the input `dt`.
-        - Asserts that the instance attribute `T` is equal to the input `T`.
+        - Asserts that the instance attribute `horizon_size` is equal to the input `horizon_size`.
         - Asserts that the instance attribute `qp_iters` is equal to the input `qp_iters`.
         - Asserts that the instance attribute `solver_iters` is equal to the input `solver_iters`.
         - Asserts that the instance attribute `termination_tolerance` is equal to the input `termination_tolerance`.
@@ -46,7 +46,7 @@ def test_initialization(self):
         - Asserts that the instance attribute `callbacks` is False.
         """
         dt = np.float64(0.01)
-        T = 100
+        horizon_size = 100
         solver_iters = 50
         qp_iters = 200
         termination_tolerance = 1e-3
@@ -55,7 +55,7 @@ def test_initialization(self):
         callbacks = False
         params = OCPParamsBaseCroco(
             dt=dt,
-            T=T,
+            horizon_size=horizon_size,
             solver_iters=solver_iters,
             qp_iters=qp_iters,
             termination_tolerance=termination_tolerance,
@@ -64,7 +64,7 @@ def test_initialization(self):
             callbacks=callbacks,
         )
         self.assertEqual(params.dt, dt)
-        self.assertEqual(params.T, T)
+        self.assertEqual(params.horizon_size, horizon_size)
         self.assertEqual(params.qp_iters, qp_iters)
         self.assertEqual(params.solver_iters, solver_iters)
         self.assertEqual(params.termination_tolerance, termination_tolerance)

From b8eb8ab7577483e43ea5a48e58766c785870b492 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Mon, 6 Jan 2025 13:09:35 +0000
Subject: [PATCH 60/95] [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
---
 agimus_controller/tests/test_ocp_croco_base.py | 18 +++++++++++++-----
 1 file changed, 13 insertions(+), 5 deletions(-)

diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index 7cdf4f84..a370c494 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -114,7 +114,7 @@ def terminal_model(self):
             terminal_cost_model = crocoddyl.CostModelSum(self._state)
 
             ### Creation of cost terms
-           ### Creation of cost terms
+            ### Creation of cost terms
             # State Regularization cost
             x_residual = crocoddyl.ResidualModelState(
                 self._state,
@@ -174,7 +174,9 @@ def setUp(self):
         self.state_warmstart = [np.zeros(robot_model.nq + robot_model.nv)] * (
             self.ocp_params.horizon_size - 1
         )  # The first state is the current state
-        self.control_warmstart = [np.zeros(robot_model.nq)] * (self.ocp_params.horizon_size - 1)
+        self.control_warmstart = [np.zeros(robot_model.nq)] * (
+            self.ocp_params.horizon_size - 1
+        )
         # Create a concrete implementation of OCPBaseCroco
         self.ocp = self.TestOCPCroco(self.robot_model_factory, self.ocp_params)
         self.ocp.solve(self.state_reg, self.state_warmstart, self.control_warmstart)
@@ -199,19 +201,25 @@ def test_check_results(self):
         # Checking the states
         for iter, state in enumerate(results[0]):
             np.testing.assert_array_almost_equal(
-                state, self.ocp.ocp_results.states.tolist()[iter], err_msg="States are not equal"
+                state,
+                self.ocp.ocp_results.states.tolist()[iter],
+                err_msg="States are not equal",
             )
 
         # Checking the ricatti gains
         for iter, gain in enumerate(results[1]):
             np.testing.assert_array_almost_equal(
-                gain, self.ocp.ocp_results.ricatti_gains.tolist()[iter], err_msg="Ricatti gains are not equal"
+                gain,
+                self.ocp.ocp_results.ricatti_gains.tolist()[iter],
+                err_msg="Ricatti gains are not equal",
             )
 
         # Checking the feed forward terms
         for iter, term in enumerate(results[2]):
             np.testing.assert_array_almost_equal(
-                term, self.ocp.ocp_results.feed_forward_terms.tolist()[iter], err_msg="Feed forward term are not equal"
+                term,
+                self.ocp.ocp_results.feed_forward_terms.tolist()[iter],
+                err_msg="Feed forward term are not equal",
             )
 
 

From 72e21854c1327a5319073004512085f05cbab1fa Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Mon, 6 Jan 2025 13:36:13 +0000
Subject: [PATCH 61/95] methods instead of properties for the croco models

---
 .../agimus_controller/ocp_base_croco.py         | 17 ++++++++++-------
 agimus_controller/tests/test_ocp_croco_base.py  | 12 ++++--------
 2 files changed, 14 insertions(+), 15 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 458c4b50..693668c0 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -48,16 +48,14 @@ def dt(self) -> float:
         """Integration step of the OCP."""
         return self._ocp_params.dt
 
-    @property
     @abstractmethod
-    def running_model_list(self) -> list[crocoddyl.ActionModelAbstract]:
-        """List of running models."""
+    def create_running_model_list(self) -> list[crocoddyl.ActionModelAbstract]:
+        """Create the list of running models."""
         pass
 
-    @property
     @abstractmethod
-    def terminal_model(self) -> crocoddyl.ActionModelAbstract:
-        """Terminal model."""
+    def create_terminal_model(self) -> crocoddyl.ActionModelAbstract:
+        """Create the terminal model."""
         pass
 
     def solve(
@@ -75,7 +73,12 @@ def solve(
             u_warmstart (list[npt.NDArray[np.float64]]): Predicted control inputs for the OCP.
         """
         ### Creation of the state and actuation models
-
+        # Create the running models
+        self.running_model_list = self.create_running_model_list()
+        # Create the terminal model
+        self.terminal_model = self.create_terminal_model()
+        
+        # Create the shooting problem
         problem = crocoddyl.ShootingProblem(
             x0, self.running_model_list, self.terminal_model
         )
diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index a370c494..329274a0 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -40,12 +40,10 @@ def setUp(self):
 
         # Create a concrete implementation of OCPBaseCroco
         class TestOCPCroco(OCPBaseCroco):
-            @property
-            def running_model_list(self):
+            def create_running_model_list(self):
                 return None
 
-            @property
-            def terminal_model(self):
+            def create_terminal_model(self):
                 return None
 
             def set_reference_horizon(self, horizon_size):
@@ -64,8 +62,7 @@ def test_dt(self):
 
 class TestSimpleOCPCroco(unittest.TestCase):
     class TestOCPCroco(OCPBaseCroco):
-        @property
-        def running_model_list(self):
+        def create_running_model_list(self):
             # Running cost model
             running_cost_model = crocoddyl.CostModelSum(self._state)
 
@@ -108,8 +105,7 @@ def running_model_list(self):
             running_model_list = [running_model] * (self._ocp_params.horizon_size - 1)
             return running_model_list
 
-        @property
-        def terminal_model(self):
+        def create_terminal_model(self):
             # Terminal cost models
             terminal_cost_model = crocoddyl.CostModelSum(self._state)
 

From b6266d00744f4f460d02c0a64d3cc815a58257b5 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Mon, 6 Jan 2025 13:36:27 +0000
Subject: [PATCH 62/95] [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
---
 agimus_controller/agimus_controller/ocp_base_croco.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 693668c0..e64dbc5b 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -77,7 +77,7 @@ def solve(
         self.running_model_list = self.create_running_model_list()
         # Create the terminal model
         self.terminal_model = self.create_terminal_model()
-        
+
         # Create the shooting problem
         problem = crocoddyl.ShootingProblem(
             x0, self.running_model_list, self.terminal_model

From 303dd30112edcf278335385c578f274cae2e6a83 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Tue, 7 Jan 2025 10:59:55 +0100
Subject: [PATCH 63/95] Update
 agimus_controller/agimus_controller/ocp_base_croco.py

Co-authored-by: TheoMF <140606863+TheoMF@users.noreply.github.com>
---
 .../agimus_controller/ocp_base_croco.py       | 33 ++++++++++---------
 1 file changed, 17 insertions(+), 16 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index e64dbc5b..712f5832 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -79,25 +79,26 @@ def solve(
         self.terminal_model = self.create_terminal_model()
 
         # Create the shooting problem
-        problem = crocoddyl.ShootingProblem(
-            x0, self.running_model_list, self.terminal_model
-        )
-        # Create solver + callbacks
-        ocp = mim_solvers.SolverCSQP(problem)
-
-        # Merit function
-        ocp.use_filter_line_search = self._ocp_params.use_filter_line_search
-
-        # Parameters of the solver
-        ocp.termination_tolerance = self._ocp_params.termination_tolerance
-        ocp.max_qp_iters = self._ocp_params.qp_iters
-        ocp.eps_abs = self._ocp_params.eps_abs
-        ocp.eps_rel = self._ocp_params.eps_rel
-        ocp.with_callbacks = self._ocp_params.callbacks
+        if self._ocp is None:
+            self._problem = crocoddyl.ShootingProblem(
+                x0, self.running_model_list, self.terminal_model
+            )
+            # Create solver + callbacks
+            self._ocp = mim_solvers.SolverCSQP(self._problem)
+    
+            # Merit function
+            self._ocp.use_filter_line_search = self._ocp_params.use_filter_line_search
+    
+            # Parameters of the solver
+            self._ocp.termination_tolerance = self._ocp_params.termination_tolerance
+            self._ocp.max_qp_iters = self._ocp_params.qp_iters
+            self._ocp.eps_abs = self._ocp_params.eps_abs
+            self._ocp.eps_rel = self._ocp_params.eps_rel
+            self._ocp.with_callbacks = self._ocp_params.callbacks
 
         # Creating the warmstart lists for the solver
         # Solve the OCP
-        ocp.solve([x0] + x_warmstart, u_warmstart, self._ocp_params.solver_iters)
+        self._ocp.solve([x0] + x_warmstart, u_warmstart, self._ocp_params.solver_iters)
 
         # Store the results
         self.ocp_results = OCPResults(

From 0dfe4508bc6a22ecf7f601932c5e3a3a7d1ed8be Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Tue, 7 Jan 2025 10:00:03 +0000
Subject: [PATCH 64/95] [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
---
 agimus_controller/agimus_controller/ocp_base_croco.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 712f5832..acc75064 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -85,10 +85,10 @@ def solve(
             )
             # Create solver + callbacks
             self._ocp = mim_solvers.SolverCSQP(self._problem)
-    
+
             # Merit function
             self._ocp.use_filter_line_search = self._ocp_params.use_filter_line_search
-    
+
             # Parameters of the solver
             self._ocp.termination_tolerance = self._ocp_params.termination_tolerance
             self._ocp.max_qp_iters = self._ocp_params.qp_iters

From 6b731cad803463a2bc4211ef4a49af13b5073a2f Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Tue, 7 Jan 2025 13:09:34 +0100
Subject: [PATCH 65/95] Update agimus_controller/tests/test_ocp_param_base.py

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 agimus_controller/tests/test_ocp_param_base.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/agimus_controller/tests/test_ocp_param_base.py b/agimus_controller/tests/test_ocp_param_base.py
index 98108f21..a27b01bb 100644
--- a/agimus_controller/tests/test_ocp_param_base.py
+++ b/agimus_controller/tests/test_ocp_param_base.py
@@ -6,7 +6,7 @@
 
 class TestOCPParamsCrocoBase(unittest.TestCase):
     """
-    TestOCPParamsCrocoBase is a unit test class for testing the OCPParamsBaseCroco class.
+    TestOCPParamsCrocoBase unittests parameters settters and getters of OCPParamsBaseCroco class.
 
     Methods:
         __init__(methodName="runTest"): Initializes the test case instance.

From 7c703d6e31dd6ff0d526dcc71ccc7424fb62fd67 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Tue, 7 Jan 2025 13:10:22 +0100
Subject: [PATCH 66/95] Update agimus_controller/tests/test_ocp_param_base.py

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 .../tests/test_ocp_param_base.py              | 40 +++++++------------
 1 file changed, 14 insertions(+), 26 deletions(-)

diff --git a/agimus_controller/tests/test_ocp_param_base.py b/agimus_controller/tests/test_ocp_param_base.py
index a27b01bb..b1e23555 100644
--- a/agimus_controller/tests/test_ocp_param_base.py
+++ b/agimus_controller/tests/test_ocp_param_base.py
@@ -45,32 +45,20 @@ def test_initialization(self):
         - Asserts that the instance attribute `eps_rel` is equal to the input `eps_rel`.
         - Asserts that the instance attribute `callbacks` is False.
         """
-        dt = np.float64(0.01)
-        horizon_size = 100
-        solver_iters = 50
-        qp_iters = 200
-        termination_tolerance = 1e-3
-        eps_abs = 1e-6
-        eps_rel = 0
-        callbacks = False
-        params = OCPParamsBaseCroco(
-            dt=dt,
-            horizon_size=horizon_size,
-            solver_iters=solver_iters,
-            qp_iters=qp_iters,
-            termination_tolerance=termination_tolerance,
-            eps_abs=eps_abs,
-            eps_rel=eps_rel,
-            callbacks=callbacks,
-        )
-        self.assertEqual(params.dt, dt)
-        self.assertEqual(params.horizon_size, horizon_size)
-        self.assertEqual(params.qp_iters, qp_iters)
-        self.assertEqual(params.solver_iters, solver_iters)
-        self.assertEqual(params.termination_tolerance, termination_tolerance)
-        self.assertEqual(params.eps_abs, eps_abs)
-        self.assertEqual(params.eps_rel, eps_rel)
-        self.assertFalse(params.callbacks)
+        params = {
+            "dt": np.float64(0.01),
+            "horizon_size": 100,
+            "solver_iters": 50,
+            "qp_iters": 200,
+            "termination_tolerance": 1e-3,
+            "eps_abs": 1e-6,
+            "eps_rel": 0,
+            "callbacks": False,
+        }
+        params = OCPParamsBaseCroco(**params)
+        for key, val in params:
+            res = getattr(params, key)
+            self.assertEqual(res, val, f"Value missmatch for parameter '{key}'. Expected: '{val}', got: '{res}'")
 
 
 if __name__ == "__main__":

From 25d83449d407c53905b7d206ef11862055c46a16 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Tue, 7 Jan 2025 12:10:35 +0000
Subject: [PATCH 67/95] [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
---
 agimus_controller/tests/test_ocp_param_base.py | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/agimus_controller/tests/test_ocp_param_base.py b/agimus_controller/tests/test_ocp_param_base.py
index b1e23555..f7648115 100644
--- a/agimus_controller/tests/test_ocp_param_base.py
+++ b/agimus_controller/tests/test_ocp_param_base.py
@@ -58,7 +58,11 @@ def test_initialization(self):
         params = OCPParamsBaseCroco(**params)
         for key, val in params:
             res = getattr(params, key)
-            self.assertEqual(res, val, f"Value missmatch for parameter '{key}'. Expected: '{val}', got: '{res}'")
+            self.assertEqual(
+                res,
+                val,
+                f"Value missmatch for parameter '{key}'. Expected: '{val}', got: '{res}'",
+            )
 
 
 if __name__ == "__main__":

From d2b9107fbea68019f0fd316937fd0c98e490e4b9 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Tue, 7 Jan 2025 13:14:22 +0100
Subject: [PATCH 68/95] Update agimus_controller/tests/test_ocp_croco_base.py

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 agimus_controller/tests/test_ocp_croco_base.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index 329274a0..e34f59ca 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -147,7 +147,7 @@ def set_reference_horizon(self, horizon_size):
             ### Not implemented in this OCP example.
             return None
 
-    def setUp(self):
+    def setUpClass(self):
         # Mock the RobotModelFactory and OCPParamsCrocoBase
         self.robot_model_factory = RobotModelFactory()
 

From ad39d409731a857e50cce5dad9348e65c54efb83 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Tue, 7 Jan 2025 13:14:42 +0100
Subject: [PATCH 69/95] Update agimus_controller/agimus_controller/ocp_base.py

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 agimus_controller/agimus_controller/ocp_base.py | 2 --
 1 file changed, 2 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base.py b/agimus_controller/agimus_controller/ocp_base.py
index c0f8b96b..cc1c2327 100644
--- a/agimus_controller/agimus_controller/ocp_base.py
+++ b/agimus_controller/agimus_controller/ocp_base.py
@@ -9,8 +9,6 @@
 
 class OCPBase(ABC):
     """Base class for the Optimal Control Problem (OCP) solver. This class defines the interface for the OCP solver.
-    If you want to implement a new OCP solver, you should derive from this class and implement the abstract methods.
-    If you want to use Crocoddyl, you should inherit from the OCPCrocoBase class instead.
     """
 
     def __init__(self) -> None:

From 2885b0a08b45005be0e7db8d21609772ca501eac Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Tue, 7 Jan 2025 12:17:38 +0000
Subject: [PATCH 70/95] [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
---
 agimus_controller/agimus_controller/ocp_base.py | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base.py b/agimus_controller/agimus_controller/ocp_base.py
index cc1c2327..955940f4 100644
--- a/agimus_controller/agimus_controller/ocp_base.py
+++ b/agimus_controller/agimus_controller/ocp_base.py
@@ -8,8 +8,7 @@
 
 
 class OCPBase(ABC):
-    """Base class for the Optimal Control Problem (OCP) solver. This class defines the interface for the OCP solver.
-    """
+    """Base class for the Optimal Control Problem (OCP) solver. This class defines the interface for the OCP solver."""
 
     def __init__(self) -> None:
         pass

From 8083c11350a4d335666748252a307f8ded28a3af Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Tue, 7 Jan 2025 13:53:40 +0000
Subject: [PATCH 71/95] getting rid of useless docstrings

---
 .../tests/test_ocp_param_base.py              | 23 -------------------
 1 file changed, 23 deletions(-)

diff --git a/agimus_controller/tests/test_ocp_param_base.py b/agimus_controller/tests/test_ocp_param_base.py
index f7648115..ceecd9d8 100644
--- a/agimus_controller/tests/test_ocp_param_base.py
+++ b/agimus_controller/tests/test_ocp_param_base.py
@@ -21,29 +21,6 @@ def __init__(self, methodName="runTest"):
     def test_initialization(self):
         """
         Test the initialization of the OCPParamsBaseCroco class.
-
-        This test verifies that the parameters passed to the OCPParamsBaseCroco
-        constructor are correctly assigned to the instance attributes.
-
-        Test Parameters:
-        - dt (float): Time step for the OCP solver.
-        - horizon_size (int): Total number of time steps.
-        - solver_iters (int): Number of solver iterations.
-        - qp_iters (int): Number of QP iterations.
-        - termination_tolerance (float): Tolerance for termination criteria.
-        - eps_abs (float): Absolute tolerance for the solver.
-        - eps_rel (float): Relative tolerance for the solver.
-        - callbacks (bool): Flag to enable or disable callbacks.
-
-        Assertions:
-        - Asserts that the instance attribute `dt` is equal to the input `dt`.
-        - Asserts that the instance attribute `horizon_size` is equal to the input `horizon_size`.
-        - Asserts that the instance attribute `qp_iters` is equal to the input `qp_iters`.
-        - Asserts that the instance attribute `solver_iters` is equal to the input `solver_iters`.
-        - Asserts that the instance attribute `termination_tolerance` is equal to the input `termination_tolerance`.
-        - Asserts that the instance attribute `eps_abs` is equal to the input `eps_abs`.
-        - Asserts that the instance attribute `eps_rel` is equal to the input `eps_rel`.
-        - Asserts that the instance attribute `callbacks` is False.
         """
         params = {
             "dt": np.float64(0.01),

From c1f61268e7c5361150c494d2f43d097140c215df Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Tue, 7 Jan 2025 13:54:16 +0000
Subject: [PATCH 72/95] displacing the creation of running model & terminal
 model in the if statement

---
 .../agimus_controller/ocp_base_croco.py          | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index acc75064..b063486c 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -73,13 +73,13 @@ def solve(
             u_warmstart (list[npt.NDArray[np.float64]]): Predicted control inputs for the OCP.
         """
         ### Creation of the state and actuation models
-        # Create the running models
-        self.running_model_list = self.create_running_model_list()
-        # Create the terminal model
-        self.terminal_model = self.create_terminal_model()
 
-        # Create the shooting problem
         if self._ocp is None:
+            # Create the running models
+            self.running_model_list = self.create_running_model_list()
+            # Create the terminal model
+            self.terminal_model = self.create_terminal_model()
+            # Create the shooting problem
             self._problem = crocoddyl.ShootingProblem(
                 x0, self.running_model_list, self.terminal_model
             )
@@ -102,9 +102,9 @@ def solve(
 
         # Store the results
         self.ocp_results = OCPResults(
-            states=ocp.xs,
-            ricatti_gains=ocp.K,
-            feed_forward_terms=ocp.us,
+            states=self._ocp.xs,
+            ricatti_gains=self._ocp.K,
+            feed_forward_terms=self._ocp.us,
         )
 
     @property

From 0cc5936de0057e448a95df24a717a045d9615767 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Tue, 7 Jan 2025 14:11:23 +0000
Subject: [PATCH 73/95] proper type hinting

---
 agimus_controller/agimus_controller/ocp_param_base.py | 8 ++++----
 agimus_controller/tests/test_ocp_param_base.py        | 8 +-------
 2 files changed, 5 insertions(+), 11 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_param_base.py b/agimus_controller/agimus_controller/ocp_param_base.py
index 81735d59..040d2691 100644
--- a/agimus_controller/agimus_controller/ocp_param_base.py
+++ b/agimus_controller/agimus_controller/ocp_param_base.py
@@ -8,14 +8,14 @@ class OCPParamsBaseCroco:
     """Input data structure of the OCP."""
 
     # Data relevant to solve the OCP
-    dt: np.float64  # Integration step of the OCP
+    dt: float  # Integration step of the OCP
     horizon_size: int  # Number of time steps in the horizon
     solver_iters: int  # Number of solver iterations
     qp_iters: int = 200  # Number of QP iterations (must be a multiple of 25).
-    termination_tolerance: np.float64 = (
+    termination_tolerance: float = (
         1e-3  # Termination tolerance (norm of the KKT conditions)
     )
-    eps_abs: np.float64 = 1e-6  # Absolute tolerance of the solver
-    eps_rel: np.float64 = 0.0  # Relative tolerance of the solver
+    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
     use_filter_line_search = False  # Flag to enable/disable the filter line searchs
diff --git a/agimus_controller/tests/test_ocp_param_base.py b/agimus_controller/tests/test_ocp_param_base.py
index ceecd9d8..31ab1f19 100644
--- a/agimus_controller/tests/test_ocp_param_base.py
+++ b/agimus_controller/tests/test_ocp_param_base.py
@@ -7,14 +7,8 @@
 class TestOCPParamsCrocoBase(unittest.TestCase):
     """
     TestOCPParamsCrocoBase unittests parameters settters and getters of OCPParamsBaseCroco class.
-
-    Methods:
-        __init__(methodName="runTest"): Initializes the test case instance.
-        test_initialization(): Tests the initialization of the OCPParamsBaseCroco class with various parameters.
     """
 
-    """Test the OCPParamsBaseCroco class."""
-
     def __init__(self, methodName="runTest"):
         super().__init__(methodName)
 
@@ -23,7 +17,7 @@ def test_initialization(self):
         Test the initialization of the OCPParamsBaseCroco class.
         """
         params = {
-            "dt": np.float64(0.01),
+            "dt": 0.01,
             "horizon_size": 100,
             "solver_iters": 50,
             "qp_iters": 200,

From 9e5c374bc9d9a694767b7179148f1a9902d159fe Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Tue, 7 Jan 2025 14:14:37 +0000
Subject: [PATCH 74/95] fixing unit tests

---
 agimus_controller/agimus_controller/ocp_base_croco.py | 2 +-
 agimus_controller/tests/test_ocp_croco_base.py        | 6 ++++--
 2 files changed, 5 insertions(+), 3 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index b063486c..f450e351 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -35,7 +35,7 @@ def __init__(
 
         # Setting the OCP parameters
         self._ocp_params = ocp_params
-
+        self._ocp = None
         self._ocp_results = None
 
     @property
diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index e34f59ca..67514a0a 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -12,7 +12,8 @@
 
 
 class TestOCPBaseCroco(unittest.TestCase):
-    def setUp(self):
+    @classmethod
+    def setUpClass(self):
         # Mock the RobotModelFactory and OCPParamsCrocoBase
         self.mock_robot_model_factory = RobotModelFactory()
 
@@ -146,7 +147,8 @@ def create_terminal_model(self):
         def set_reference_horizon(self, horizon_size):
             ### Not implemented in this OCP example.
             return None
-
+    
+    @classmethod
     def setUpClass(self):
         # Mock the RobotModelFactory and OCPParamsCrocoBase
         self.robot_model_factory = RobotModelFactory()

From d9e49cc6f24bd71ef726df1674aac9bf6f2c1337 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Tue, 7 Jan 2025 14:14:49 +0000
Subject: [PATCH 75/95] [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
---
 agimus_controller/tests/test_ocp_croco_base.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index 67514a0a..fabedaea 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -147,7 +147,7 @@ def create_terminal_model(self):
         def set_reference_horizon(self, horizon_size):
             ### Not implemented in this OCP example.
             return None
-    
+
     @classmethod
     def setUpClass(self):
         # Mock the RobotModelFactory and OCPParamsCrocoBase

From 88d244936298ed2b381b0acdf614395ed9f70dc7 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Tue, 7 Jan 2025 15:22:11 +0100
Subject: [PATCH 76/95] Update agimus_controller/agimus_controller/ocp_base.py

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 agimus_controller/agimus_controller/ocp_base.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/agimus_controller/agimus_controller/ocp_base.py b/agimus_controller/agimus_controller/ocp_base.py
index 955940f4..ed2a4947 100644
--- a/agimus_controller/agimus_controller/ocp_base.py
+++ b/agimus_controller/agimus_controller/ocp_base.py
@@ -8,7 +8,9 @@
 
 
 class OCPBase(ABC):
-    """Base class for the Optimal Control Problem (OCP) solver. This class defines the interface for the OCP solver."""
+    """Base class for the Optimal Control Problem (OCP) solver.
+    
+    This class defines the interface for the OCP solver."""
 
     def __init__(self) -> None:
         pass

From 2aa81eb9e511e9735b89ddb0d37918c58bdf8f3a Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Tue, 7 Jan 2025 14:22:22 +0000
Subject: [PATCH 77/95] [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
---
 agimus_controller/agimus_controller/ocp_base.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/agimus_controller/agimus_controller/ocp_base.py b/agimus_controller/agimus_controller/ocp_base.py
index ed2a4947..19f9a77c 100644
--- a/agimus_controller/agimus_controller/ocp_base.py
+++ b/agimus_controller/agimus_controller/ocp_base.py
@@ -9,7 +9,7 @@
 
 class OCPBase(ABC):
     """Base class for the Optimal Control Problem (OCP) solver.
-    
+
     This class defines the interface for the OCP solver."""
 
     def __init__(self) -> None:

From 88f572d8fc0c6dd974b414f2b0ebfad7854cee3c Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Tue, 7 Jan 2025 16:55:18 +0100
Subject: [PATCH 78/95] Update
 agimus_controller/agimus_controller/ocp_base_croco.py

Co-authored-by: TheoMF <140606863+TheoMF@users.noreply.github.com>
---
 agimus_controller/agimus_controller/ocp_base_croco.py | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index f450e351..08e3c1da 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -58,6 +58,10 @@ def create_terminal_model(self) -> crocoddyl.ActionModelAbstract:
         """Create the terminal model."""
         pass
 
+    @abstractmethod
+    def update_crocoddyl_problem(self,x0: npt.NDArray[np.float64]) -> None:
+        """Update the Crocoddyl problem's references, weights and x0."""
+        pass
     def solve(
         self,
         x0: npt.NDArray[np.float64],

From c7cb95c6385e0664beb9f3370376ef20f27cc43f Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Tue, 7 Jan 2025 15:55:33 +0000
Subject: [PATCH 79/95] [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
---
 agimus_controller/agimus_controller/ocp_base_croco.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 08e3c1da..6c933dbe 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -59,9 +59,10 @@ def create_terminal_model(self) -> crocoddyl.ActionModelAbstract:
         pass
 
     @abstractmethod
-    def update_crocoddyl_problem(self,x0: npt.NDArray[np.float64]) -> None:
+    def update_crocoddyl_problem(self, x0: npt.NDArray[np.float64]) -> None:
         """Update the Crocoddyl problem's references, weights and x0."""
         pass
+
     def solve(
         self,
         x0: npt.NDArray[np.float64],

From 91bf33ce8bd29e7c2749e81001dfd7056cd2c83b Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Tue, 7 Jan 2025 16:14:26 +0000
Subject: [PATCH 80/95] adressing the change in update crocoddyl problem

---
 agimus_controller/agimus_controller/ocp_base_croco.py | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 08e3c1da..5b941b82 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -59,9 +59,14 @@ def create_terminal_model(self) -> crocoddyl.ActionModelAbstract:
         pass
 
     @abstractmethod
-    def update_crocoddyl_problem(self,x0: npt.NDArray[np.float64]) -> None:
+    def update_crocoddyl_problem(
+        self,
+        x0: npt.NDArray[np.float64],
+        trajectory_points_list: list[npt.NDArray[np.float64]],
+    ) -> None:
         """Update the Crocoddyl problem's references, weights and x0."""
         pass
+
     def solve(
         self,
         x0: npt.NDArray[np.float64],

From e29276f1cfe88bd344b0c39814a8fa490dee0943 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Tue, 7 Jan 2025 16:50:19 +0000
Subject: [PATCH 81/95] changing the input of the update croccodyl problem
 method

---
 .../agimus_controller/ocp_base_croco.py       |  5 ++--
 .../tests/test_ocp_croco_base.py              | 29 +++++++++----------
 2 files changed, 16 insertions(+), 18 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 5b941b82..fdcfa644 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -5,10 +5,11 @@
 import numpy as np
 import numpy.typing as npt
 
+from agimus_controller.factory.robot_model import RobotModelFactory
 from agimus_controller.mpc_data import OCPResults, OCPDebugData
 from agimus_controller.ocp_base import OCPBase
 from agimus_controller.ocp_param_base import OCPParamsBaseCroco
-from agimus_controller.factory.robot_model import RobotModelFactory
+from agimus_controller.trajectory import TrajectoryPointWeights
 
 
 class OCPBaseCroco(OCPBase):
@@ -62,7 +63,7 @@ def create_terminal_model(self) -> crocoddyl.ActionModelAbstract:
     def update_crocoddyl_problem(
         self,
         x0: npt.NDArray[np.float64],
-        trajectory_points_list: list[npt.NDArray[np.float64]],
+        weighted_trajectory_points: list[TrajectoryPointWeights],
     ) -> None:
         """Update the Crocoddyl problem's references, weights and x0."""
         pass
diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index fabedaea..2d545624 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -26,18 +26,9 @@ def setUpClass(self):
         self.mock_robot_model_factory._complete_collision_model = mock_collision_model
         self.mock_robot_model_factory._armature = mock_armature
 
-        self.mock_ocp_params = MagicMock(spec=OCPParamsBaseCroco)
-
-        # Set mock parameters
-        self.mock_ocp_params.horizon_size = 10
-        self.mock_ocp_params.dt = 0.1
-        self.mock_ocp_params.use_filter_line_search = True
-        self.mock_ocp_params.termination_tolerance = 1e-6
-        self.mock_ocp_params.qp_iters = 10
-        self.mock_ocp_params.eps_abs = 1e-8
-        self.mock_ocp_params.eps_rel = 1e-6
-        self.mock_ocp_params.callbacks = True
-        self.mock_ocp_params.solver_iters = 100
+        self.ocp_params = OCPParamsBaseCroco(
+            dt=0.1, horizon_size=10, solver_iters=100, callbacks=True
+        )
 
         # Create a concrete implementation of OCPBaseCroco
         class TestOCPCroco(OCPBaseCroco):
@@ -47,18 +38,21 @@ def create_running_model_list(self):
             def create_terminal_model(self):
                 return None
 
+            def update_crocoddyl_problem(self, x0, trajectory_points_list):
+                return None
+
             def set_reference_horizon(self, horizon_size):
                 return None
 
-        self.ocp = TestOCPCroco(self.mock_robot_model_factory, self.mock_ocp_params)
+        self.ocp = TestOCPCroco(self.mock_robot_model_factory, self.ocp_params)
 
     def test_horizon_size(self):
         """Test the horizon_size property."""
-        self.assertEqual(self.ocp.horizon_size, self.mock_ocp_params.horizon_size)
+        self.assertEqual(self.ocp.horizon_size, self.ocp_params.horizon_size)
 
     def test_dt(self):
         """Test the dt property."""
-        self.assertAlmostEqual(self.ocp.dt, self.mock_ocp_params.dt)
+        self.assertAlmostEqual(self.ocp.dt, self.ocp_params.dt)
 
 
 class TestSimpleOCPCroco(unittest.TestCase):
@@ -110,7 +104,6 @@ def create_terminal_model(self):
             # Terminal cost models
             terminal_cost_model = crocoddyl.CostModelSum(self._state)
 
-            ### Creation of cost terms
             ### Creation of cost terms
             # State Regularization cost
             x_residual = crocoddyl.ResidualModelState(
@@ -148,6 +141,10 @@ def set_reference_horizon(self, horizon_size):
             ### Not implemented in this OCP example.
             return None
 
+        def update_crocoddyl_problem(self, x0, trajectory_points_list):
+            ### Not implemented in this OCP example.
+            return None
+
     @classmethod
     def setUpClass(self):
         # Mock the RobotModelFactory and OCPParamsCrocoBase

From d805ea48799fb8121363225adf9ede5acc3ab5a3 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Fri, 10 Jan 2025 15:52:17 +0000
Subject: [PATCH 82/95] fixing unit test ocp croco base param

---
 agimus_controller/tests/test_ocp_param_base.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/agimus_controller/tests/test_ocp_param_base.py b/agimus_controller/tests/test_ocp_param_base.py
index 31ab1f19..2bcc34c3 100644
--- a/agimus_controller/tests/test_ocp_param_base.py
+++ b/agimus_controller/tests/test_ocp_param_base.py
@@ -26,9 +26,9 @@ def test_initialization(self):
             "eps_rel": 0,
             "callbacks": False,
         }
-        params = OCPParamsBaseCroco(**params)
-        for key, val in params:
-            res = getattr(params, key)
+        ocp_param_base_croco = OCPParamsBaseCroco(**params)
+        for key, val in params.items():
+            res = getattr(ocp_param_base_croco, key)
             self.assertEqual(
                 res,
                 val,

From d6caca3c815b2ed2117cb62b0ef816c12481b2c2 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Fri, 10 Jan 2025 18:23:53 +0000
Subject: [PATCH 83/95] changing factory to models

---
 agimus_controller/agimus_controller/ocp_base_croco.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index fdcfa644..4a655300 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -5,7 +5,7 @@
 import numpy as np
 import numpy.typing as npt
 
-from agimus_controller.factory.robot_model import RobotModelFactory
+from agimus_controller.factory.robot_model import RobotModels
 from agimus_controller.mpc_data import OCPResults, OCPDebugData
 from agimus_controller.ocp_base import OCPBase
 from agimus_controller.ocp_param_base import OCPParamsBaseCroco
@@ -15,7 +15,7 @@
 class OCPBaseCroco(OCPBase):
     def __init__(
         self,
-        robot_model: RobotModelFactory,
+        robot_model: RobotModels,
         ocp_params: OCPParamsBaseCroco,
     ) -> None:
         """Defines common behavior for all OCP using croccodyl. This is an abstract class with some helpers to design OCPs in a more friendly way.

From faf187e6b87bcbd3ed255e0c1b1d66655d52585a Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Fri, 10 Jan 2025 18:29:48 +0000
Subject: [PATCH 84/95] first two unit tests working

---
 .../agimus_controller/ocp_base_croco.py       |  4 +-
 .../tests/test_ocp_croco_base.py              | 76 +++++++++++++------
 .../tests/test_ocp_param_base.py              |  1 -
 3 files changed, 54 insertions(+), 27 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 4a655300..f18bece0 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -26,8 +26,8 @@ def __init__(
         """
         # Setting the robot model
         self._robot_model = robot_model
-        self._rmodel = self._robot_model._rmodel
-        self._cmodel = self._robot_model._complete_collision_model
+        self._rmodel = self._robot_model.robot_model
+        self._cmodel = self._robot_model.collision_model
         self._armature = self._robot_model._params.armature
 
         # Stat and actuation model
diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index 2d545624..9fa81a96 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -1,6 +1,7 @@
+from os.path import dirname
 import unittest
 from unittest.mock import MagicMock
-
+from pathlib import Path
 import crocoddyl
 import example_robot_data as robex
 import numpy as np
@@ -8,23 +9,37 @@
 
 from agimus_controller.ocp_base_croco import OCPBaseCroco
 from agimus_controller.ocp_param_base import OCPParamsBaseCroco
-from agimus_controller.factory.robot_model import RobotModelFactory
+from agimus_controller.factory.robot_model import RobotModels, RobotModelParameters
 
 
 class TestOCPBaseCroco(unittest.TestCase):
     @classmethod
     def setUpClass(self):
         # Mock the RobotModelFactory and OCPParamsCrocoBase
-        self.mock_robot_model_factory = RobotModelFactory()
-
-        mock_robot = robex.load("panda")
-        mock_robot_model = mock_robot.model
-        mock_collision_model = mock_robot.collision_model
-        mock_armature = np.array([])
+        robot = robex.load("panda")
+        urdf_path = Path(robot.urdf)
+        srdf_path = Path(robot.urdf.replace("urdf", "srdf"))
+        urdf_meshes_dir = urdf_path.parent.parent.parent.parent.parent
+        free_flyer = False
+        q0 = np.zeros(robot.model.nq)
+        armature = np.full(robot.model.nq, 0.1)
+
+        # Store shared initial parameters
+        self.params = RobotModelParameters(
+            q0=q0,
+            free_flyer=free_flyer,
+            locked_joint_names=["panda_joint1", "panda_joint2"],
+            urdf_path=urdf_path,
+            srdf_path=srdf_path,
+            urdf_meshes_dir=urdf_meshes_dir,
+            collision_as_capsule=True,
+            self_collision=True,
+            armature=armature,
+        )
 
-        self.mock_robot_model_factory._rmodel = mock_robot_model
-        self.mock_robot_model_factory._complete_collision_model = mock_collision_model
-        self.mock_robot_model_factory._armature = mock_armature
+        self.robot_models = RobotModels(self.params)
+        self.robot_model = self.robot_models.robot_model
+        self.collision_model = self.robot_models.collision_model
 
         self.ocp_params = OCPParamsBaseCroco(
             dt=0.1, horizon_size=10, solver_iters=100, callbacks=True
@@ -44,7 +59,7 @@ def update_crocoddyl_problem(self, x0, trajectory_points_list):
             def set_reference_horizon(self, horizon_size):
                 return None
 
-        self.ocp = TestOCPCroco(self.mock_robot_model_factory, self.ocp_params)
+        self.ocp = TestOCPCroco(self.robot_models, self.ocp_params)
 
     def test_horizon_size(self):
         """Test the horizon_size property."""
@@ -148,32 +163,45 @@ def update_crocoddyl_problem(self, x0, trajectory_points_list):
     @classmethod
     def setUpClass(self):
         # Mock the RobotModelFactory and OCPParamsCrocoBase
-        self.robot_model_factory = RobotModelFactory()
-
         robot = robex.load("panda")
-        robot_model = robot.model
-        collision_model = robot.collision_model
-        armature = np.full(robot_model.nq, 0.1)
+        urdf_path = Path(robot.urdf)
+        srdf_path = Path(robot.urdf.replace("urdf", "srdf"))
+        urdf_meshes_dir = urdf_path.parent.parent.parent.parent.parent
+        free_flyer = False
+        q0 = np.zeros(robot.model.nq)
+
+        # Store shared initial parameters
+        self.params = RobotModelParameters(
+            q0=q0,
+            free_flyer=free_flyer,
+            locked_joint_names=["panda_joint1", "panda_joint2"],
+            urdf_path=urdf_path,
+            srdf_path=srdf_path,
+            urdf_meshes_dir=urdf_meshes_dir,
+            collision_as_capsule=True,
+            self_collision=True,
+            armature=np.linspace(0.1, 0.9, robot.model.nq),
+        )
 
-        self.robot_model_factory._rmodel = robot_model
-        self.robot_model_factory._complete_collision_model = collision_model
-        self.robot_model_factory.armature = armature
+        self.robot_models = RobotModels(self.params)
+        self.robot_model = self.robot_models.robot_model
+        self.collision_model = self.robot_models.collision_model
 
         # Set mock parameters
         self.ocp_params = OCPParamsBaseCroco(
             dt=0.1, horizon_size=10, solver_iters=100, callbacks=True
         )
         self.state_reg = np.concatenate(
-            (pin.neutral(robot_model), np.zeros(robot_model.nv))
+            (pin.neutral(self.robot_model), np.zeros(self.robot_model.nv))
         )
-        self.state_warmstart = [np.zeros(robot_model.nq + robot_model.nv)] * (
+        self.state_warmstart = [np.zeros(self.robot_model.nq + self.robot_model.nv)] * (
             self.ocp_params.horizon_size - 1
         )  # The first state is the current state
-        self.control_warmstart = [np.zeros(robot_model.nq)] * (
+        self.control_warmstart = [np.zeros(self.robot_model.nq)] * (
             self.ocp_params.horizon_size - 1
         )
         # Create a concrete implementation of OCPBaseCroco
-        self.ocp = self.TestOCPCroco(self.robot_model_factory, self.ocp_params)
+        self.ocp = self.TestOCPCroco(self.robot_models, self.ocp_params)
         self.ocp.solve(self.state_reg, self.state_warmstart, self.control_warmstart)
         # self.save_results()
 
diff --git a/agimus_controller/tests/test_ocp_param_base.py b/agimus_controller/tests/test_ocp_param_base.py
index 2bcc34c3..0855f7b4 100644
--- a/agimus_controller/tests/test_ocp_param_base.py
+++ b/agimus_controller/tests/test_ocp_param_base.py
@@ -1,5 +1,4 @@
 import unittest
-import numpy as np
 
 from agimus_controller.ocp_param_base import OCPParamsBaseCroco
 

From a000c3c47b294bec11fa478959ae6c79c56b947f Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Mon, 13 Jan 2025 10:19:53 +0000
Subject: [PATCH 85/95] unit tests passing for ocp class

---
 .../agimus_controller/factory/robot_model.py  |  26 ++++++++---
 .../agimus_controller/ocp_base_croco.py       |  14 +++---
 .../ressources/simple_ocp_croco_results.npy   | Bin 15149 -> 10077 bytes
 .../tests/test_ocp_croco_base.py              |  43 +++++++++++-------
 4 files changed, 53 insertions(+), 30 deletions(-)

diff --git a/agimus_controller/agimus_controller/factory/robot_model.py b/agimus_controller/agimus_controller/factory/robot_model.py
index efae318a..b1d8f9a5 100644
--- a/agimus_controller/agimus_controller/factory/robot_model.py
+++ b/agimus_controller/agimus_controller/factory/robot_model.py
@@ -11,9 +11,12 @@
 
 @dataclass
 class RobotModelParameters:
+    full_q0: npt.NDArray[np.float64] = np.array(
+        [], dtype=np.float64
+    )  # Initial full configuration of the robot
     q0: npt.NDArray[np.float64] = np.array(
         [], dtype=np.float64
-    )  # Initial configuration of the robot
+    )  # Initial reduced configuration of the robot
     free_flyer: bool = False  # True if the robot has a free flyer
     locked_joint_names: list[str] = field(default_factory=list)
     urdf_path: Path = Path()  # Path to the URDF file
@@ -34,10 +37,15 @@ class RobotModelParameters:
     )  # Red color for the collision model
 
     def __post_init__(self):
-        # Check q0 is not empty
-        if len(self.q0) == 0:
-            raise ValueError("q0 cannot be empty.")
-
+        # Check full_q0 is not empty
+        if len(self.full_q0) == 0:
+            raise ValueError("full_q0 cannot be empty.")
+
+        # Check q0 is not empty or if there is no reduced model, q0 is the full configuration
+        if len(self.q0) == 0 and not len(self.locked_joint_names) == 0:
+            raise ValueError("q0 cannot be empty while reducing the model.")
+        elif len(self.q0) == 0:
+            self.q0 = self.full_q0
         # Handle armature:
         if self.armature.size == 0:
             # Use a default armature filled with 0s, based on the size of q0
@@ -74,6 +82,7 @@ def __init__(self, param: RobotModelParameters):
         self._collision_model = None
         self._visual_model = None
         self._q0 = deepcopy(self._params.q0)
+        self._full_q0 = deepcopy(self._params.full_q0)
         self.load_models()  # Populate models
 
     @property
@@ -109,6 +118,11 @@ def q0(self) -> np.array:
         """Initial configuration of the robot."""
         return self._q0
 
+    @property
+    def full_q0(self) -> np.array:
+        """Initial full configuration of the robot."""
+        return self._full_q0
+
     def load_models(self) -> None:
         """Load and prepare robot models based on parameters."""
         self._load_full_pinocchio_models()
@@ -146,7 +160,7 @@ def _apply_locked_joints(self) -> None:
                 raise ValueError(f"Joint {jn} not found in the robot model.")
 
         self._robot_model = pin.buildReducedModel(
-            self._full_robot_model, joints_to_lock, self._q0
+            self._full_robot_model, joints_to_lock, self._full_q0
         )
 
     def _update_collision_model_to_capsules(self) -> None:
diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index f18bece0..e5d6077b 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -15,23 +15,23 @@
 class OCPBaseCroco(OCPBase):
     def __init__(
         self,
-        robot_model: RobotModels,
+        robot_models: RobotModels,
         ocp_params: OCPParamsBaseCroco,
     ) -> None:
         """Defines common behavior for all OCP using croccodyl. This is an abstract class with some helpers to design OCPs in a more friendly way.
 
         Args:
-            robot_model (RobotModelFactory): All models of the robot.
+            robot_models (RobotModelFactory): All models of the robot.
             ocp_params (OCPParamsBaseCroco): Input data structure of the OCP.
         """
         # Setting the robot model
-        self._robot_model = robot_model
-        self._rmodel = self._robot_model.robot_model
-        self._cmodel = self._robot_model.collision_model
-        self._armature = self._robot_model._params.armature
+        self._robot_models = robot_models
+        self._robot_model = self._robot_model.robot_model
+        self._collision_model = self._robot_model.collision_model
+        self._armature = self._robot_models._params.armature
 
         # Stat and actuation model
-        self._state = crocoddyl.StateMultibody(self._rmodel)
+        self._state = crocoddyl.StateMultibody(self._robot_model)
         self._actuation = crocoddyl.ActuationModelFull(self._state)
 
         # Setting the OCP parameters
diff --git a/agimus_controller/tests/ressources/simple_ocp_croco_results.npy b/agimus_controller/tests/ressources/simple_ocp_croco_results.npy
index 1f976f0b0c7665feeeed50de40040caa08966b6a..03abea3f9202e9fd80ea8a0d7f9af97d19c28069 100644
GIT binary patch
literal 10077
zcmchcc{CR9AFz?E*|N*NE2KiCnh!#DJ@(4J%QiezB3qO#Ye?3VtrA(%=Dwp%rA<Xt
zB9x-+Ny$sUzRr37e$RR5kLR3cX3jO&oO`bO%#vkhZE9gpOA|(OM9$qK#4T8Eo48zr
zxtyZ7oM&J#HP|J<DKOaGV|lf{i(iPx()ti@mmrU&|MFWD6%@rI#s6OxmgrS(0ipgu
zhn3v|gFTe}L;a{eF2TVrhgqD0J=_8VLa4!^Zd4YM=6`#!1i1fiD@kii6Hn6V(z=qX
zj+68ha<#lMt?{aOlHuQu?$pCU9xNoI9UTn~jkyNNlz8Ge$xI<x<c(L^(JrlvAX&}K
z%#0S7mcKv71eILln&?2X$vcqj^4>Jwe-FXoy~O9FkX*}r_IPrw_y3Td8YK5Jl7~X_
z{)gO7^8MdP8Y;=}ny4GJbh!Vo3n@S)1zrC;1)=3rSVtj+|DA$}H}8^DfWy86WhU0B
zeexde^EpNwYgMDaZBT$7?0T|O?@1A1L{mUBTk40xpQP^6z0*WQ=4&u3iZ!5}nq4_v
zkEfBKp-!y(cPW^O5<a@NNDC_lS{DcM?gT@-yzT2}S@F3eleJY^HqfU}OG}npVp5b!
zimf<_FFQ$4NXdUrQh%KWjBosU=S(7A>C>O0oeGH3OQj)(Q&&(TH|P13tOmmEV53)x
z;4xIdaZYI0uP4NT%<wlNiw{xGmYsL*CNbh#Z<f-GTbn@Uc)Pl!ssYZbX&wlZF$5NE
z9pUHtg4pk8QdP2)8=U0mKmR4s8I#gfQf9?TcG*deLdyShQuyn{CE1-{IT3@h_r=G#
zOIH)JZ#oul)L}#i^k+3c-zGHQ4Wv%`WFZ$D{dZzCZ;5AdRvlMFx)HhUbb>qgT3j;H
z5b(@?D+up#9}(JOg8Q~a228e^!qNV&;(0?!d{9B*`nv#c2y?7h`~IdoCf8HR4J%HH
z%T7uZQu&|L#{Z37@wL7;)hCg{;<uWx#(H8XPMc-d?0wWAdNN~`NjtICpY11;K?w>9
zOv!#dKS<D*z3s^w=tte&C$=@b5x~77p+;?En(+HYr+2-Z3I3FN<>}*hCXlrEmU(Km
z0?yyeKbNa}0Bjx&T64B}Vp4@lZd!5LyzI1vLaP3AQv2&9ob~vV;?Z2R`|Wm}B9ZGv
zudhblm+enby8f)vfXh>&eUzJqZm<$v@A*P^VeKfP8gxi&OVcN$ofZ8-sZR`JIq%#a
z22I!;mXbdow;MP2&Sv`GHGmCu%`Q1FRIpH|_*34-1CVgqIitLF$w{3`Ze4NGSa#B+
zklX$_Y5jF_4^vrVY*&J~`JJm>;}KE)mh(_$<a0z@{^b1e*UyQT!d=t4d+X3>V)BPk
ziwR;!P2)w))niB!^#_N}O5jU{?drB7YLK$m>pnWJgZ2GiYE|9^s1sS8q_<%U7DR7z
z%!+-0vdwmV-SXINr;<BXoOUid5fl>sIcfiO`f+uOeAwx8$mwyE@}b(>L__KRcaPaR
zQFq>6HZ0mnT)Z`9C^FfI+*-#~eP*W!y65&2Qzhd_zS++sy+|CdkvY0bS4tTghf$4&
zp9Z$M@?{Unqzboq8fN7uHsST{HkQ2?-9ek?b1YNp(lxF_C3RPvb}c*UQAquNP6mIS
zt|5k^oWKgi$#zjf{$eXpQoCw==lf2ieR-&1=w>JJI8UF;YoQs%`(D%*Pn{tSDf5+;
z4^E)mi_v-S<oWQKA=$uf*F?cSbMW3Mselb5uGb5gNx_i$Ru7d2a+qIgT6Xk>9qd*W
z6MS%cX)f4JB@I`cjFz2@DWu6iC)2-97vnGFhJ39+*LGlYx$IVAWHj%q?OZ3qDRkcC
zXeZH2hHH8WUPFyVb6o3B&k&ouPB~PLOrU#~*C&H_d_yTmcGGkOeL^XHH!pic{6qt?
z0-@?#8R6+%sg9goFG?GI^>Te2BUoq8MB|fFh%}>;=B^$Nq{YhIzh_yDLLn`e#Tbp*
z6G*EiH66}XG&GwX?{IDoiig(F4<B!eBw|ymg=<gglHgWg>z2zrhk#bBsyEa>1`4fC
zy;0~lhx_|jKl0Ej;cjiC*gvA%KtIF(Vn#L%b{XnweN`<9?^@eN-{{T|6@qMqnaBBo
zuQs#bL1HH{A`&iK_;MH(UE;3XI;;k&TF)#wKAGd+4s7ms{Vnj`MZ=LOr67=S;BTj6
z;KmXz0ge)7J3-Ux+~Mw1mRR-UN0ZKor^GgeSQD9=t-vQ8(l`@bO!Tmc$*Ar8LbP7w
zl{s(thS&;=_Z0P-(01pEr;0y2k$#I%Xi0b$XsElK*~lM=l`lE&rLjK?f*04inmrB0
zrUJu{#g3*!=dUQrp93C{B5e3V>W2%U8SfyAw+diiU=-7|3Zpgqf}`7>?*N(<qum2z
z)9AeG%||gk;=tYH{Z5Cbhj7Zgkg%rk6|$IlP$&}U28XuuD0R6V$9sa(?!D3R!SN+<
z1_x)sf*TXvfaGRS)m-<~-!Kw}x#}l&U8F$2d35Z0$^}Bs|BvULQys*fiBlKY&VNGV
zS6`TSS8Kvu>wAm`)xQ%y#@bj&exC4oWq)qH=yRg!u>XmlCa2&?UEk!JQ|`Da)|p9K
z;xw=`w>-63RK#jx<WQP=1_Vt+e|x>g5B5)p-pQQ^!zT@`gA2PQAz#vt!&i<Qb26$J
z>P1RHW2nD<G95e6zX)Qwa!d^<4R?Lk$Uj5=Cz6Mb8`5BHt21S%E`|Wzv+eEuELpfB
zwB`Nk^k85d<MsQLlm)_*eA1j&fuLdTTbSaP0L(#rYj&pl!bg+yX*19Ih_x?|RkrTm
zh-c%2OdQjD2(Ga>hs|!1u)TlrTE5sdB7{jUJxM?f)D&l*ckdl0E?xP2O|6g%s=kTG
zB%IF1j$=}GCZ#?wAltakz%&j_-@KQ2weAppC}RF0-7*Lo*ZUMiY%PVPusw{E1^vi4
zeDgiIR%v{H@)nJ|+ZAGyVa{+5FE@Pkdfa=@a}jOm%F#NJKTK>fd1g00sRpDqm9$wY
z+IBe{I|^yP63*WL!dX*1>T>W;9MF@_ys{H#@FrzlX3Lgn5XD<QU2;u>KT<aa7}Db4
zS>%)Fc3&LvRE@e}5hD$bdG~1G8`F09JoNp#*WfsDRXa4u;DZ$C$QqkGy7dxqB|1Bw
zSjP`6Z4C}1%ss^0h~oZ*@)1-sE7|Bjr~+aMp4*;j*yGNZu9v$wC@}KUA@|%M2{&ak
zb_=IUfifrS2UJVJ8!Lo$a&}O#-}foDUH3YP@tqE#p0yf~&S6!uJ)i~28SWcYOrIo*
z<_&tL?z|)1PJe1qr%6TCYrT)$JMtWv*5$9&v&jTHS4wjADL4G>Miq^yQZYPR#eF}F
zGYrD!boS9rWk4j6b6d!3FP>ce)_3usJLvy8T9tiS9+KPM?Y!~fHTq04(;hL{32M_f
zK6e_@;tKb=*HJG-;b8Fd9^3R@!d*k-eLS@f)qVENjWu(H@d@9q)W-?fiur2VmCbsv
zPuuk2gU+Q3%Q=`iXtO>pwKS(oRg49Ps{*TDx>*8meUErEzKym&4?fuTfEhTe&$Q|)
zO`!e$uBJ4{G+~|2?KOnU6d~kR!YN(+jR?`Wb90R05uqviC}#sxHWcK>x{YK8;y0yl
zGT(p5f@dN-28<)ap;kO(j&~pfPKAmbzcC_;BXyK4ZiG^?6EXYtp{^t>{K@Z2*w2OC
zbFAsz{iHzs<uy#_#tQ2*Ggp1GR0o>;$B}`yJ!s;OPt@VFf6$=M4^h`p61-a@srm0S
za618&c}o6ZDbwn*@HicKNezjEdTyXvbnVua&Sdzqvuf0V-yik{((O7NcL8}EsQDTg
zrijJIM!V+a+mQ8ZC;mWlahPBIhrNum7fqFFUmTZFhxtxLhL4*D2)j;chAAgj_%(J_
zvCcIcuR3<~mIId;Y~{aA*T3f&Tp~A5yICH>r&&*y2bG5cJ00&j;qxVMfhQpN*|Q;Z
zwk+0;*Hr>Ps+@HyGiXB=KON1SZg7C#sGrtFr3K_8yxYdO|2<)JfZ^A}(WP+qQOW%)
z;W#XZ<47T$R>E=qFPw`4Ef3W#V_`u=!J`_^;syybKVfM<nCvE5_9mZ#&bZS4-Y3T(
zAW1&=xeygIG{zP&scwce?nyz3wFKmH_DA^W@&JwKtLjVLGB9JVc5a~l4`P#(a5)zt
z0Py{rdC#@?#MgIEg<oABLrD!CO{q@eK!53#<%>WUeBmzp^|s&EAf)#29d9%h&Xjkj
z*euO-i}r$Fwp6>|yOpN~Hm}`--IU(GpN{)Lyy~^kGMv|hBc?%3Du<S4c%#<C(K-u6
z=DYdd+hslySU^N^n^raI>AWp~96HdGh>LHc8q(p~qO;XcJ}o@gpQb&VoDbJ{-?P?s
z$3Tf}bbBFpCXn~HH(qxQ#UY)sZL@`5AP~ZQYSK;~p33Ov*0Il{ExWURipmo(k?8UB
zSpXyc>2q~DUs439%KF+J`aMK3mj-vE;V|mlqD7cxJHqwnLpm}k@pvw-I6Q}T4;&O_
z=Mz)UhWV3C?iDO{IBjpyyt7g~*o)}gzt3q7^V1(zoyd5F-q{sciX0Gv`vxf;j3vKN
zXvX-KHoa}ok^S(_R<1chfb+ZoK60Cgsniuv{M1Sq2<-@C^2&sYV$J=gF~Rs-QczmI
zZwg#=e0A1}`2_Ub;EAuIW`GZ^NA(p=KkT`m|98dRAbi@Oa>D4QBt(ZcRQ@SigFB*{
zc2*5b!QyW9B5xHIs1Nsw_#~?VE%&<PT!Q)$V*vMe*@y*1lV#eTRpAec8ILMfZB54~
z{Vy`L9rOe_!G~KHZX|(arglB+O$(?v)Lb?teiFngpWc0^<Ojcu%A&Tu`-m)xaus=A
zD&TtW^n1sehLOqLY>JJD2=p~1X(_pnqBpsMDW_j<1$Q|O)tAye#P|j)wD6t=IGQSX
z4%lU3s=~f+%!?jy)1uaCa914g2WXG|ICc=r$DFMG5PuMwVW`Dmya*Ii!)U*6_==*B
z-4eR2vJMA3<|%(D?nPVJ^wi%fG6BC+;86XMU#L2y;aa)(3t~oec*sml9Y_}{>ADh*
z+j2PW6w+fQ9MAv4F>xQ3VM>mM$?|<{$v(ySeZa23EAlS5IrCsu`Gagoe{K84gf1RV
zSG1@q4n^Yq^><7n6%4^wR(b2t9c}n@JmK!Ri3Hd(nECIyB@dShyWKYIV8VxvrnZE4
ztb_7felO1~U08b81aw2k$I-pD=S9<P=ZOy!ZLQWv-0^we0f}Svc9=b+DfH6iL*Trn
zc#}4x7Ce(zvCX7&!JiKrv(&{{;C_)?iX9rGM5)wG9m4`GNLzEV{pP{<C@whk?4Qp}
zpkjN6+RHvl+%qGuh8%u?vO;OLNl86GVx?^5Y<j8imAyW3fX@~-N;6llznlwCjL)a*
zRyhKT7E}Eu`wS2ol24otipD>N6~)r`902a_E%9H@$wM34>&=H6+3?ZGj?PU%06Nd!
zn|4;PVxHguc2T?z?tEJiMgy-22~EF;v(HD-{PgVhjw7}Zr_<IYZhQ<s2^Gk;eHVmP
zo<s-t`=5eGvin=^47%V{R-SO?u0$AHweZ&S(soz_A}L}5T?mC+^mdF%L;6Be$i_iB
zoaR+Y`I@mEJ|(i&HVM;!;igS`LEpZh65HJmqw*UG2fL2QyOHTYaqDdUY#oeCSbHyO
z{5T3D7bNvZEYbn#7X}9>roru_azUwlhwz41hf#K-7yi~CQt(nk8f*fvOSSuIES+{U
z$Y)9#Mql^IiC<-gfhmR^+}pLFdSAw_`hCM_W>DOF3;zPDu{&;g$<rI!KMw3X_&Eia
z8XGho+~5rS>m)x96vct46GIT?r2y{n!q&e2$uKL>&*}ZZ6U_STTuThcQOoYPp(ZY}
z*a5nmUfWM1n>C8m^;v?@Y~?`rQDq7-Z4DdYbJ+&d>N-{zjh+y%g(cU05}ij(C$D_q
z@Xf%^W5*&k(|Q8EmRnf>OEh=}GgSxO48`hP()1e>LP5;RF;YV746J_6H(LPX=%n$d
zrzz*TvH9In2A<wwR9-3X_1=yK<^?*XORDD4_Y;o$ksczhoX$O;+`ANx7nSs03CCwS
z9A64~U?m*C|H9e%`dXZWV<eczpI5w2dmImp3_10OM&pC}{JlvviIC^tW|OXX3^u0<
z9Z)rl#tg3?RS(~!z>9=KGh?53L5GgVC5u=Yh!Xp0^1@CLHu2;SaZy?DV1MPeIBRj}
zxPaKk!`g_|eIHFP?4Lvu;x=4o8i$CM(y&?M4tG2+;ot)&j^bvq!NbE(BcX`tWyKAr
zIh1Mfl^>clF<*t;FF{ue%q+V-bHDa1aY}c{Ouldj1lAVy^cBBHce~acu9)J5OI_|2
zN@X(yN4%w(zsYMPz>!$p6mtia>832ak2nF<1C{RF5za6sBgA_mE*HuSZ1x&9CgUe1
zlzuVo4Ct>+ajAK63`?u-zctS82RhvjARnaw62<Q_n)&##-5Qm*1e*?M%(iRGS+Qe7
zwPKmxd_lN&HYnp;>|^3>)|>auN5)Z2!x}UDhCQIY-~wX|$1vBgdUlE2llWov_t5ue
za)Ct#R~djG{<X*PY4W{v@J(C$c5%N1=K1l2!u|X@DmYscJd&dX_Gdry?L5GM13k;m
z7}Nq3ey=P`uV;V*vfr!+(-%-;Je_G)QwCuen3LC9oC4w1<Yr}-5d63tyEUG)2G9O%
zU*)q|kdl<x61Oytg<}0CM}CFlh^N+^{Wbfrh*VYLRdZRO4B4J3S^WncFB|FuPFc8#
zjeltjF@svOL-X1f1mtxYMdoXbqTRESJ7>mzqrO*_)#j(%KrL{U*6+QEIM(T$V2-6W
z-1x9svlNAcQi{9z%o-c)Pc!Wja4!~$w-o27u5kbfo!DAU&k3ZL^-=bVjszC&bRBh3
zo<%#xk53Y5yg<gS)6Qd^MjM-jN-a0-1dH-)?b{+(h-~Etx}lvCI4yceGGK4&(k<E9
z3+>-L!06s&>23Oh!2YCS<5k)aJiy$twM8=sM(!6q<#Wq{9my#pJRDP~YS_t|{wXsK
zu2MKBVEG+otS0yTN}VJgw?1S`5&ngeLm5tY$5K(tk*JdoiZy`rr;-6H;RG&+6GS1&
zm2iUp3rD}iPx*}JVK}1rQO5d0I*hN*e#p+3jOkd~?CDQ#fzAq#Uq^Swg5llEpDkn~
zFq9N$F;-hZp1oZ~KgAHFZ-3rkDkBR@CHXs4Bvl~CSEOZA>}tG<#jWxluM{+F^t6HD
zyNH>a-1RwW5;=P3+bgPZz(LNCp9?oUaO~&NJEY1<JbbujdQK`D)ZfM#NhsQ2-(mc_
zfyoO{%!S9@$Q*Zzjhr@Aq5%<J4-QU7fTw)x=eF|npoKLPzrtjtpuR>|tt(-XD6?;s
zljV7Z-iT&UnQyhCXWdz&oIY`o#Kd*M>ZvuD+J!ye8IuROQ4F?frX|=qi9zUwKo-Dl
zp9_g?C-9BY2v#pgPf#dx*qy+&0q#+__BM{M!>)!Y607L-;OD^R_^*1LxbHxV&9)tU
z5Oj?Fd3taI5`N0+*qc3$C>HAKueKS(xx(Yw8Cw!?G53zOv6BVZQMS(_nB_EtmI|Hi
zx_1zp9o0%+WX^%6sp=D#j{yE97VB{$<P8xwK4NUuv<0S_*J>zaG2=_mp+s(a7Yyaj
z9CG5~fQGi+3^{-3am;3$y_7!}P`{S(6I^^8MlXLF8cjQle=D($p8xKL7x|)?Zi(hV
z#5X5?_cJjNVj$|3ICl^?edxaEb<_g?>It&y^^}LZX>5y%ywk|7=jP?a7+KKBEQr7B
zzyfRsR0>p8bs?A~wp(Or=2j1DyLT({H~M*h_m%y;j<9ie9le-!JmwA>3F;bk#<bGj
zgB*haz{j2!#r(t<JMaju4|wDSnWlqJ+ZJ|1xU%U~Zsu2X>etaxYj0khpO<7?wfF;F
z70*?S%isbn^~oD|+{e+3Zc!@(s}77UO7#p`*CBDe<mN4-D%i1w!=vbW0zMO?l{2H^
z4)g+|J2pQ!0Kdm$UX;HHz!^8?pO!njLUYmWFwqz3@G+yXAnV2qy49NGTj{faUTv#j
z*|so=nm^E-zyI<b@xVvLhClc>N_)d@m)YNdm;@uDPHx{0WC)d{u7nf198MU8Jh&3h
zq5s19)<^$L{Wleki$r(0a+CuKdVT2)V+NkyC=h5=mWUfjrJTASkx;;0ziG%U6#HK~
z?jKsJ35RK{8CbuY!)i<M!z{0*z(wxn3-k4x!Lj}8KI(~C6jE!?Wrk(o=9ZFrgL_?w
z?)DJth}8t*e{qy3cNPPNB+=^XKfd@Y<BrFYcBgPz;`%z?;7G8$ebM;*pfCPhd-%t(
z&*5;Rd)@9UO(uBoM=Nto5(6aGCxraW(}v)&+FJ9IooG`okDQsZ3hcPz8C9J|50;_M
zM0GJH_G!{>koCHaT%EGaz0O3y`E71Y>TZkZtA~VryKEkOtPu69Pp-trWf$o*3Ua{1
zI$dx~A_ZRw^?0wIZv(Z$Uo)9Xm7q?l<s+l0Fjfw$wpj0P1mZ_`WbgjLi8qF)w#m?~
zh26^ABoc{_D5lQB<(u&sIympjxyY*rRl0OWE`rH;n8~3sZQD8Q(fqRAStbwajRcix
zhhng@xL@%YaRv@IlxBbS(#3Q}tIlSnjT5go6pe^H(}d20o7mYk*|Cd#fcjjoG1RJc
zi-aX}!`O)GgZh(<OYfZbJvd_2g5q5EwT!=xfr5^*0~;LTaO#zKareb=JU=uVog|(I
zzZ)iB=u-k<a*c}J*KZ-%BKE_Ereag9bF_wiEwLUhXWMH9Sq~w6OhDOUS_anWQ{{fi
zv49o_c><a4hL`OPq>=wPvR^&UX8!gUGS_Q*Xg^{FwO84-1eRXoMns;88MX<<Oc4+`
z+HDMqwTA)|Di7fnuLIg*+(Fp1Pcl#?eFi1RNa(BBen9m{xla!!FQ9ypJv|b4=Ftc1
z^9s#eY%msaQ2a>HM|6pPqL`#HfP{dm!>4%epc_5AWMNPhKdsUzU)OOI|GeP1##qo9
z_S+sAoTssa;r@4FUD}>FukMRV$Yla##MX_3KaU3%+SdwmdOy%NbykzG!O!TNe=RTa
zokn~Gv=)(35yZW(v=v)EE+D%XM`qMIucE#J4=0NnZ6FU*$?%nMjx2{0K_Meo!a4e1
zIFi5n;eKx*RQ0C}^9$9&hner96!A=aCsnyLv^)oE<+Wz`sZxQlj{fn52Z5N&koNZR
zaW#D7>ej+f`PRVcHAfR3Ee2P1KIA>cqY5({o?Y(rNGCFl=9?cg%Y$~L$3s^15*h!Y
zD0!!jA*Wsus_PypsB>0L&-WtnZ-bK?*f{g>X2WfNeD;L`UmjmaQCbLQ|9!pcja(eW
zN~=9zz10BUqMx^?@Nj?^XLQZ60bP*E^ErCS^8|9KvZKxG+6HGGXsZm>mtN9$$VUcp
z%@W%3a{ZTGZy;IM_7huA27{9Gi^T0h9{6rl=HkfGvqr+gp<WK*8f=zL91~fW5A<=B
z#D}6(tX3-MR5Zkc2dcQ)-utM)>$2*=5gmTamu>W`ESLgw65$|`%85ng3Q9S{Sm5UN
zk*&feGwAM#FPmfU4k6zfrA%i}0v?<S&pX1Oj(<9|r|LW^$8?~X`OzaAHVT`r@~w@>
zDhWq=nuZGDmN;`?lLCR8wxzGzR4_-ZyFH<L%T62aee&TxvU@Eqq%^K>h_Hk|`3c+I
zI(flB@ukMd2m`J%D$Eq~YC{LO##&Q+!oUZA)V}v88IR_4=xfu*;Em}oGN?lNpc<O(
z)w<w>E2S%}zpDh}^=Dy6xA-2cu+Y8QxoZQk(vp8B*HsYC+1F{UEu`UYb{+k#QARi+
z`{Rtrpas;s@YssDj-ke`xz|&)zfnWt2+!&+eduLjNQnJ<2q%!LDOZn2;jYQ|?F#K~
zcr+<2Hasg9ml%i0R{l7Kqujnb_YVV}<8a@7zNr=MNTN|a-}Hm{quP}^kVb>0ZeOJK
z4=}+i-L(f_*4#l4dBRt<*;qhT8)~eOzK>ow7#ehxZo)a--NnH-0`TJwg)ll~1%|Yb
zq@ow)F%dVDCfQ|&pKxqs6PBi6fAxp448<Xke{P1u_WCSZ-*?_tpSKn@GgfAY%1$Ax
z*PIH&^pDYn2<b(gbBicfui=}}nqqWUGkHUjnIVu-R5E%coS5ZsVku<YN;t><3r99o
zcv?%$9pn#RP+&-|gT$yXNrt0oc$y|o#{5Vw4j#8oFf6czRu;`aW_vxbU0p-c`yadT
z;5q4^@{`W+la)`H&xsFeJ^a+!Hf)CHmQx<ZRxi=|!F^kr_sW9Vg2jPLzgh`AGx;uB
z<s(ve*WJGBi5N`&xl{P7F&K-`JR9g#%f{L$W}m;UGfZW0WLF+0vF){!6Na&|Ae`;!
z{Ci>t4*88j89jJFygh%;`Hum7%>Ti&adwWl&U&69WL_UiOYT0}l}!VzM+N=7nYqBq
zOX0dysTGl(xawi?J{P#E^SoAyITm+Qw%Q6UJv8U*%Jn*{*5Gv3C%3;?<U{PU_v|*o
zNw|qcEZ0wa4}NZ7Zt+!I39N$He+FgJ;o5TlDKmK|nC8!!s!3<SJS+!3lk3NcMDZ_y
zPlIW&v*>|qhshU6`POKZ6}>93$$S)>Xivjh35O*)b;|HQ3qc;X;6%8w`Mu5kXEAti
zr|`?SU8lj?C)d?x&n8@w`0Q}V@M}VcrZbeq&k)8Wr`hi5a^d|Pm;Az`9AIuZ;<x}K
z7hEnh&7!}=hO3k{I3gFDQF*sJ>-`=luwMVL(0n8lx9pc=$T5q--4-8BoaGBZLMfJ`
zhcgOa%k+(qwhYFF{l#UEC(ZG2Oj3(ay#jblZ~W$5`jXgq7*6U)iozm`R)S)}4+2X*
zz7}rb0Q)IgUk55bqcFb{V+Q(*C^+-%n3<9I(k)|e=_H#!PEIIF%B+dTCTZ&~EI1s(
ztNaz`hYu%UU484{#{6lx)=;lpca<SlD%r=pt-hN$e&D)^9z73qtg6^3V?~c8*d5B{
z{bvZ%x#~iz4_AmdKSxLXZ4MCC$Q&o^hLKG*FTY8J2v&L*s&zYOnuv}*FW@Ss4b&Oo
zvTJwu;v&Oo%u=O@cRbXhXMEy@ON4^!ADNlrn*oY(Qk;`$AhAzOt)!EP_-;E#C;t`Y
zU0OSTb+H}Ep$*p~UeaPU`9!K-RWo5O(eSIN&KAgcDw(kS74-PZSI`s7^(0crq`&pV
zlgUf%=xSs~J5J4BEyCwAr;m$;<-_BJnfw#C>aZ;Tnw$9YHQ+WGc>H}X9@Cb%UVO~=
z5T?z)96GLoF_}UoQ&-A7xy(+Zkm>)}8GqTkCK5JgzR1Pvqgb~OS7k!AyQ5V=Q6;{=
zTj6Z4*cJHlb8WKyqZnNC?hDJ?53QgZsn^K2wh5D&R5ELYoxRLHMIm$kv2*{jr7i7v
z*UP2jFvjbbXip}<YTw5SH${suC3EmpxJEf#Z<U{Jxq1W}eR-?Xf3+FTD@+`ii@ku!
zJSus5g`L05K0_f3{;>=HveoA@DDpPZxYnhrcW~?gXsymv-!FUuFIL41h?ix8>#xR(
zuVtussIuQnZ>kc$-FQfx4bR795tS@nVV5kkODW{pf9$fqY>((yY&%DV@EgH41=G4<
z?0rY;-ccbc7|*S@+n*SO`CkfN^0sxwlQcto?)x1f;&V`b#2r3No}-fISJ>st><S85
z`Hx-omp$vjTmD5a8mPzm?oaq8;);ab!$T3-P~BVG=e=fW9KtpCT_|*gkq&N7!R!ir
z+%JxDN;@COYAShQg?(|EeThQW{9|AK%Vwr&r$#rX19p>o^GYKdm+J8Mc}kW-!smwP
zn|RBxTfhz8M#EUR`Brh3?MXAP-Vh!yVSfS0S}Iw$!oIT1uBVU<|JaRx+3{mPXY12X
z!=|${FN`(L;K3cSiVyQEfaaaiRX@Ed474-TpFd<k>5mED!o)UQ@9}`<<9H*GO;qyg
z3cGokeT_n1|Hr=ZmwmcD>WahC2hlx@Nyc(Qr8v8jUWAjZfiT7&%-0z%<L{013*LXu
hKpvfvvv2An%s5FW{BbP?@+OtM<?7)<C0ksT{}1ZUAE^KU

literal 15149
zcmd7YX*5-TxWI8!$WSRmrZOaRhBC)~WS$~Ih|HxjY>rHkdCHVA(Z7;V8e}RZzn@T2
zQX!QR4JuKT3?<!a(^~iSeRIzX-{r9O^EvCR_3U-d-Z@)sEvy{q=?>6ENP2n&c?3#o
zibx)`l#~;Z^!5*=1-kj|@(=X%`t!7*+x{T0-_Hl_aSQPJ{kzmgIcYhONRj``gDslT
z!!Ou3AZ&w&f1uX}-{Ac;AGg3jw=lL{fnFZ|enGUrU=JD_Nk{z`o1Z83SCZb0E`emw
zqjx76lSn3Oa*31~y%}Qy$-MB7p0uz4FE)~82Ll})ouxX-nv$GEF104vq|6w1(Eon!
zAj!UU>sHfWzaM{pm=QFR!#%~B<dkwImr3oR+w*tC@;$$gzrvd2`g8o13FOK>)FHjq
z$yI+2$!$&YEFN+*$@{+#Nk=33+*9<jevkWq^&nT%NPhRlmniV(B??-TLJKcZcn{BS
zb99#ZjGMD9VjE~W&gdIYJa@5D%eN^;M;+g@X5amfu;bHo_fYmlYx*@<)Z6L_#jfPm
zexX{VR~e?hjpnmhLbxsbywZzF)aS{O9Q;KbN)IwspFFFD!{1jI-&?5z9oI%VRpL1C
zk3-9@9NlaOw#UYfZz#3F@NoUMf_~P;0=b4puKi~g`C}HfCdC%a;(yICFB%&+`;aKW
zM6k!tqnNn8jOWGY@72inip0KyM{0>(*TQ^dgX2-}yh?)P&-=tX^O*K8KRS@cUagAM
z2hocqRz`VrOk}fQ^B|#+mq-axUv-F?Ya3xE)zNA!W&+3E`%QIf`0>l|%*~_gJm6DE
zU;Ec2SKL=ZCl%+zy;vaE(MXAZ=JkKflGdcuf?4{nnR{M#R_$913hOzeAt!l`kV#a(
zp?wvjjSQ-?O2f@WE#GC;-E}!=+O;6}3d3{4N3CVob%7^neb$nu;)wLc65|mL400=1
zVgv19i9Jqgpjv5vDOcMZ+iXo(z7$!2DZ>Z(p*S%d|4C$uCvXplGHSh)x#o#iJ<70N
z+q8PIK+4ca*?(raKW2Gra>IgI;jeiH{StL8&p?*t3^QlzYlyg-Kvwr(w-M*_T9`Jz
zLsaibeWM?G5?$`+whR9GhPWvV%Qg-UA!qKZf@oJB+DVnD%D>@1b$2z+3RlirGNu7*
zb^F`i`kLeU*y)zur{*A+Jd$vzPa1bTSlZEIybtK)PK6#p-uS1|I=9^S{CJ)!kcu=?
z>7QBok9ngtxoN?y^4FZa;-kospaQgS^TT0=HP;Bu_}(df<NGM+dFds6mo8%COSxNm
z{Aba6&pLxgyrYEoHLvQ%>UZeu`UsKRRYj<RDsj4Q!}_GgwK#O@v1bjV23Y=53aU>t
z#%VDQVX7@gQ0sU30r^Z3v+%I9uDZSthCV8v^=b0Pp}z{Plg_WkVN`)srIBj?%<6y4
z8rG!df?4aYxx-S;>!R67#IQss{6Rb-CN57tl#73e;wJYgclA6ZoMW!u(FnYZ{B)|<
zSK3Sv^_*{;v+2fAy!60JeB$I{3D2+Gw?8};#TlOmS#tPQ;M^&mR;!FHSk$L$pWQ70
zPo57upNee6i8G8%aiu=s>zpFTan%z$<nhiY*YPYC$jvlT`=43okD0J0VZp5X*WA0#
z>Y`qJDY_zGQvBvpGhsi!@5@u}Zgk9|&1jEsHxY4Te)0`t{qJ|tpZn<SS7KgZG@0wa
zar9_%E+ew5WU+)2U5W%tqzK*+=eGUInhj99)L8HcO&$MQo;zm0Y!mc4vYldjs)W6g
zcF>zud4ff+#nsAWSG+A(Wc3$rE^JK|$SpKd@1I%!kJ-SQG+Zzn{WZ@W+{!*~Q;x!L
z{(;oWn}m4Bi{Z4l-RJ?!gOj7zyNMv0-6rLw4d{FJqc?F`--ylw0|FjH6Nq2tNzs^a
z>0*iVE8_A4r+Bc)h(Q1Rl{L^H^2Y8-m^9|r${Z+n5QqH+FQWDpN@5UTJ-Y7k4#;}8
zH~HCh8+<@ys6F3@2?tXJ(wIh?{4<;WF`HSF<_l(vzvi(^J>rZH%F(0s5k`k|ZW1l>
zC#5d@>P9p9`rAW3b`#9CrzWf<8c^O{#iG>WZ^S9gz_ojL0`*mJ{5m99x>(|=f6KwM
zrk{{I=Oo=G|91$P_Nqn4&7y<DA5Ju=vcOi}gZ)eg`q5Mdv!YNW3p9A*$gqO1sO#a-
zw;eOt#C@uW+)5)Y-MySitAFRtZGYg_)}+lJ_)@c#iRAX*^erov(9tyolZK%I32>xm
zg*hK*3f?_<l#%CG3h3@w6XeAb3fg_ojm^mz*cEPd!+P8jgrr(_8Yau*jsky?c%@DF
z2326uDjBF=qX~@jGOs0r=<p8LyK=o1Vo<j`Uocta8^Iz!FSs?C57<B2&u?w%CfH1P
z4qWdYMme%Czunv1fYPWEJf?z)mM>Hw_MP|H%jK5X>r9>W@?tBzYybNYjV%G76Ic3c
zZ~sc%%&4d?KB)s{W`zu=j@#gdM{LjPc~;@;RDpEjoO_bnMX-i(9&Ed;22#(2=YwmC
z35|Kx0IMA%gjr{;wu#MCf<As{TKD#PRJUm?@yKL1dg~t-xa4v;>ZeM$&zah$hvh&S
z?!5Fu!yk*&M_*jKqXc4_bagC-gK?3VP)Ku37K}Vnm7WXrg7~V8-t=ZSaO@IPZun|~
z7Yc4lyN4>CNJF%>02%hE8Lg1Orw(;#1G%2RxP|c>Vq{pUuKP>`T(!6BE!+5vsFtt4
zTmP{a#U?Z;socM}SYlI4;8=%;2YgJm=o7I^!nPR-`U3}j@cjoyr{{h<b)37I)ZQq8
zN7r7xd7Vhu_+--VWw$k)+IRW=polR3k1FhL-FLakqmo!XNd~;ze2*YwFY~PHd57+A
ztYa6g)&S=6iC3zIpNWXzgp;xg-wE^1qI?dQhs5KLYtXJE@hF`tF?j32?ly}&c&1_4
zs(92Bv*-F1l46D6#WCZNdsPneEt|0B)H(voOFEqjx%b2AN3Zi|rw`!k8fJB=HCq-7
z#hs2Go&92<vUBqiHRV;f<LU8zgNMXHS#b5cQ>&IiXkN7QjwBU${Y|gjv9JsMmm2a*
z+>{Q7&6&+_>}*2|C49u_SB5qRL8W1j@-@yJT(35(>v%R0h7)t&WE{?cy?39Ny!P`4
z^-{KJ<KRRvwH1(yNZAWl!cWf7MQh<3RDt2XTTQ0=AklfW&CJMC0Ury+I)czo#Js&$
zxz<Mv^ywQ#{A{ihdVWtQh6PmM6vGWo1&?9knDH5|d!o09T&jq4n`ZnqvmCB&5b7~1
z$i=5suD^M_#|H*qYK*dOjf1$&5i9Sm4aMru`7_M41Hj!V(ab>o6tJ#z=S=X~4D3|#
z8xJWt(>p^bAWhf9t!o`N6$y<CI(mulcz&(0S9%pV^180_CVwG;N$k;HH%v6gIJQoF
zQvvP6MGWc{7YGBYNPqXu-ujc$_>T9pXQ<T>oa4A|s4ME>PoH`C_xJ;@x3K+MnIn${
zBt~}$4H;o`{<n@beafIU?ml2Vn*c6yYM1Ga+X)l%4?9usZKSOdnrR$yn~*MZ6#4n}
z0h(Shw4oE<ME@;Gy->h%8@*>gSzOD1kC17v4|ZB{nXuYt{AG)ZGF+1A%{$~r5(MRe
zX3B$|C=a}$JaC!v01xGX1rPRHaBN<1ZJ|7bBIQ9+l!tgtd5Ghbhj>nTkSygvi;hJX
zX-gyR{%w5wKZ|pRHR<qgaXKz8PVLZyrq*9^(3jPebZoE)8~C3PP`efl-qxq2G#xVG
z&SVZ~WXHkU43nZgCtUEmJ?ZMJ-KUY)oeeC<BAz3=+kZ<8CXgIOtefFo=$CUYToXiT
z?{-mx0dcq_qH(<CSr6LsaYaFu5Fco3?(kCiy%^Z4_|{Zbe?aRZZ@)|rZ$eU33EAYT
zOI1UPa8<RhTu{IPZ;PICdeFHIh9W-8N6eAH_@%aRR7)Ie?`FF*rCVcvn=^GbvetOe
zMS`O)oE^WW3Qu|;xF<a8CT70pcb%(Ihttw`Rwi~dp#dAClSBV~A=ZnFt$X|Q1>v!z
zCEABA6AAQfb()QQh;$}nYiLoiNR}!g6}md6(l#6Ro^?wa#vb^4`mQ(s$((@Iqb|?c
zKL>+}$hPr~Q%B%q{k^EypB(URj@WW~4^J3qPLqhx(!>h|51-7wt(8(R>pB{8^hzH>
zrw9KV(bs|aardkfR`mGF9veH4hikxGboSn-%6=koTd%W2-%Hducb>KG-92=bDseHq
zGn3Q69a7GPSbV^VSY9!%czl^Y?A3eB*7hO?UcJ8gt1!S2d*0Hk+qNMVn#<3;N9Szd
zaRjz2s;9#bs6zUSUY?oQW>oTQdq{HGQrPk+FepuZ0{!sNbkIxFfZN3ueINY45?=#W
zvY)*Ci5RqIc`%rLmyjH2ug&w1Mk6o&mT3F9VY3rkE(lIC^(`y%$BHi$iu_;YfY)eV
zk3>c|XxO&zihFkiboQS%XN=;*BU6p$6Z>iS%;e>Z2el+{8C58rP3!q!C<e`G^hr!U
zT-Zw?@a|_nai}cen9ZZH!-nxFj$S)eIOLG7dYtna(iwD{E~uPGy$^RQzAL@6Si-A-
zjqi9G31Yh{UKG4Kf-S7anCV=7p?19m2gidfc(5$Rqs+$xTp9;<zJ7if^j5vvthm`1
z?CKhM!+E7)8&zB=eb+m}uM7z(7FK+Wk;5*E>z^FI-HfWlC)POZ5P?kzYUYfQ&rn8P
zZ=k)DDlE~=;wanricrxj5V`B#L2ROmfGuzLXa)-qpXFBwyXE5U`>tzF_IksszW$-z
zHt}#!cDj^qXDF`uaboI9ZZO<Ryy-GfeiE+Tu1K6bB@2iurZHwOU-kVhQc(4;A_GKm
zhq(L(?Wk51!Ngia_kKADeQ*6tr}h)wk_!Fx)Z`^`DA1*Ceq05vzc_cTv#Of7NfqgS
zH5OD<mB1=v^QFEI^`L59!Ks~VwK1K-G9#5+`dHv@@oa~fGPVisjPA<S#X-(2Y4=rx
z;Dt|m+w4d(ycnv~Z2s_$@clZadp@!SIc&9I+vs(bDD6%Su;%YXb@Tq-7Pd9WCM9C<
zntBVWO^vXa>+T`C9nZwt*47eHdr11X8#lt;G?k1c6*NMJ@_;<$!L^hJrZ@ccfFR`o
zOUeV`ln1|Da11QC_ER20m-3*Kl!wTtJcJYFAudrK^qBIX-UUa@f{S#bkvsn_PUk<1
z)5V(H^>1;yE-p@ci`|E;ZDXNCp7ZEFl@iRTYA?#fwI6=zrp|pz$b-y>U4mi{<AKSl
z@qOx#AS}>s673SX5#Z8w+r8)4gDq7|v^8!Q<sx9`kAxvELvA=?yk*R?R{}&?k=3UQ
z^JrhlJGH`y)o}ZgnXB530piq~H=WyWjG>tI9BxypH&G;2qB$Qusop074fo20<6PWu
zPViuA`&nCXH{q*1a*_tRgQI)SnX16W%PG^gS#Ef{<)#d`2`gMD^F_%2?lN3K74Vo=
zkXp_l5f`<&#Nnp~oY^$@%8cwq;@Vl)Zf^TYr0rd~JVWI*;TTp9QR<b5+o68#=$=k=
zFQ_4-(=HX=ph~FszW;7on*|pI)m8*IY{t1(n;7^~k3rIs-ouAmV!+{hWA-WDY@oAr
zJQS!JjD7b-c6t=;21CKYpcFb8tWOn`C2#PBSWCfL+E~OF?eC~-)BV<fbp$kzL@7A?
zvf%S&{%-q(h2gq)d&_#QXT)^DD=w1*!-(B=>VxU^yNe}W8kaxpOLT$R)%nKZu?aY(
z#Ck5##tL*v@n%bdTnI3!`21XD2Tt_J+Rd>k0os}T0?d0YA<to#a6{cZF-aBaJbiSY
z&9WCgxHUBzWhVgFzjcOhI`#v(TU`8cajPb<KQK$tmYgAsxWcm7`<e;j#VYZH@D^g|
zWc85QsT@>Kl{mku-dlHXHgtScYxPJ7#I$-3<~M$6!1I3YQWS48bnrB`Uk*P4%-xK3
z`u+Q`7<b5u+^PU<ArfQE7q<!wsN$1uiS*nz#NbwYanPfy9GExIPN8^M9L`AGZ5iCc
z2HOt#ZReC$hn&7|&dKgWX!}ZG*0`jfD2icb$5HnC=nz$c!(@B7UY{>8i8xF;X=P#A
zV$V;E72e=#BIBCZnhN~e4ONU-w*g<W%YGiI3|N^H!?>2*59#V<I<alWaF{CI7$CbR
ztMD~i85~dOzm&$=K}EM|e#2;sL3g>EqA=*VaoqVNIf|BcyyCQZtOj}i31sF=JR=mu
zn2Um{y9i0DXmlVz+$MCEc>Co>JMWGhe1hLN;o)^J=(8Ko3owa;lpT+Pg>yo1<I0{0
z&I2LfSoLvIWbQu@mx<13(U$^gs`&mFM)ew#kBC$6(1rGGf>^0s{MrrP=SayvLC?yC
z6~-@;Zm$phKuuhnGEbLw6QiH*-Y#CJ3ab+i^A<Z^BLt|Teb<6hZSRQUn=IX91;GS3
zTS(^hYN+6dy5`&s@rL-D(7N4gKW)P6lAA~8&Z=RJN1MBS9k?OhD4=wx{xEz^56~=O
z{!Dl+^$|X~t`$A9l9PU#bqcZgye-{AyNmjz?kCZ&N<(s^4|!8%TT!J2Uj=UOCt{y`
zVU6LwK^TtBtTYSQ1aZ<r%9qN6iM^Bu>L?F(qC5~pd0>|EfH36&1ImNv796PyE;q_U
z$ZY%TAs8tS(INTQLnu%lV!?wFC=W_raF{N*NH-ek{%>)5{8^ly)}+_J#p%7cIK9|c
zU(8I8hO%qyK9So_;1Wizur2Ct*hi;D?NLiET)WvP(#4Pf>Rq>dCQ&3_y>!L+n@y&$
zH>pmD#ccybQ^i>q3mS@Bbb+a7%fl{XQP2qqnZHyo1<GG68~PMjF}r7Vx#6wfA9j4$
zHHQ?Q5r#*+L~f3cqwW4ik@Z@wXe(91uQBQ72+I$`+1=VN-^&x{gmfrQt#H8jglq1|
zKqw3^Pqlj8qY0JUZ!DJ^bj5Ghikmn(T44ibjUX9DUTjDeKI`uM^u=_PVAwa`%XLBv
z#QF+W>Dvz=eaFayH^y0^h(6r)iRdU16jqy@-qns2S(dceZD>b{4-8F=1qx6yRl+$W
z_K3DYI^4jk$Dilg!S0QLrzI~Hz#g5gWqy;+Fv7Bx?TOP7*lp{`vfeQo_s~WMogIIF
znd#}TTYgajFBHyci_cXblL9q0KG_xRocMZA4a+`XfQ{0sgEx!Vaq6(AmO_mn969jj
zh~TZqM1hTa59i1zik&ev=S;he4pAi>vX+Ym2H8V#NZtjDE%A6rN2|nY8Ho)fW;N2{
z^Pu?Yxv=~5ZkQu|?r6lz6xh!a-1_9K4)jPJu8q`N4ezMp9hc5ZpvuQ+wA6O@nOEyT
z?67w0)+Y>jMAH1n#-h!@c>I3fZdp3Gr`ND4`u+$SlK*~Ix2~SBedD`wmvkl4qe?7Y
z)^tDk_g5A79YuE{yFlF2Aaz4@$syoIS5Uis77TTENm=G)LQik}`s+g>n9t=@V~pNz
zEZH0J%KP+s5TS~n)ZaV0VbeNbJjPsO61@Zub$pRa7+VLJyz+G0y`>;?rBURzmKJ1(
zd(7Q+9!6e_7L#`cexfIbM|WE8d$3qSe=m2YQ1%{Z(ixcL`<#XkJe`YV*zO8D%MI6R
zm&d_Lqt_QQk8<PJG?yAH@-Up&3)^*f)oys!_T%IV+3nCz6}z17@~l!CM{}}=ukG5i
z9<vD-&b);$$YE~cyLJgb+^b;ItyuFFv3fsJ4R_Oo?FW-Je0JX_1kQbD;qvY#R#HU_
zj2BYFH_>CyHj|>LeMj&C*0G~zm%ZTh-h&q=xufCy)ppUXUBQ^k#DZ~<77R*#jvw||
z7Qr=R7L`6#b!eoD2YbuytF_0G(C7YhD>_zT_xz70X&;ACNNTI;p>#SB`EZ?mWAS&y
zC2Z!?E!<2bSt^d68B&F}oW^HL+cEK;Dtc<KA5f997F+nF2v&!v!n|oO->jl6Uaozi
zaraw8d|2%EHyIUGJiFqE^)_!MJYDv7ViUhCIE`N~+)AGgj|ab<K2LlkvdU)9+}PBH
zl6=Nz`d_xA`MHCSD#h=hUl-C@PE?l?O*w14=xy3i-OUwFi7-SkJ4{D*&EF)XL&%iF
z(yGv-Sz+G3ghq5y9@tEI@OR1sOq2%}JTOjq;3wt5FBTjw3$9ekLu6APg8t`U4?$e~
z>p=@1qK5Jy4$6a`FF52DT;y&Vx#!>F^!c+m_ga(t{w>b^i;MFL+3jNO5edxjsBo-5
z5m#nws9vQ-<EgH$?a#|oAR%L5GmA_-ER%g1%B~fSXKj6C&cxV&{^Na}+C$1<K@}@B
z2==E9>qC~vcuI1p1YE%Ww+b!fAnlQS%{~V<oN`YtXVP2*PGGYwAADMfl53jm8_d6;
zyQ@@lKbf|oJ5&jSyo+Mb-wzW43a_tDU-ra|52J{0$D*)ifmKgBXB5~*M>y8!%%ZTR
zpC7ZQt7ErbkqQerD}3>5fU~v4DqKeu`a_(fbWNrRz1{Es(>kFI4GI!xZiWt^huQ09
zTc&y7cA~R+-q~+N*jP!NSlDA^S2+GPDz+8*R)nO>*`HY~A*rH6=N6d^sa9*_6y;sv
zSm2Mhx$y;XvyIg-=JsLy-V>dj(LDm@M$!E5TjFtFPlCZ#vHc)F^h5KtzciMl3Mc-1
zyH<^qhCa>ZM(+gquyoy+@Uf*^faUy!(pH^iIKAesNkjraWchiOREOOoke}j(&X95R
z!F{LOnipuXgpP;eJ*lc~Aa!S+qiHS<uaRBxsOx+N78c-MZPri#C$DEZT({qk&xssq
zO1YB-j{8nN75>DB8Mf-*n*2`=W-a~}yIRSw)CjwZZY{HM!m0A`We;DLp#w9HbGvkz
zy$&F+>Xz--7G{{s6uT7=`V+~S-O2p+IFGpe;@RGKz8k2LD$)9xoPJ%F2K39GXDBoM
z{zO6~@6w*Pg$=@OCCkflK;nY*lH!IGIK;9G|9l*dH`PjO2tRei%jpVu*Yqj_Csiyi
z7QH)0Z#^(IZr*ayU>+&ZtMC;vtcPMfq5D7Pmx7CE&e@ty0*+ljKO3MjihQqT&%S&%
zhwQd^XkS_J0NGO|h?urr_=*R-9eE^S?VN(kn-VyuAL~M9tIp-*u5jr88Z+$HX^Vr7
zToJs~84KoFf>-IRoZ;k?k7@6J+QWZTak|~aM32)1n#ig^jE17vt#i5VWzi{A!ffzi
z)lnX>Z{f?zSUQQiSk_j2R?vZzo>-N`<JH80vdb>p%AXK^R8js!ykh$UF)XviC0oWV
z9iL9V=Q4fR3!XT|xTwg6fUDXzBSXF*?8}ut_E91L9{ze3eKR^At}TzG{ikgRja2cw
z{pJz;AHJaM1dp#GTx{6Z)8bO{n$O7XnsTkh<QGC@D6?YC$agduuw+xRO%Nd)-Sk|o
zOdW2!Y<kyhbd$J46(yWu*`Iw^2-}HG)f9NiLF4_G_rgU)Ft6qQ3u?Uv*iPL~Y3j`;
z?4SK^S8k{>_WSH{Gj_#hP`=pa9@usms-xz=KfXFd^u<Jniau#Z$tR|tdJOcSHwuBQ
zob@-+X=9f)n@`pdrS<AfVoPtLtX~^9@3VYEY}sb9j_LPzzxN6q=-QvCgZD^>(;-HZ
z2%|ipNqKNR<pD0r0}hl2yeJRQC=YI3aHuZ0x+o8kL3xN*l!s`eJm@s#Ar4a>q)&NJ
z;DY1ff{XN}k$(Rcr~jYD8DLG4{}yN9;^N#f*X|OvHw-kU1fJ*BWWgDk4GA_JhjH{b
zj@yI$8$rq=Qkh;O7E19^>Hj1S;)&C{W-irkgP-jcdtzmEV9A}o#l<gw+GJRo{QihM
zu;BskdZ-iLw&bROBK$N-^gHCZ1b^`A<$TW~4xIhulyE{D>ZT2^<#PFgWQI154<<IF
zbySIz;pr#&JD0=7YXcwD^SyBA8nzT4r40P~Y8!{EK{P}W-<y95+TkOcoxjM4>;_4a
z!$p$Y5;rw|2srhD0j^WUUCvteZ3=Yo{GydY1PcKFj^)owW}l+73LF7ZQsThNa6{iL
z{uj}k`0_lTb}#Y>VN+}_zKITx1lIMvtwL#33070vQdgfiF!L&ru@tlgu8gtFx6wx-
zb8r6^xveMheuXAyE1nz>`DDetzd0Fe2#7`6M0vxrIDOXB!*Y0tDg=o>8^1*_3&NfX
zVV&cGILY|l{pEB9;5_GdOIKqB&RD*#f1fxnynQrhd-r+`+9=!dUq$pdvVZ48-&B8Y
zvBXnHy{2GIGiZII`BGjy5eHsZ9^#)a#`}G%y+247!gedw!P+|^_??zqxI1e;L<Dl&
z8Z*$w3E{Cpo|Xh8QpL^m+5WdTJR`1fsd%Se+6c;9WrXq%EyeSOgR?U;`jBpXL(_ci
za+u$mE_r>F2_Mwuv`UITi$eJ2Ci1M?Q7BcScJ*vg=9whuV2a0DAz`>x>EO8?dHZqZ
z3l>=;!F*skbbr*TJO%=KUh>J#hv10s51E$KZ^dFFLwRDZx*$Roza$5O25qFkZl{T<
zf!ic<Gp#Re_FWI8^$hoD1RIEd(*Inipa(Ob)Yr+$kE6YE_Olzd%%R!m{=LVGIu=Vj
zUi0&Pt-cGiWe#Vt*(Bht(L&B1w_NeHi|Wx*?Eb*6aN;|E>0Uf==Fz*uYxcket#c6v
z$Bf{Oz#fIXU3Tz_D)!Pf4Qwg-h_q<0J)8V^u*|gM#UH<>(N$B&44*tMDCZGoU-$dO
zroHOI75CpKK!zQ>;fY3<P?OTDIH8S$#5Jm@`?$(ik)29-^(<<YzxDh5%GbA+z3Blb
zF6d@sVn3KSa9cXC`eR?7WAdhiJIIe{TJg_j0ej!|-<Nc@0S8q~_iRO)g!eb3+(dtF
z=EhINToZVXz3~gO&20-P+xU`bRGVHQ$U28=j%^%{eO-h2>++xUY}pK_oAbAx-HwS;
zs>s;yUU``cKYnvYd-;U`ArL+PdiIH-2&OBF=IKw^g3D6h?Q6<W!luv~Y2U1dO~S6c
zVB#}`<LhRkuY68|*B4`Mm{oNXvudB0%3i8Md|l_c2YUOEW6IU5#{;WSOd$6eZPt3i
zWXHg*K;J4fq*7YE72Xj?b}}{nkiSi6C9Ar(?9c|O+l+0--}e8$xc1irEtCffQ6Bhp
z?5_tZC=VD>9<ZW3nCTMbIJw~ZN_hw&%0t*u9wLqMAXCaiyrMiPm+~Op1;>^J7a2q&
zY5x{y@Snwbz?uyCw>U!=7bkmw)fe}98dN-pmUZGRgBKrJs}KCj!s-!W4=<fc!8}X4
zUFpUm!Tj1grjvoe`1)5#W8U1~_h4;u`_Lz04EL$xZt3~2o*ym2x^`>svNmyuXJtxi
z6H|tcJR!c*Qd4N`>hsX~ixS|pGH`FYQy<!SqSXBZ{REO05IQ~5T8~7j5}MJi!sYI3
zL5F;$6WhEO7tMT*&$G_Mrz10rj$McZLx~|~#k+fPaooW|<JaME`t*8opvw%ued7H6
z@*xFSC|12S_^9?UGx+XGRVz8F3wmGf?XXYkMow+s?}j%hLUPp3r4q?Z&?utpxmFz$
zOxK7=W0q#r?x5i|7E*^gs1hk_ha1L^9fW1!6-CY$c`!e(i%!tGqtGg!e9J$f0=umK
zQL`Z@A9Rh4@UT=G=4;#Ed4|~@c#3$sxMzM}Ri+B>RlGLXX3GO9er0<mzYwM!TNQfL
z#}tw)j_a)+TY()c-*j}2aKXBjk7;=;UZXk~?J{3BhFU@nOv<pv5M@+R-I>ik?m`B@
zdTQd-o$sl*<V|PUX7y4`?;de20**pn+8b%Pmod0d_eH3UY7sbUzVQ95rHiBTpUn<e
znZZKwgqU5s=B;t!Yp}FvNxKHft>v<nmt2OcnPk*U2F>95Pwk7onX6zb!y@-#2n!~e
zgJMKBH6rVl>9s~@9xRp+JD_+g=UEIiN>9y{=)~czckRtDw}<2N$HxO}R~`imSw<zC
z;|Hcr;TrGdXm}#k@U`z(Q#?^DzFwoi9LA{PobAAyD<uQx#F%-Cn1|5`Z$Wp#mlBZZ
zo+Dgn%mxXQyxq1&#t<Bk=ziN^9KEpl5N_l31JUg2SG7JwXb)8)&`IPf+Y(#2VOSz&
z|0x=)@vhh&tr3h@ioD_*KVc5jy_FmL5<>CAjUYLd;*S+HYBED^uf?M^Z{7=ATEPdZ
zcqsc@?AeS#w5;U8mB#j;=sx53>y};LkpzFe+FBk?a4x=HcL~2nzN8Xk=TAfETU}7L
z?Ac9ZMWo6k^}QubsUnXa;fYWs6)gVFtJU&r6mDI%R&q#V7mya*mzG*MfMO-f>Sw{;
z_>-xzzDfiEhcrL5&M79sZN@?lv}?y=u~+V<gTFrqqF49C%TDloK<x=%t(4d%(G!9!
zBXkRhHL>o}!JB>}o)h#=pEMiLl#)o3#${bl-96}Wrl^)!C^FugWgPr`B|h}oo%Q*h
zVdTF~_^Vl~3_d7#al?B?O`P#Ivz&XGJRaA#{^=yN8K)236Il7h0whYleYG-+h26y;
zJ%!hG5b-0NJNB$OLp%<u$~>~R7csWJDSDW2l(6rq>|MV3GEwpH^PXCli^RzzGY(2W
zM~JfFoad*mv=Q4rNAMn~-vS@FbT`WJ`x09y55V|e58+FBAe{2R5y}ITlm`kZ5AIxW
z$Sk<3DGwT;JcK0WL6(%~_q|7dJ?I4GK^2q-(XXT&4ht?aj7En4TbvPp7Uw~0GV<Tz
zj9OfrG;@=?N1ywH;MZlfHoR2;dKpJQ^JU|}2mAK-oy^DM9m_eKm1t1qWmaD)9)Q0a
zCYG5`DdP9c(n9@KR`8B0=3qOw-L}vcxTiP^rQOzonE0IwE0%A9fckpNy#}R(JHOj(
z&Wt4Zuspc2(Xa>2rVAC4IiqO)*T-VmeFRNXB}DecOlw+*gD<a>5}O8z-6!N(DwZ6@
z>%9sRUM2;D!hx*0;fNsoX!&((EEos*`9BPn?K1p*b6kE&8jCjkq>4RS4lqw}F9)5y
zJI#ZJ^njU3Lw{mjD)MO%J}~!K6EYq$$0$m(K$>R5Q@?NDi1@Da#Y~ddkeGpO8yu-a
z>!=bI13}_4>4CsD%NWOV!3#Ue%_ywcp9e|ecjRo?FJk+1>&`ZE9s@u8@$lNRbR7RW
z<<me40~U3R^pZ1{z+zNk_O{aN2v0?ryUCO7sKtlbq|+}Ok=F1|@5PtA)D`$%e%X|{
z6C3EX7qpEld_(iJsgnJPZ_!OQuXj<lB}5ff<jsNf+CvCPSrXxP?`I}fz=gR2ZDsg5
zZ;P8$Xf6Z?*M4n2lYsjpMi`d%90!xc`=UnrTKH_`ES*k^H7pcIGS|yQJ)I#uQc5~*
zZPx`)`VZ35x+`&J|C9UoqHLg4`5Zgy;Q{$72aW13W-QvV-?HIsGx}WFcx-iR_hJc>
zFNlMF{{dLe6;jaIei)l?$!?ea7>!$OhMk7!kHN(0)Xl|PcHzU{vzWz81M$--`owKf
zmN?KTuj>fwcA!zkhgg{($~4PDf8LubIlXy=jQBZAIbt37p|f{wma#&i(Xt4$K`R)j
zSNb~kWfa*K`R5;1{egNSa9%-~H*xOJ-=eq1At5ks2+dWeJ`)c^v3}&G2nM%9c<kMt
z-ppJNygkTccui<5exX}ECEgs5_m14CPCjmk4c|Xz98m`_po-t6rt&4PZbj<t?<E*5
zripm<g9*Ei(&2-bSMTE-W`!peWvNanw~)&kvis_GD_FUE@Q%(%8%q5yIvalG6A?od
z-L~sIE?%pIgEE!rrj-0~hxa*`FC#XfE%_-+(25TysTijCSvX*Z;fS3l`?g{F#JUNI
zNE)~T7ayIP>tgZg=bKhDa84moo=k{beG$b5vPQJme?f^=v&%KEcAx_7t-hlt=8-u6
zwQTwK6==FQ>zwyC6A)6%wEo2sM9i!HExM3Y*&$%Vfa&>7W|`4?EKwb<7(T6xW#n80
z)+9^eIvo7X`m78tYQ80J?XnrKi)*{KIn5e$)2f$^;}AGqw)_5n!(GG!?N_T$2DT8x
zk2>Gzvvr|!Ym1mK;5ve(?MY;bd=8<-V%N&F?;a7BwEAQ1-Em^okKkQm8O;RU9*?Z~
zZ>Dg%=z3U+;sK(B^1yP+12U8c)=?g~N_k+x1G1C{4=*^37hG>B5BfxTh^v$bF;E`l
zKzUFE<w4w(2Q@A@SQcF5AsQL|Z*j)_S)8%fWZb{S8NawVAM<ZodPdk2?6euLCMH~l
z!+rfMk_R&J`4T}v;lKiH81uQ5v&tU)!UvzN@AJZMq=H8Jh4paxxo_LbJguOND$ag+
z^*EYv1*v<X8LKUL!M*Y86?SQ5DAjcujr{Tm4PSJbpWM10K6^)bD=4%O`Skj-^rEj&
zXIO^t$k}s*2UYa+*vLW4TWev*PPKmixIoOZ_5Pt*nOr=-#fOv3afONC%(1RLB!2Qe
zjIlE)7Mw)6^v`i=WB#fM4$l`luu$BsZJG4LiyPF081#$gj9`^yrdRvu3{fJ=necSR
z5PtBkTWXX@2i^KtPnu7!fMZi32ObW&5|VcFXZD0wAqA?$<yhXcGmdVcFL_U<R5=zq
zIcEvyY|8@YXlWH)fs0t{3$1y0%Q48s&Urchsrcd=&L(DaE4(B6sPwh(Ja{KnaIBwx
zYAhrVHXPeW55_Ry!Cf7(mXf=m^I_iY>&KX}SVM2t`OV`*&CZ904SVQt$%vhc{mw_I
zC0al)GBcWRqKa^~**n+YC#0`+bkk-znfQK=!&@$`(|EV`juQe2zrT}qddO6LCkBt(
z*FJS^EreyG<!7>@<uNmF;rYS~+hC!%;rkVPDfTBsiT?6A<~=4bp>}I5W-}M&R`{}Q
zNsKcL3<vKv<KTj`_xkQ1NaDo58XPOXan_-+U7J7VZ+eVIs1jK=dGiCkyP&_!w&+7k
zHs)%sV3j3eaL*0vuaYdqP(E?zlkSr!T#;;$Q};R$`!>p!t_`rjJ6@cc>fdb-_EfRZ
z_q^d<CDKq>vZ?pBW;gNVcv70ssx@#=rfGEU$uu!>?9{4Yb7#<DZK!%{@*bJ4Z@eHn
z`x713(mPvW-$4XXMZ)RgQ!lMWpel-0$r<m*7p>2!oGFUN6U!Bo&l`o}c$-h|p>~OQ
z4a2V)ji(vdIm-4>*iI{~ba(xe;els_Emd@d>$s9pT_3^GSyK3Ha20sRJ7ikBFyZ?@
z*f+jN`9^5Ae;hFJtR$K;!_{(CongCvqehA)Mqi?K*aqlN6Q8IeLxV2E2ls@2U&Pem
zw@+G*pYDxas>}<JFvC`LsBH%xvO2Qsu%jZL)ie!%9O8kiav15fYYi|Pi$tF9um>y@
zKe=sX=-@hmQmngfSU%_`(k(JwYa&O`K^Zz}W}7>xqD(u9SVfOV8lnwsJ1-Kt!!Lf6
z*4V>6lcpz2KGmRhs>GqC#kykx?+DjPUiMdh>R2>sF!#UX8*%<zL)_>T6Ub!nt#Y)L
z!g)CaPrr~V{$%ia_qWOIaI`4U_)?KKM8Q<3sNr+squ9v#ZRhV2tp`3Hds*9p1XHhS
zPO()JPO%THws_Pc0j_zP`m=rlZL74`e>zRv5^S0rIdz3#nqu0L%I5$tj|^~6Nn{b>
zlm|Ie9uT2CkVJX#f(LRa4~$YCd}zT@u;6+_c@U;N1TW=54wQ#jMtM*(<w0$f2Q512
z7hGfljZFM=7ktvcyWo@mY|j*HGWGBFOdt>c-ko~JYPZqK>gNCO8m|tI^n=Hs*Xoql
zSj%PXOA4@UJl_B<|Ai;S$tU8X)jfPSR(62t`Bj5D8!+Dg%auumaW~#a705Iinf`B6
zXZ)F!X-#G=%sTRSmaWJ66DOY*;D#uZY2!=T@JmC#aYuOteoNzw`6W~hc1Oh+>c?U*
zTm15-voCJK)Q?XMY0K*I&v&)m5eyD^jw+DZG&1MktlU4d@~p}Hg;@oEXE{X%qCVLy
z+@CW4^=^J56w3D)%E<hOwaipnH*6|{QInI8>xLrmdM#(=8#Py<m*-WBR#YX<GIXun
z5oLrkr~-MEMi%~?b?ncqB5SgEVb<}#v$pwM%W`yz#;tW6m)3Lchlf{bKaNW!W8v`1
zV5{TVP%&`DtVxZAdzvq~bbhXYU;j1VydQiF%OAQerpK&^<){MrAB{ZmZ`R2_vrbu)
zB@44o|D83pQc>bci6GX`UJ(^}BJlU?i<xfWromOVd(3`?0l3Jy{E@P`J3e5gy8l9w
z3)tLXPPnj@2fwEsRNxc&fM`?+vXn-i`8TWV&#ZE5vSMM@*}t<=H8_J6jiTX{wM(qR
zt`w}g@&@_kP%gMlIntk9QSkd-%hax}E_aA{u|9G<y&SvCJTjyGK8ZLTzZto;R1h4f
zV)7h~to%3Y{GVADtjUWDvo8IeWtQ*WxTZb}X5=alCn@D(11U*m_VuU0I_G=DnboH;
zPwu>VieoIeYjx-6v|q(z9y+hv9V?-{D98Kq3MHtbipeS(dHLV0>OZq;tjXGiS#^JB
zZBgL~Tb5Y}y~^lUnqm>Y#`Vfw`D8idPT1J1n4QG|duHCp3?G5hJ*sj&2`yOCPJbrm
zOC7xPtL^;$Qg5-Ctf!Gz{>{4jXI6tXd2M0V^}n+?jMP2~Z94V)V(Gzz)4Zp!(4$z!
z^pK12*g%cXa``2E*_(53{`Vp<x^exIY|34%!%_7$dliP5eZK^Y<ZR&(RZKS0$Q$ln
LUNo}FeZ&6%LTcSd

diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index 9fa81a96..fea0731e 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -21,14 +21,17 @@ def setUpClass(self):
         srdf_path = Path(robot.urdf.replace("urdf", "srdf"))
         urdf_meshes_dir = urdf_path.parent.parent.parent.parent.parent
         free_flyer = False
-        q0 = np.zeros(robot.model.nq)
-        armature = np.full(robot.model.nq, 0.1)
-
+        locked_joint_names = ["panda_joint1", "panda_joint2"]
+        reduced_nq = robot.model.nq - len(locked_joint_names)
+        full_q0 = np.zeros(robot.model.nq)
+        armature = np.full(reduced_nq, 0.1)
+        q0 = np.zeros(robot.model.nq - len(locked_joint_names))
         # Store shared initial parameters
         self.params = RobotModelParameters(
+            full_q0=full_q0,
             q0=q0,
             free_flyer=free_flyer,
-            locked_joint_names=["panda_joint1", "panda_joint2"],
+            locked_joint_names=locked_joint_names,
             urdf_path=urdf_path,
             srdf_path=srdf_path,
             urdf_meshes_dir=urdf_meshes_dir,
@@ -80,10 +83,11 @@ def create_running_model_list(self):
             # State Regularization cost
             x_residual = crocoddyl.ResidualModelState(
                 self._state,
-                np.concatenate((pin.neutral(self._rmodel), np.zeros(self._rmodel.nv))),
+                np.concatenate(
+                    (pin.neutral(self._robot_model), np.zeros(self._robot_model.nv))
+                ),
             )
             x_reg_cost = crocoddyl.CostModelResidual(self._state, x_residual)
-
             # Control Regularization cost
             u_residual = crocoddyl.ResidualModelControl(self._state)
             u_reg_cost = crocoddyl.CostModelResidual(self._state, u_residual)
@@ -91,7 +95,7 @@ def create_running_model_list(self):
             # End effector frame cost
             frame_placement_residual = crocoddyl.ResidualModelFramePlacement(
                 self._state,
-                self._rmodel.getFrameId("panda_hand_tcp"),
+                self._robot_model.getFrameId("panda_hand_tcp"),
                 pin.SE3(np.eye(3), np.array([1.0, 1.0, 1.0])),
             )
 
@@ -110,8 +114,7 @@ def create_running_model_list(self):
             running_model = crocoddyl.IntegratedActionModelEuler(
                 running_DAM,
             )
-            running_model.differential.armature = self._robot_model.armature
-
+            running_model.differential.armature = self._robot_models.armature
             running_model_list = [running_model] * (self._ocp_params.horizon_size - 1)
             return running_model_list
 
@@ -123,14 +126,16 @@ def create_terminal_model(self):
             # State Regularization cost
             x_residual = crocoddyl.ResidualModelState(
                 self._state,
-                np.concatenate((pin.neutral(self._rmodel), np.zeros(self._rmodel.nv))),
+                np.concatenate(
+                    (pin.neutral(self._robot_model), np.zeros(self._robot_model.nv))
+                ),
             )
             x_reg_cost = crocoddyl.CostModelResidual(self._state, x_residual)
 
             # End effector frame cost
             frame_placement_residual = crocoddyl.ResidualModelFramePlacement(
                 self._state,
-                self._rmodel.getFrameId("panda_hand_tcp"),
+                self._robot_model.getFrameId("panda_hand_tcp"),
                 pin.SE3(np.eye(3), np.array([1.0, 1.0, 1.0])),
             )
 
@@ -148,8 +153,7 @@ def create_terminal_model(self):
             )
 
             terminal_model = crocoddyl.IntegratedActionModelEuler(terminal_DAM, 0.0)
-            terminal_model.differential.armature = self._robot_model.armature
-
+            terminal_model.differential.armature = self._robot_models.armature
             return terminal_model
 
         def set_reference_horizon(self, horizon_size):
@@ -168,19 +172,23 @@ def setUpClass(self):
         srdf_path = Path(robot.urdf.replace("urdf", "srdf"))
         urdf_meshes_dir = urdf_path.parent.parent.parent.parent.parent
         free_flyer = False
-        q0 = np.zeros(robot.model.nq)
-
+        locked_joint_names = ["panda_finger_joint1", "panda_finger_joint2"]
+        reduced_nq = robot.model.nq - len(locked_joint_names)
+        full_q0 = np.zeros(robot.model.nq)
+        q0 = np.zeros(reduced_nq)
+        armature = np.full(reduced_nq, 0.1)
         # Store shared initial parameters
         self.params = RobotModelParameters(
             q0=q0,
+            full_q0=full_q0,
             free_flyer=free_flyer,
-            locked_joint_names=["panda_joint1", "panda_joint2"],
+            locked_joint_names=locked_joint_names,
             urdf_path=urdf_path,
             srdf_path=srdf_path,
             urdf_meshes_dir=urdf_meshes_dir,
             collision_as_capsule=True,
             self_collision=True,
-            armature=np.linspace(0.1, 0.9, robot.model.nq),
+            armature=armature,
         )
 
         self.robot_models = RobotModels(self.params)
@@ -205,6 +213,7 @@ def setUpClass(self):
         self.ocp.solve(self.state_reg, self.state_warmstart, self.control_warmstart)
         # self.save_results()
 
+    @classmethod
     def save_results(self):
         results = np.array(
             [

From cede4b6aecf97a29f6dd3065aecc21e6f16e5c3a Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Mon, 13 Jan 2025 10:45:43 +0000
Subject: [PATCH 86/95] fixing unit test of robot models

---
 agimus_controller/tests/test_robot_models.py | 46 ++++++++++++--------
 1 file changed, 28 insertions(+), 18 deletions(-)

diff --git a/agimus_controller/tests/test_robot_models.py b/agimus_controller/tests/test_robot_models.py
index c8ab1813..fd792c2e 100644
--- a/agimus_controller/tests/test_robot_models.py
+++ b/agimus_controller/tests/test_robot_models.py
@@ -18,9 +18,12 @@ def test_valid_initialization(self):
         srdf_path = Path(robot.urdf.replace("urdf", "srdf"))
         urdf_meshes_dir = urdf_path.parent.parent.parent.parent.parent
         free_flyer = False
-        q0 = np.zeros(robot.model.nq)
+        full_q0 = np.zeros(robot.model.nq)
         locked_joint_names = ["panda_joint1", "panda_joint2"]
+        reduced_nq = robot.model.nq - len(locked_joint_names)
+        q0 = np.zeros(reduced_nq)
         params = RobotModelParameters(
+            full_q0=full_q0,
             q0=q0,
             free_flyer=free_flyer,
             locked_joint_names=locked_joint_names,
@@ -29,9 +32,9 @@ def test_valid_initialization(self):
             urdf_meshes_dir=urdf_meshes_dir,
             collision_as_capsule=True,
             self_collision=True,
-            armature=np.linspace(0.1, 0.9, 9),
+            armature=np.linspace(0.1, 0.9, reduced_nq),
         )
-
+        self.assertTrue(np.array_equal(params.full_q0, full_q0))
         self.assertTrue(np.array_equal(params.q0, q0))
         self.assertEqual(params.free_flyer, free_flyer)
         self.assertEqual(params.locked_joint_names, ["panda_joint1", "panda_joint2"])
@@ -40,7 +43,9 @@ def test_valid_initialization(self):
         self.assertEqual(params.urdf_meshes_dir, urdf_meshes_dir)
         self.assertTrue(params.collision_as_capsule)
         self.assertTrue(params.self_collision)
-        self.assertTrue(np.array_equal(params.armature, np.linspace(0.1, 0.9, 9)))
+        self.assertTrue(
+            np.array_equal(params.armature, np.linspace(0.1, 0.9, reduced_nq))
+        )
 
     def test_empty_q0_raises_error(self):
         """Test that an empty q0 raises a ValueError."""
@@ -55,8 +60,11 @@ def test_armature_default_value(self):
         urdf_meshes_dir = urdf_path.parent.parent.parent.parent.parent
         locked_joint_names = ["panda_joint1", "panda_joint2"]
         free_flyer = False
-        q0 = np.zeros(robot.model.nq)
+        full_q0 = np.zeros(robot.model.nq)
+        locked_joint_names = ["panda_joint1", "panda_joint2"]
+        q0 = np.zeros(robot.model.nq - len(locked_joint_names))
         params = RobotModelParameters(
+            full_q0=full_q0,
             q0=q0,
             free_flyer=free_flyer,
             locked_joint_names=locked_joint_names,
@@ -70,27 +78,27 @@ def test_armature_default_value(self):
 
     def test_armature_mismatched_shape_raises_error(self):
         """Test that a mismatched armature raises a ValueError."""
-        q0 = np.array([0.0, 1.0, 2.0])
+        full_q0 = np.array([0.0, 1.0, 2.0])
         armature = np.array([0.1, 0.2])  # Wrong shape
         with self.assertRaises(ValueError):
-            RobotModelParameters(q0=q0, armature=armature)
+            RobotModelParameters(full_q0=full_q0, armature=armature)
 
     def test_invalid_urdf_path_raises_error(self):
         """Test that an invalid URDF path raises a ValueError."""
-        q0 = np.array([0.0, 1.0, 2.0])
+        full_q0 = np.array([0.0, 1.0, 2.0])
         with self.assertRaises(ValueError):
-            RobotModelParameters(q0=q0, urdf_path=Path("invalid_path"))
+            RobotModelParameters(full_q0=full_q0, urdf_path=Path("invalid_path"))
 
     def test_invalid_srdf_path_type_raises_error(self):
         """Test that a non-string SRDF path raises a ValueError."""
-        q0 = np.array([0.0, 1.0, 2.0])
+        full_q0 = np.array([0.0, 1.0, 2.0])
         with self.assertRaises(ValueError):
-            RobotModelParameters(q0=q0, srdf_path=Path("invalid_path"))
+            RobotModelParameters(full_q0=full_q0, srdf_path=Path("invalid_path"))
 
 
 class TestRobotModels(unittest.TestCase):
     @classmethod
-    def setUpClass(self):
+    def setUpClass(cls):
         """
         This method sets up the shared environment for all test cases in the class.
         """
@@ -100,10 +108,12 @@ def setUpClass(self):
         srdf_path = Path(robot.urdf.replace("urdf", "srdf"))
         urdf_meshes_dir = urdf_path.parent.parent.parent.parent.parent
         free_flyer = False
-        q0 = np.zeros(robot.model.nq)
+        full_q0 = np.zeros(robot.model.nq)
         locked_joint_names = ["panda_joint1", "panda_joint2"]
-        # Store shared initial parameters
-        self.shared_params = RobotModelParameters(
+        reduced_nq = robot.model.nq - len(locked_joint_names)
+        q0 = np.zeros(reduced_nq)
+        cls.params = RobotModelParameters(
+            full_q0=full_q0,
             q0=q0,
             free_flyer=free_flyer,
             locked_joint_names=locked_joint_names,
@@ -112,15 +122,15 @@ def setUpClass(self):
             urdf_meshes_dir=urdf_meshes_dir,
             collision_as_capsule=True,
             self_collision=True,
-            armature=np.linspace(0.1, 0.9, robot.model.nq),
+            armature=np.linspace(0.1, 0.9, reduced_nq),
         )
 
     def setUp(self):
         """
         This method ensures that a fresh RobotModelParameters and RobotModels instance
-        are created for each test case while still benefiting from shared setup computations.
+        are created for each test case.
         """
-        self.params = deepcopy(self.shared_params)
+        self.params = deepcopy(self.params)
         self.robot_models = RobotModels(self.params)
 
     def test_initial_configuration(self):

From a60d68085553c4d8849218c8f1cb2700c43511b1 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Mon, 13 Jan 2025 14:41:41 +0100
Subject: [PATCH 87/95] Update agimus_controller/tests/test_ocp_croco_base.py

Co-authored-by: Naveau <maximilien.naveau@gmail.com>
---
 agimus_controller/tests/test_ocp_croco_base.py | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index fea0731e..30f96489 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -213,7 +213,15 @@ def setUpClass(self):
         self.ocp.solve(self.state_reg, self.state_warmstart, self.control_warmstart)
         # self.save_results()
 
-    @classmethod
+    def get_results_as_numpy(self):
+         return np.array([
+                self.ocp.ocp_results.states.tolist(),
+                self.ocp.ocp_results.ricatti_gains.tolist(),
+                self.ocp.ocp_results.feed_forward_terms.tolist(),
+            ], dtype=object,  # Ensure the array is dtype=object before saving)
+
+    def save_results(self, destination=pathlib.Path("ressources/simple_ocp_croco_results.npy")):
+            np.save(str(destination), self.get_results_as_numpy())
     def save_results(self):
         results = np.array(
             [

From 3227e9ef5569947fa73089cad5a85be5ad3e9d83 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Mon, 13 Jan 2025 13:47:19 +0000
Subject: [PATCH 88/95] order of import / duplicate import / typo

---
 .../agimus_controller/factory/robot_model.py         |  1 -
 .../agimus_controller/ocp_base_croco.py              |  4 ++--
 agimus_controller/tests/test_ocp_croco_base.py       | 12 ++++++++----
 3 files changed, 10 insertions(+), 7 deletions(-)

diff --git a/agimus_controller/agimus_controller/factory/robot_model.py b/agimus_controller/agimus_controller/factory/robot_model.py
index b1d8f9a5..ae133145 100644
--- a/agimus_controller/agimus_controller/factory/robot_model.py
+++ b/agimus_controller/agimus_controller/factory/robot_model.py
@@ -1,4 +1,3 @@
-from dataclasses import dataclass, field
 from copy import deepcopy
 from dataclasses import dataclass, field
 from pathlib import Path
diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index e5d6077b..2af08e4f 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -26,8 +26,8 @@ def __init__(
         """
         # Setting the robot model
         self._robot_models = robot_models
-        self._robot_model = self._robot_model.robot_model
-        self._collision_model = self._robot_model.collision_model
+        self._robot_model = self._robot_models.robot_model
+        self._collision_model = self._robot_models.collision_model
         self._armature = self._robot_models._params.armature
 
         # Stat and actuation model
diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index 30f96489..e2ee6621 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -214,14 +214,18 @@ def setUpClass(self):
         # self.save_results()
 
     def get_results_as_numpy(self):
-         return np.array([
+        return np.array(
+            [
                 self.ocp.ocp_results.states.tolist(),
                 self.ocp.ocp_results.ricatti_gains.tolist(),
                 self.ocp.ocp_results.feed_forward_terms.tolist(),
-            ], dtype=object,  # Ensure the array is dtype=object before saving)
+            ],
+            dtype=object,  # Ensure the array is dtype=object before saving)
+        )
+
+    def save_results(self, destination=Path("ressources/simple_ocp_croco_results.npy")):
+        np.save(str(destination), self.get_results_as_numpy())
 
-    def save_results(self, destination=pathlib.Path("ressources/simple_ocp_croco_results.npy")):
-            np.save(str(destination), self.get_results_as_numpy())
     def save_results(self):
         results = np.array(
             [

From 3436ee96df062d82f93f3c935db76e6da200d68c Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Mon, 13 Jan 2025 14:49:03 +0100
Subject: [PATCH 89/95] Update
 agimus_controller/agimus_controller/ocp_base_croco.py

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 agimus_controller/agimus_controller/ocp_base_croco.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index e5d6077b..f65f0400 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -21,7 +21,7 @@ def __init__(
         """Defines common behavior for all OCP using croccodyl. This is an abstract class with some helpers to design OCPs in a more friendly way.
 
         Args:
-            robot_models (RobotModelFactory): All models of the robot.
+            robot_models (RobotModels): All models of the robot.
             ocp_params (OCPParamsBaseCroco): Input data structure of the OCP.
         """
         # Setting the robot model

From 4da3b41e95f0f7c78d471e6f7b3ca9d9d487212d Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Mon, 13 Jan 2025 14:49:52 +0100
Subject: [PATCH 90/95] Update
 agimus_controller/agimus_controller/ocp_base_croco.py

Co-authored-by: Krzysztof Wojciechowski <49921081+Kotochleb@users.noreply.github.com>
---
 agimus_controller/agimus_controller/ocp_base_croco.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index f65f0400..ebdc6c02 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -111,7 +111,7 @@ def solve(
         self._ocp.solve([x0] + x_warmstart, u_warmstart, self._ocp_params.solver_iters)
 
         # Store the results
-        self.ocp_results = OCPResults(
+        self._ocp_results = OCPResults(
             states=self._ocp.xs,
             ricatti_gains=self._ocp.K,
             feed_forward_terms=self._ocp.us,

From da7724f03179f00b7d965f39c63b1aafecf93438 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Mon, 13 Jan 2025 14:06:19 +0000
Subject: [PATCH 91/95] new pickle way of loading the benchmark

---
 .../agimus_controller/ocp_base_croco.py       |   2 +
 ...sults.npy => simple_ocp_croco_results.pkl} | Bin 10077 -> 9561 bytes
 .../tests/test_ocp_croco_base.py              |  49 +++++++-----------
 3 files changed, 20 insertions(+), 31 deletions(-)
 rename agimus_controller/tests/ressources/{simple_ocp_croco_results.npy => simple_ocp_croco_results.pkl} (54%)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 2af08e4f..98408a5e 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -38,6 +38,8 @@ def __init__(
         self._ocp_params = ocp_params
         self._ocp = None
         self._ocp_results = None
+        self.running_model_list = None
+        self.terminal_model = None
 
     @property
     def horizon_size(self) -> int:
diff --git a/agimus_controller/tests/ressources/simple_ocp_croco_results.npy b/agimus_controller/tests/ressources/simple_ocp_croco_results.pkl
similarity index 54%
rename from agimus_controller/tests/ressources/simple_ocp_croco_results.npy
rename to agimus_controller/tests/ressources/simple_ocp_croco_results.pkl
index 03abea3f9202e9fd80ea8a0d7f9af97d19c28069..1ea7aae7427452d1f351d2260843b67644484add 100644
GIT binary patch
delta 1014
zcmccXchk$Mfpw~%DgzkQPSNOLD=tYaNiCid3uFuBmF5;y>LuqFrRo8hsYRK|Q+l}L
z(~9zQlS<RlQj4Zco}w|WU~(a2%H#${VgD&TtY95LI;EttAQdRx!<1$*rL&`bO3)My
zZ{{9Gn<;*NeqR5908Dr@luSwT=4+kO!{|Jv1gI#36=)#DH4Kw~G8RZq$>0IWX9!@(
z_A=p<Rb<8|yA`jjCksBkck#*=vf|Ur%7#yNCR>3vihomkc!6P_SW=Q1pPrbR2aXj`
z2njHQf@rfTyAU%tfY`nH+K_?>DxJrLS9&=wUg-w{c%>yp@Ja`X<CX4~!Yh457O(VQ
z1tDe=P(UI1JGDnBEj2YIJ}tkfJh3Pxz9h9M7am>~uoPvXVgU&-<P-&$oq|{P172Al
hReUC|z$?q5hEHz-UfDf(WhK-t1i%3W^KWXY9soGzC8huX

delta 1533
zcmbu+S$9)a6bJB|hB5>wP%V@>Kp?4vii}~l1P~fX%Mq|BrDnJ_NSow;vkMdoEkY;(
z9BM%X6lHK^P#GLRSu9?7;sfYfegYSt0q^cP?B%=X;jDG``R)IH*nigl9q;asZ{5_m
z-DB;s4g}M7A>{;@`h#5!!LUErnsdlWWLt7h+UD}QL|egj>xE1rZ@cf%{BTX!f6)Jb
zPPebeo62^y=R0dtImfPT?`Wf43CBrv`dS=2mCF{$=}3_emR7!OTK~cmvx+E-dXn(=
zVvs_4DCUWIix_NRnmY5g4?~*DEX!($U}))hFNP`jLNRZX$E|f?cs$-3uYcyAfh)F(
zFd|uM#>h}JDnc16GcaRR#vL$PVGIwbD55f>Gg~8gfiquJsCu5c9Ao40|3+GbamiA&
zuMPoJ6UHZ>r%vG1i3*cU>f}sSDVpye>d6N%g)mj1pW!rx=?2cwu#aJYFjL?xhCzjp
zfi)UVD0AUUgxLba4Cg4+8u+q?R~XJEydrQO!}$se3|y#T*z3YYgvA0Q43{V@HE@}R
zcNs1xtPr@8;VOk!4Xo2}<scWXCPW3UVYpUdoq?}u_?Y2(!Ulo$3}Xr#4cw$*V|hM|
zIH4hFH=|K@xtX<6c%Ai-Sj91HasBGi-rN6_S9*ikO4ugRZ!m6B*e>`@gU1eI{1#z{
z;AX}<6<P$pZE)QP#tA}FaEftS!4}+VaN8)x?+`MAcQJlfVYlEmgU?nnZYN{~=NRV|
zAlNbZQ5E9?fdqFj-lMQr@IHf|Rx|D->=%51ahJkD!G{d4p2WDD&?ERT<M$Mf2tI0X
zbZVuy21UX#-b}A-<~Vnh6iy6u6min^(MW%3dL)2Tg!d)-1BM?ed}LsshJP{qn9wiq
zG{Z9rpBQ*n!`U-j_$lF>!1D|*D12t%=Nfh~yh!*$;Fk<9DO@)2iiS@ZUL|}b@N0(O
zD12++cN$I!xbS<zHG$U|-cYz{;13$+82(83N#HGpw-xRfcvr)R41Xs4BJfv+zbV`^
W@V<s)W=2Nhcfub@+a^3n*8U5IBHqvd

diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index e2ee6621..657e7ad4 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -6,7 +6,7 @@
 import example_robot_data as robex
 import numpy as np
 import pinocchio as pin
-
+import pickle
 from agimus_controller.ocp_base_croco import OCPBaseCroco
 from agimus_controller.ocp_param_base import OCPParamsBaseCroco
 from agimus_controller.factory.robot_model import RobotModels, RobotModelParameters
@@ -211,39 +211,26 @@ def setUpClass(self):
         # Create a concrete implementation of OCPBaseCroco
         self.ocp = self.TestOCPCroco(self.robot_models, self.ocp_params)
         self.ocp.solve(self.state_reg, self.state_warmstart, self.control_warmstart)
-        # self.save_results()
-
-    def get_results_as_numpy(self):
-        return np.array(
-            [
-                self.ocp.ocp_results.states.tolist(),
-                self.ocp.ocp_results.ricatti_gains.tolist(),
-                self.ocp.ocp_results.feed_forward_terms.tolist(),
-            ],
-            dtype=object,  # Ensure the array is dtype=object before saving)
-        )
-
-    def save_results(self, destination=Path("ressources/simple_ocp_croco_results.npy")):
-        np.save(str(destination), self.get_results_as_numpy())
+        self.save_results()
 
+    @classmethod
     def save_results(self):
-        results = np.array(
-            [
-                self.ocp.ocp_results.states.tolist(),
-                self.ocp.ocp_results.ricatti_gains.tolist(),
-                self.ocp.ocp_results.feed_forward_terms.tolist(),
-            ],
-            dtype=object,  # Ensure the array is dtype=object before saving
-        )
-        np.save(
-            "ressources/simple_ocp_croco_results.npy",
-            results,
-        )
+        results = {
+            "states": self.ocp.ocp_results.states.tolist(),
+            "ricatti_gains": self.ocp.ocp_results.ricatti_gains.tolist(),
+            "feed_forward_terms": self.ocp.ocp_results.feed_forward_terms.tolist(),
+        }
+
+        with open("ressources/simple_ocp_croco_results.pkl", "wb") as handle:
+            pickle.dump(results, handle, protocol=pickle.HIGHEST_PROTOCOL)
 
     def test_check_results(self):
-        results = np.load("ressources/simple_ocp_croco_results.npy", allow_pickle=True)
+        file_path = "ressources/simple_ocp_croco_results.pkl"
+        # Load the results
+        with open(file_path, "rb") as handle:
+            results = pickle.load(handle)
         # Checking the states
-        for iter, state in enumerate(results[0]):
+        for iter, state in enumerate(results["states"]):
             np.testing.assert_array_almost_equal(
                 state,
                 self.ocp.ocp_results.states.tolist()[iter],
@@ -251,7 +238,7 @@ def test_check_results(self):
             )
 
         # Checking the ricatti gains
-        for iter, gain in enumerate(results[1]):
+        for iter, gain in enumerate(results["ricatti_gains"]):
             np.testing.assert_array_almost_equal(
                 gain,
                 self.ocp.ocp_results.ricatti_gains.tolist()[iter],
@@ -259,7 +246,7 @@ def test_check_results(self):
             )
 
         # Checking the feed forward terms
-        for iter, term in enumerate(results[2]):
+        for iter, term in enumerate(results["feed_forward_terms"]):
             np.testing.assert_array_almost_equal(
                 term,
                 self.ocp.ocp_results.feed_forward_terms.tolist()[iter],

From ca034e1ba5e28dc91ff0507cdbb75f69946cc7a6 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <arthur.haffemayer@yahoo.fr>
Date: Mon, 13 Jan 2025 14:27:27 +0000
Subject: [PATCH 92/95] getting rid of shortcut robot model

---
 .../agimus_controller/ocp_base_croco.py         | 13 ++++++-------
 agimus_controller/tests/test_ocp_croco_base.py  | 17 ++++++++++-------
 2 files changed, 16 insertions(+), 14 deletions(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 19ec43a4..d95f6222 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -26,20 +26,19 @@ def __init__(
         """
         # Setting the robot model
         self._robot_models = robot_models
-        self._robot_model = self._robot_models.robot_model
         self._collision_model = self._robot_models.collision_model
         self._armature = self._robot_models._params.armature
 
         # Stat and actuation model
-        self._state = crocoddyl.StateMultibody(self._robot_model)
+        self._state = crocoddyl.StateMultibody(self._robot_models.robot_model)
         self._actuation = crocoddyl.ActuationModelFull(self._state)
 
         # Setting the OCP parameters
         self._ocp_params = ocp_params
         self._ocp = None
         self._ocp_results = None
-        self.running_model_list = None
-        self.terminal_model = None
+        self._running_model_list = None
+        self._terminal_model = None
 
     @property
     def horizon_size(self) -> int:
@@ -88,12 +87,12 @@ def solve(
 
         if self._ocp is None:
             # Create the running models
-            self.running_model_list = self.create_running_model_list()
+            self._running_model_list = self.create_running_model_list()
             # Create the terminal model
-            self.terminal_model = self.create_terminal_model()
+            self._terminal_model = self.create_terminal_model()
             # Create the shooting problem
             self._problem = crocoddyl.ShootingProblem(
-                x0, self.running_model_list, self.terminal_model
+                x0, self._running_model_list, self._terminal_model
             )
             # Create solver + callbacks
             self._ocp = mim_solvers.SolverCSQP(self._problem)
diff --git a/agimus_controller/tests/test_ocp_croco_base.py b/agimus_controller/tests/test_ocp_croco_base.py
index 657e7ad4..6a523dd1 100644
--- a/agimus_controller/tests/test_ocp_croco_base.py
+++ b/agimus_controller/tests/test_ocp_croco_base.py
@@ -1,6 +1,5 @@
 from os.path import dirname
 import unittest
-from unittest.mock import MagicMock
 from pathlib import Path
 import crocoddyl
 import example_robot_data as robex
@@ -41,8 +40,6 @@ def setUpClass(self):
         )
 
         self.robot_models = RobotModels(self.params)
-        self.robot_model = self.robot_models.robot_model
-        self.collision_model = self.robot_models.collision_model
 
         self.ocp_params = OCPParamsBaseCroco(
             dt=0.1, horizon_size=10, solver_iters=100, callbacks=True
@@ -84,7 +81,10 @@ def create_running_model_list(self):
             x_residual = crocoddyl.ResidualModelState(
                 self._state,
                 np.concatenate(
-                    (pin.neutral(self._robot_model), np.zeros(self._robot_model.nv))
+                    (
+                        pin.neutral(self._robot_models.robot_model),
+                        np.zeros(self._robot_models.robot_model.nv),
+                    )
                 ),
             )
             x_reg_cost = crocoddyl.CostModelResidual(self._state, x_residual)
@@ -95,7 +95,7 @@ def create_running_model_list(self):
             # End effector frame cost
             frame_placement_residual = crocoddyl.ResidualModelFramePlacement(
                 self._state,
-                self._robot_model.getFrameId("panda_hand_tcp"),
+                self._robot_models.robot_model.getFrameId("panda_hand_tcp"),
                 pin.SE3(np.eye(3), np.array([1.0, 1.0, 1.0])),
             )
 
@@ -127,7 +127,10 @@ def create_terminal_model(self):
             x_residual = crocoddyl.ResidualModelState(
                 self._state,
                 np.concatenate(
-                    (pin.neutral(self._robot_model), np.zeros(self._robot_model.nv))
+                    (
+                        pin.neutral(self._robot_models.robot_model),
+                        np.zeros(self._robot_models.robot_model.nv),
+                    )
                 ),
             )
             x_reg_cost = crocoddyl.CostModelResidual(self._state, x_residual)
@@ -135,7 +138,7 @@ def create_terminal_model(self):
             # End effector frame cost
             frame_placement_residual = crocoddyl.ResidualModelFramePlacement(
                 self._state,
-                self._robot_model.getFrameId("panda_hand_tcp"),
+                self._robot_models.robot_model.getFrameId("panda_hand_tcp"),
                 pin.SE3(np.eye(3), np.array([1.0, 1.0, 1.0])),
             )
 

From e8ada6038be5c5ba6c26ac9b47b78b16be5a1605 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Mon, 13 Jan 2025 20:48:58 +0100
Subject: [PATCH 93/95] Update
 agimus_controller/agimus_controller/ocp_base_croco.py

Co-authored-by: Naveau <maximilien.naveau@gmail.com>
---
 agimus_controller/agimus_controller/ocp_base_croco.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index d95f6222..ac1cdf86 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -37,7 +37,7 @@ def __init__(
         self._ocp_params = ocp_params
         self._ocp = None
         self._ocp_results = None
-        self._running_model_list = None
+        self._running_model_list = []
         self._terminal_model = None
 
     @property

From 103f20a4bd183b6c7c24a72610d89c98e278ef98 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Mon, 13 Jan 2025 20:49:11 +0100
Subject: [PATCH 94/95] Update
 agimus_controller/agimus_controller/ocp_base_croco.py

Co-authored-by: Naveau <maximilien.naveau@gmail.com>
---
 agimus_controller/agimus_controller/ocp_base_croco.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index ac1cdf86..2a534e58 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -38,7 +38,7 @@ def __init__(
         self._ocp = None
         self._ocp_results = None
         self._running_model_list = []
-        self._terminal_model = None
+        self._terminal_model = []
 
     @property
     def horizon_size(self) -> int:

From b6a9d0e1ecf8627bdb617b6a041e3890c4268340 Mon Sep 17 00:00:00 2001
From: Arthur Haffemayer <106057062+ArthurH91@users.noreply.github.com>
Date: Mon, 13 Jan 2025 20:49:26 +0100
Subject: [PATCH 95/95] Update
 agimus_controller/agimus_controller/ocp_base_croco.py

Co-authored-by: TheoMF <140606863+TheoMF@users.noreply.github.com>
---
 agimus_controller/agimus_controller/ocp_base_croco.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/agimus_controller/agimus_controller/ocp_base_croco.py b/agimus_controller/agimus_controller/ocp_base_croco.py
index 2a534e58..fa45b033 100644
--- a/agimus_controller/agimus_controller/ocp_base_croco.py
+++ b/agimus_controller/agimus_controller/ocp_base_croco.py
@@ -39,6 +39,7 @@ def __init__(
         self._ocp_results = None
         self._running_model_list = []
         self._terminal_model = []
+        self._problem = None
 
     @property
     def horizon_size(self) -> int: