small stuff

config/walk_parameters.yaml

@@ -11,7 +11,7 @@ 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
     perfect_estimator: true  # Enable/disable perfect estimator by using data directly from PyBullet
@@ -22,23 +22,20 @@ 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.015  # Time step of the model predictive control
+    dt_mpc: 0.01  # 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
     save_guess: false # true to interpolate the impedance quantities between nodes of the MPC
     movement: "circle" # name of the movement to perform
     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+
-#     Kff_main: 0.0  # Feedforward torques multiplier for the PD+
     Kp_main: [3, 3, 3]  # Proportional gains for the PD+
     Kd_main: [0.3, 0.3, 0.3]  # Derivative gains for the PD+
     Kff_main: 1.0  # Feedforward torques multiplier for the PD+
 
     # Parameters of Gait
     N_periods: 1
-    gait: [2, 1, 1, 1, 1,
-           30, 1, 0, 1, 1]  # Initial gait matrix
+    gait: [5, 1, 1, 1, 1,
+           45, 1, 0, 1, 1]
 
     # Parameters of Joystick
     gp_alpha_vel: 0.003  # Coefficient of the low pass filter applied to gamepad velocity

python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py

@@ -13,7 +13,7 @@ class OCP:
     def __init__(self, pd: ProblemData, params, footsteps, gait):
         self.pd = pd
         self.params = params
-        self.max_iter = 1
+        self.max_iter = 500 if params.save_guess else 1
 
         self.state = crocoddyl.StateMultibody(self.pd.model)
         self.initialized = False

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -62,7 +62,7 @@ class ProblemData(problemDataAbstract):
 
         # Cost function weights
         self.mu = 0.7
-        self.foot_tracking_w = 1e1
+        self.foot_tracking_w = 2. * 1e1
         self.friction_cone_w = 1e4
         self.control_bound_w = 1e3
         self.control_reg_w = 1e0

python/quadruped_reactive_walking/WB_MPC/Target.py

@@ -15,10 +15,10 @@ class Target:
         )
 
         if params.movement == "circle":
-            self.A = np.array([0, 0.03, 0.04])
-            self.offset = np.array([0, 0, 0.05])
-            self.freq = np.array([0, 0.5, 0.5])
-            self.phase = np.array([0, 0, -np.pi/2])
+            self.A = np.array([0.05, 0., 0.04])
+            self.offset = np.array([0.05, 0, 0.05])
+            self.freq = np.array([0.5, 0., 0.5])
+            self.phase = np.array([-np.pi/2-0.5, 0., -np.pi/2])
         elif params.movement == "step":
             self.initial = self.position[:, 1].copy()
             self.target = self.position[:, 1].copy() + np.array([0.1, 0.0, 0.0])

python/quadruped_reactive_walking/tools/LoggerControl.py

@@ -230,15 +230,13 @@ class LoggerControl:
         }
 
         # Target plot
+        _, axs = plt.subplots(3, sharex=True)
         legend = ["x", "y", "z"]
-
-        fig, axs = plt.subplots(3, sharex=True)
         for p in range(3):
-            plt.subplot(3, 1, p + 1)
-            plt.title("Free foot on " + legend[p])
-            plt.plot(self.target[:, p])
-            plt.plot(m_feet_p_log[self.pd.feet_ids[1]][:, p])
-            plt.legend(["Target", "Measured"])
+            axs[p].set_title("Free foot on " + legend[p])
+            axs[p].plot(self.target[:, p])
+            axs[p].plot(m_feet_p_log[self.pd.feet_ids[1]][:, p])
+            axs[p].legend(["Target", "Measured"])
             # "Predicted"])
         if save:
             plt.savefig(fileName + "/target")