diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 626168918fbe0525600cd2ac50150d933ab2bb75..211ab0502541a8cd8b6040fe22d620156286fdb3 100644
--- a/config/walk_parameters.yaml
+++ b/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]
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index ea0cbc38e72d52b916bdc4ab212a8ba3f9959341..e66c40f6827f81e04da42b089f88409aaa700611 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/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
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index ab1bed245c19c1c997a3c2ab11c11193235aefcb..990364665190dac3cee867f49ae56bcc4a5199cd 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/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
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index 28e0862ccb1cc3a01bc7997495e7340e6d40e827..ea8162b459383385263cac1e46700aa4adfb06d2 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/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
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index e73c0841d6cc9b08283f73ecbc4e6f41d3813612..a55fdeb7c3dd8263ed9a9f0adc6bf2d7192948fd 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/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
diff --git a/python/quadruped_reactive_walking/tools/Utils.py b/python/quadruped_reactive_walking/tools/Utils.py
index 35779603b36a8ae1e6de929111078ea8cf6f8488..7ef8304a4fd608dc441ae14ab14789a1301ab70d 100644
--- a/python/quadruped_reactive_walking/tools/Utils.py
+++ b/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):