circular motion ready for testing

### Not with integration

config/walk_parameters.yaml

@@ -11,9 +11,9 @@ robot:
     envID: 0  # Identifier of the environment to choose in which one the simulation will happen
     use_flat_plane: true  # If True the ground is flat, otherwise it has bumps
     predefined_vel: true  # If we are using a predefined reference velocity (True) or a joystick (False)
-    N_SIMULATION: 5000  # Number of simulated wbc time steps
+    N_SIMULATION: 10000  # Number of simulated wbc time steps
     enable_corba_viewer: false  # Enable/disable Corba Viewer
-    enable_multiprocessing: true  # Enable/disable running the MPC in another process in parallel of the main loop
+    enable_multiprocessing: false  # Enable/disable running the MPC in another process in parallel of the main loop
     perfect_estimator: true  # Enable/disable perfect estimator by using data directly from PyBullet
 
     # General control parameters
@@ -26,7 +26,7 @@ robot:
     type_MPC: 3  # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
     save_guess: false # true to interpolate the impedance quantities between nodes of the MPC
     movement: "circle" # name of the movement to perform
-    interpolate_mpc: false # true to interpolate the impedance quantities between nodes of the MPC
+    interpolate_mpc: true # true to interpolate the impedance quantities between nodes of the MPC
     interpolation_type: 3 # 0,1,2,3 decide which kind of interpolation is used
     # Kp_main: [0.0, 0.0, 0.0]  # Proportional gains for the PD+
     # Kd_main: [0., 0., 0.]  # Derivative gains for the PD+

python/quadruped_reactive_walking/Controller.py

@@ -271,13 +271,6 @@ class Controller:
                     if self.params.interpolation_type == 3:
                         self.interpolator.update(xs[0], xs[1], xs[2])
                     # self.interpolator.plot(self.pd.mpc_wbc_ratio, self.pd.dt_wbc)
-
-            if self.params.interpolate_mpc:
-                if self.mpc_result.new_result:
-                    if self.params.interpolation_type == 3:
-                        self.interpolator.update(xs[0], xs[1], xs[2])
-                    # self.interpolator.plot(self.pd.mpc_wbc_ratio, self.pd.dt_wbc)
-
                 t = (self.k - self.k_solve + 1) * self.pd.dt_wbc
                 q, v = self.interpolator.interpolate(t)
             else:

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -177,7 +177,7 @@ class ProblemDataFull(problemDataAbstract):
         self.control_bound_w = 1e3
         self.control_reg_w = 1e0
         self.state_reg_w = np.array([1e1] * 3
-                                    + [1e-5] * 3
+                                    + [1e-1] * 3
                                     + [1e1] * 6
                                     + [1e1] * 3
                                     + [1e0] * 3