Tuning yaml parameters to debug the control

src/config_solo12.yaml

@@ -9,14 +9,16 @@ robot:
     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)
     velID: 10  # Identifier of the reference velocity profile to choose which one will be sent to the robot
-    N_SIMULATION: 30000  # Number of simulated wbc time steps
-    enable_pyb_GUI: true # Enable/disable PyBullet GUI
+    N_SIMULATION: 9000  # Number of simulated wbc time steps
+    enable_pyb_GUI: false # Enable/disable PyBullet GUI
     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: false  # Enable/disable perfect estimator by using data directly from PyBullet
     
     # General control parameters
-    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
+    # [0.0, 0.865, -1.583, 0.0, 0.865, -1.583, 0.0, 0.865, -1.583, 0.0, 0.865, -1.583] # h_com = 0.2
+    # [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.02  # Time step of the model predictive control
     type_MPC: 0  # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
@@ -41,21 +43,22 @@ robot:
     vert_time: 0.03  # Duration during which feet move only along Z when taking off and landing
 
     # Parameters of MPC with OSQP
+    # [0.0, 0.0, 20.0, 0.25, 0.25, 10.0, 0.05, 0.05, 0.2, 0.0, 0.0, 0.3]
     osqp_w_states: [2.0, 2.0, 20.0, 0.25, 0.25, 10.0, 0.2, 0.2, 0.2, 0.0, 0.0, 0.3]  # Weights for state tracking error
     osqp_w_forces: [0.00005, 0.00005, 0.00005]  # Weights for force regularisation
     osqp_Nz_lim: 35.0  # Maximum vertical force that can be applied at contact points
 
     # Parameters of InvKin
-    Kp_flyingfeet: 1000.0  # Proportional gain for feet position tasks
-    Kd_flyingfeet: 100.0  # Derivative gain for feet position tasks
-    Kp_base_position: [1000.0, 1000.0, 1000.0]  # Proportional gains for the base position task
-    Kd_base_position: [100.0, 100.0, 100.0]  # Derivative gains for the base position task
-    Kp_base_orientation: [1000.0, 1000.0, 1000.0]  # Proportional gains for the base orientation task
-    Kd_base_orientation: [100.0, 100.0, 100.0]  # Derivative gains for the base orientation task
+    Kp_flyingfeet: 10.0  # Proportional gain for feet position tasks
+    Kd_flyingfeet: 1.0  # Derivative gain for feet position tasks
+    Kp_base_position: [0.0, 0.0, 0.0]  # Proportional gains for the base position task
+    Kd_base_position: [100.0, 100.0, 10.0]  # Derivative gains for the base position task
+    Kp_base_orientation: [100.0, 100.0, 100.0]  # Proportional gains for the base orientation task
+    Kd_base_orientation: [10.0, 10.0, 10.0]  # Derivative gains for the base orientation task
     w_tasks: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]  # Tasks weights: [feet/base, vx, vy, vz, roll+wroll, pitch+wpitch, wyaw]
 
     # Parameters of WBC QP problem
     Q1: 0.1  # Weights for the "delta articular accelerations" optimization variables
-    Q2: 5.0  # Weights for the "delta contact forces" optimization variables
+    Q2: 10.0  # Weights for the "delta contact forces" optimization variables
     Fz_max: 35.0  # Maximum vertical contact force [N]
     Fz_min: 0.0  # Minimal vertical contact force [N]
\ No newline at end of file