diff --git a/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py b/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py
index 8ccd20eada556597969baf85357c8852e4bbfc58..cb1ac42bcdff50f7d5e3579340ec2d11d39face6 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CasadiOCP.py
@@ -15,7 +15,7 @@ class NodeData:
 
     Attributes:
         x (vector): State vector (q + dq)
-        cost (float): Cost for this node
+        costs (dict): Costs for this node
         isTerminal (bool): True if last node of shooting problem
         a (vector): Acceleration slack variables (ddq)
         u (vector): Control vector (tau)
@@ -28,7 +28,7 @@ class NodeData:
 
     def __init__(self, isTerminal=False):
         self.x = None
-        self.cost = None
+        self.costs = {}
         self.isTerminal = isTerminal
         if not isTerminal:
             self.a = None
@@ -119,7 +119,7 @@ class OCP:
         self.make_opti_variables(x0)
 
         # Assemble the optimization control problem
-        self.cost, eq_constraints = self.make_ocp(targets, gait)
+        self.costs["cost"], eq_constraints = self.make_ocp(targets, gait, x0)
 
         # Set the costs to be minimized ...
         self.opti.minimize(self.cost)
@@ -132,7 +132,7 @@ class OCP:
         s_opts = {
             "tol": 1e-4,
             "acceptable_tol": 1e-4,
-            # "max_iter": 21,
+            "max_iter": 11,
             # "compl_inf_tol": 1e-2,
             # "constr_viol_tol": 1e-2
             # "resto_failure_feasibility_threshold": 1
@@ -143,17 +143,55 @@ class OCP:
         # Warm start the OCP with initial value and guess
         self.warmstart(x0, guess)
 
+        self.log_costs = {}
+        for cost in self.datas[0].costs:
+            self.log_costs[cost] = []
+        self.opti.callback(lambda i: self.my_debug(i))
+
         # Solve the optimal control problem
         self.opti.solve_limited()
 
         # Store state of each node after the solving process
         for data in self.datas:
             data.x = self.opti.value(data.x)
-            data.cost = self.opti.value(data.cost)
+            for cost in data.costs:
+                data.costs[cost] = self.opti.value(data.costs[cost])
             if not data.isTerminal:
                 data.u = self.opti.value(data.u)
                 data.xnext = self.opti.value(data.xnext)
 
+        print("-- End of solve() --")
+        """from IPython import embed
+        embed()"""
+    
+    def my_debug(self, i):
+        print("---" + str(i) + "---")
+        for j in range(4):
+            print(self.opti.debug.value(self.datas[0].f[j]))
+
+        for cost in self.datas[0].costs:
+            self.log_costs[cost].append([self.opti.value(data.costs[cost]) for data in self.datas])
+    
+        """for data in self.datas:
+            for cost in data.costs:
+                self.log_costs[cost].append(self.opti.value(data.costs[cost]))"""
+
+        """log_vel_base_iter = []
+        log_friction_cone_iter = []
+        for data in self.datas:
+            log_vel_base_iter.append(self.opti.value(data.cost_vel_track))
+            log_friction_cone_iter.append(self.opti.value(data.cost_friction_cone))
+        self.log_vel_base.append(log_vel_base_iter)
+        self.log_friction_cone.append(log_friction_cone_iter)
+        # print("Base vel: ", self.opti.value(self.datas[1].cost_vel_track))
+        print("Base vel: ", log_vel_base_iter)"""
+
+        """x_test = []
+        for data in self.datas:
+            # x_test = x_test.append(self.opti.value(data.x)[19 + 0])
+            x_test = x_test.append(data.isTerminal)
+        # print(x_test)"""
+
     def make_opti_variables(self, x0):
         """Create the optimization variables of the OCP and initialize them