without multiprocessing ok"

config/walk_parameters.yaml

@@ -7,7 +7,7 @@ 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)
@@ -25,7 +25,7 @@ robot:
     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: "" # name of the movement to perform
+    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+
@@ -37,8 +37,8 @@ robot:
 
     # Parameters of Gait
     N_periods: 1
-    gait: [100, 1, 0, 1, 1]
-          #  20, 1, 0, 1, 1]  # Initial gait matrix
+    gait: [10, 1, 1, 1, 1,
+           20, 1, 0, 1, 1]
 
     # Parameters of Joystick
     gp_alpha_vel: 0.003  # Coefficient of the low pass filter applied to gamepad velocity
@@ -60,7 +60,7 @@ robot:
     # Parameters of FootTrajectoryGenerator
     max_height: 0.05  # Apex height of the swinging trajectory [m]
     lock_time: 0.04  # Target lock before the touchdown [s]
-    vert_time: 0.03  # Duration during which feet move only along Z when taking off and landing
+    vert_time: 0.0  # 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]

python/quadruped_reactive_walking/Controller.py

@@ -255,6 +255,7 @@ class Controller:
             except ValueError:
                 self.error = True
                 print("MPC Problem")
+            self.gait = np.vstack((self.gait[1:, :] , self.gait[-1, :]))
 
         t_mpc = time.time()
         self.t_mpc = t_mpc - t_measures

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 = 10
+        self.max_iter = 3
 
         self.state = crocoddyl.StateMultibody(self.pd.model)
         self.initialized = False
@@ -30,7 +30,7 @@ class OCP:
 
     def initialize_models(self, gait, footsteps):
         models = []
-        for t, step in enumerate(gait):
+        for t, step in enumerate(gait[:-1]):
             tasks = self.make_task(footsteps[t])
             support_feet = [self.pd.feet_ids[i] for i in np.nonzero(step == 1)[0]]
             models.append(self.create_model(support_feet, tasks))
@@ -86,7 +86,6 @@ class OCP:
                 self.problem.runningModels[0],
                 self.problem.runningModels[0].createData(),
             )
-            name = self.pd.model.frames[self.pd.feet_ids[1]].name + "_foot_tracking"
 
         self.problem.x0 = self.x0
 
@@ -264,9 +263,8 @@ class OCP:
         for i in self.pd.feet_ids:
             name = self.pd.model.frames[i].name + "_foot_tracking"
             if i in tasks[0]:
-                if i not in support_feet:
-                    costs.changeCostStatus(name, True)
-                    costs.costs[name].cost.residual.reference = tasks[1][index]
+                costs.costs[name].cost.residual.reference = tasks[1][index]
                 index += 1
-            else:
-                costs.changeCostStatus(name, False)
\ No newline at end of file
+            costs.changeCostStatus(name, i not in support_feet)
+            
+            # print(f"{name} reference: {costs.costs[name].cost.residual.reference} status:{i in tasks[0]}")
\ No newline at end of file

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -63,7 +63,7 @@ class ProblemData(problemDataAbstract):
         # Cost function weights
         # Cost function weights
         self.mu = 0.7
-        self.foot_tracking_w = 1e2
+        self.foot_tracking_w = 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.03])
-            self.offset = np.array([0, 0, 0.1])
-            self.freq = np.array([0, 0.5 *0, 0.5*0])
-            self.phase = np.array([0, np.pi / 2, 0])
+            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])
         elif params.movement == "step":
             self.initial = self.position[:, 1].copy()
             self.target = self.position[:, 1].copy() + np.array([0.1, 0.0, 0.0])
@@ -31,7 +31,8 @@ class Target:
             self.update_time = -1
         else:
             self.target_footstep = self.position
-            self.target_footstep[2, 1] += 0.10
+            self.ramp_length = 100
+            self.target_ramp = np.linspace(0., 0.1, self.ramp_length)
 
     def compute(self, k):
         footstep = np.zeros((3, 4))
@@ -41,6 +42,7 @@ class Target:
             footstep[:, 1] = self.evaluate_step(1, k)
         else:
             footstep = self.target_footstep.copy()
+            footstep[2, 1] = self.target_ramp[k] if k < self.ramp_length else self.target_ramp[-1] 
 
         return footstep
 

python/quadruped_reactive_walking/tools/Utils.py

@@ -63,7 +63,7 @@ def init_robot(q_init, params):
     params.CoM_offset[1] = 0.0
 
     params.mpc_wbc_ratio = int(params.dt_mpc / params.dt_wbc)
-    params.T = params.gait.shape[0]
+    params.T = params.gait.shape[0] - 1
 
     #  Use initial feet pos as reference
     for i in range(4):