step movement ready for test

python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py

@@ -12,7 +12,7 @@ from time import time
 class OCP:
     def __init__(self, pd: ProblemData, footsteps, gait):
         self.pd = pd
-        self.max_iter = 1
+        self.max_iter = 1000
 
         self.state = crocoddyl.StateMultibody(self.pd.model)
         self.initialized = False

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -181,11 +181,11 @@ class ProblemDataFull(problemDataAbstract):
         # Cost function weights
         # Cost function weights
         self.mu = 0.7
-        self.foot_tracking_w = 1e3
+        self.foot_tracking_w = 1e5
         # self.friction_cone_w = 1e3 * 0
         self.control_bound_w = 1e3
-        self.control_reg_w = 1e0
-        self.state_reg_w = np.array([1e-5] * 3 + [1e0] * 3)
+        self.control_reg_w = 1e-1
+        self.state_reg_w = np.array([1e-1] * 3 + [1e-1] * 3)
         self.terminal_velocity_w = np.array([0] * 3 + [1e3] * 3)
 
         self.q0_reduced = self.q0[10:13]