timings quite good

config/walk_parameters.yaml

@@ -7,11 +7,11 @@ robot:
     PLOTTING: true  # Enable/disable automatic plotting at the end of the experiment
     DEMONSTRATION: false  # Enable/disable demonstration functionalities
     SIMULATION: true  # Enable/disable PyBullet simulation or running on real robot
-    enable_pyb_GUI: True  # Enable/disable PyBullet GUI
+    enable_pyb_GUI: false  # Enable/disable PyBullet GUI
     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: 1000  # Number of simulated wbc time steps
+    N_SIMULATION: 5000  # Number of simulated wbc time steps
     enable_corba_viewer: false  # Enable/disable Corba Viewer
     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

python/quadruped_reactive_walking/Controller.py

@@ -72,7 +72,7 @@ class Controller:
         device = DummyDevice()
         device.joints.positions = q_init
         try:
-            # file = np.load('/tmp/init_guess.npy', allow_pickle=True).item()
+            file = np.load('/tmp/init_guess.npy', allow_pickle=True).item()
             self.guess = {"xs": list(file["xs"]), "us": list(file["us"])}
             print("\nInitial guess loaded\n")
         except:
@@ -102,6 +102,9 @@ class Controller:
             #else:
             #   self.mpc.solve(self.k, m["x_m"], self.guess)
 
+            ### ONLY IF YOU WANT TO STORE THE FIRST SOLUTION TO WARMSTART THE INITIAL Problem ###
+            #np.save(open('/tmp/init_guess.npy', "wb"), {"xs": self.mpc.ocp.get_results().x, "us": self.mpc.ocp.get_results().u} )
+
         except ValueError:
             self.error = True
             print("MPC Problem")
@@ -138,7 +141,7 @@ class Controller:
         self.t_send = t_send - t_mpc
 
         self.clamp_result(device)
-        #self.security_check(m)
+        self.security_check(m)
 
         if self.error:
             self.set_null_control()
@@ -175,7 +178,7 @@ class Controller:
                 print(m["qj_m"])
                 print(np.abs(m["qj_m"]) > self.q_security)
                 self.error = True
-            elif (np.abs(m["vj_m"]) > 500 * np.pi / 180).any():
+            elif (np.abs(m["vj_m"]) > 1000 * np.pi / 180).any():
                 print("-- VELOCITY TOO HIGH ERROR --")
                 print(m["vj_m"])
                 print(np.abs(m["vj_m"]) > 500 * np.pi / 180)

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

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