diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 5e51ab10652978c9bc3e148f2625f7b705f9dd7d..c26f46823d39af6107648b447fbdaa1e7a8e9d9c 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: false # Enable/disable PyBullet GUI
+ enable_pyb_GUI: true # 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.015 # 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
- interpolate_mpc: true # true to interpolate the impedance quantities between nodes of the MPC
+ interpolate_mpc: false # 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+
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 7986e4532957a88213e06329a829b29f3207abe4..04df1d8cf7292d15bd4a6c051293ff4aa5795349 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -37,10 +37,10 @@ class Interpolator:
self.update(x0, x0)
def update(self, x0, x1, x2=None):
- self.q0 = x0[:3]
- self.q1 = x1[:3]
- self.v0 = x0[3:]
- self.v1 = x1[3:]
+ self.q0 = x0[:12]
+ self.q1 = x1[:12]
+ self.v0 = x0[12:]
+ self.v1 = x1[12:]
if self.type == 0: # Linear
self.alpha = 0.0
self.beta = self.v1
@@ -69,8 +69,8 @@ class Interpolator:
from scipy.interpolate import KroghInterpolator
if x2 is not None:
- self.q2 = x2[:3]
- self.v2 = x2[3:]
+ self.q2 = x2[:12]
+ self.v2 = x2[12:]
self.y = [self.q0, self.v0, self.q1, self.v1, self.q2, self.v2]
self.krog = KroghInterpolator(self.ts, np.array(self.y))
else:
@@ -171,7 +171,7 @@ class Controller:
self.k_solve = 0
if self.params.interpolate_mpc:
self.interpolator = Interpolator(
- params, np.concatenate([self.result.q_des[3:6], self.result.v_des[3:6]])
+ params, np.concatenate([self.result.q_des, self.result.v_des])
)
try:
file = np.load("/tmp/init_guess.npy", allow_pickle=True).item()
@@ -209,25 +209,32 @@ class Controller:
try:
footstep = self.target.footstep(self.pd.T)
- # self.mpc.solve(self.k, m["x_m"], self.xs_init, self.us_init)
- if self.initialized:
- self.mpc.solve(
- self.k,
- self.mpc_result.xs[1],
- footstep,
- self.gait,
- self.xs_init,
- self.us_init,
- )
- else:
- self.mpc.solve(
- self.k,
- m["x_m"],
- footstep,
- self.gait,
- self.xs_init,
- self.us_init,
- )
+ self.mpc.solve(self.k,
+ m["x_m"],
+ footstep,
+ self.gait,
+ self.xs_init,
+ self.us_init
+ )
+ # OPEN LOOP MPC
+ # if self.initialized:
+ # self.mpc.solve(
+ # self.k,
+ # self.mpc_result.xs[1],
+ # footstep,
+ # self.gait,
+ # self.xs_init,
+ # self.us_init,
+ # )
+ # else:
+ # self.mpc.solve(
+ # self.k,
+ # m["x_m"],
+ # footstep,
+ # self.gait,
+ # self.xs_init,
+ # self.us_init,
+ # )
except ValueError:
self.error = True
print("MPC Problem")
@@ -250,16 +257,17 @@ class Controller:
self.result.FF = self.params.Kff_main * np.ones(12)
self.result.tau_ff = self.compute_torque(m)[:]
- # if self.params.interpolate_mpc:
- # if self.mpc_result.new_result:
- # if self.params.interpolation_type == 3:
- # self.interpolator.update(xs[0], xs[1], xs[2])
- # # self.interpolator.plot(self.pd.mpc_wbc_ratio, self.pd.dt_wbc)
+ if self.params.interpolate_mpc:
+ if self.mpc_result.new_result:
+ if self.params.interpolation_type == 3:
+ self.interpolator.update(xs[0], xs[1], xs[2])
+ # self.interpolator.plot(self.pd.mpc_wbc_ratio, self.pd.dt_wbc)
+
+ t = (self.k - self.k_solve + 1) * self.pd.dt_wbc
+ q, v = self.interpolator.interpolate(t)
+ else:
+ q, v = self.integrate_x(m)
- # t = (self.k - self.k_solve + 1) * self.pd.dt_wbc
- # q, v = self.interpolator.interpolate(t)
- # else:
- # q, v = self.integrate_x(m)
q = xs[1][: self.pd.nq]
v = xs[1][self.pd.nq :]
@@ -282,8 +290,8 @@ class Controller:
t_send = time.time()
self.t_send = t_send - t_mpc
- # self.clamp_result(device)
- # self.security_check(m)
+ self.clamp_result(device)
+ self.security_check(m)
if self.error:
self.set_null_control()
@@ -425,8 +433,7 @@ class Controller:
]
)
# x_diff = self.mpc_result.xs[0] - m["x_m"]
- tau = self.mpc_result.us[0] + np.dot(self.mpc_result.K[0], x_diff)
- # tau = self.mpc_result.us[0]
+ tau = self.mpc_result.us[0] - np.dot(self.mpc_result.K[0], x_diff)
return tau
def integrate_x(self, m):
@@ -434,9 +441,9 @@ class Controller:
Integrate the position and velocity using the acceleration computed from the
feedforward torque
"""
- q0 = m["qj_m"][3:6].copy()
- v0 = m["vj_m"][3:6].copy()
- tau = self.result.tau_ff[3:6].copy()
+ q0 = m["qj_m"].copy()
+ v0 = m["vj_m"].copy()
+ tau = self.result.tau_ff.copy()
a = pin.aba(self.pd.model, self.pd.rdata, q0, v0, tau)
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index 60c2979bd9bb4d672f0d71c52ef398315ecbb875..5958260e62bed8d14d03bdd4cd7cc8b45c3a348f 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -176,9 +176,9 @@ class ProblemDataFull(problemDataAbstract):
# self.friction_cone_w = 1e3 * 0
self.control_bound_w = 1e3
self.control_reg_w = 1e0
- self.state_reg_w = np.array([1e0] * 3
+ self.state_reg_w = np.array([1e1] * 3
+ [1e-5] * 3
- + [1e0] * 6
+ + [1e1] * 6
+ [1e1] * 3
+ [1e0] * 3
+ [1e1] * 6