good foot reaching

still have to modify the interpolation

config/walk_parameters.yaml

@@ -22,7 +22,7 @@ robot:
     # q_init: [0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407]  # h_com = 0.218
     q_init: [0.0, 0.7, -1.4, 0.0, 0.7, -1.4, 0.0, -0.7, 1.4, 0.0, -0.7, 1.4]  # Initial articular positions
     dt_wbc: 0.001  # Time step of the whole body control
-    dt_mpc: 0.01  # Time step of the model predictive control
+    dt_mpc: 0.015  # Time step of the model predictive control
     type_MPC: 3  # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
 #     Kp_main: [0.0, 0.0, 0.0]  # Proportional gains for the PD+
     Kp_main: [3,3,3]  # Proportional gains for the PD+

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -8,7 +8,7 @@ class problemDataAbstract:
         self.dt_sim = param.dt_wbc
         self.r1 = int(self.dt / self.dt_sim)
         self.init_steps = 0
-        self.target_steps =  90
+        self.target_steps =  20
         self.T = self.init_steps + self.target_steps -1
 
         self.robot = erd.load("solo12")