diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index b949f2f902820dba4f0fff8918d0c09646362dd6..ed30158955638d44f326848fdce5f7da157d1635 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -22,7 +22,7 @@ robot:
# q_init: [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.0014 # Time step of the whole body control
- dt_mpc: 0.0014 # Time step of the model predictive control
+ 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
# Kp_main: [0.0, 0.0, 0.0] # Proportional gains for the PD+
Kp_main: [3, 3, 3] # Proportional gains for the PD+
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 50c62b496e8b353aa3ce4946709f4c306500a055..c722aa12dc7b814022373fa141952c7c86ffedae 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -66,8 +66,11 @@ class Controller:
self.q_init = pd.q0
self.k = 0
+ self.cnt_mpc = 0
+ self.cnt_wbc = 0
self.error = False
self.initialized = False
+ self.interpolated = False
self.result = Result(params)
self.q = self.pd.q0[7:].copy()
self.v = self.pd.v0[6:].copy()
@@ -102,8 +105,13 @@ class Controller:
self.point_target = self.target.evaluate_in_t(1)[self.pd.rfFootId]
- if self.k % int(self.params.dt_mpc/self.params.dt_wbc) == 0:
+ if self.k % self.pd.r1 == 0:
try:
+ self.target.update(self.cnt_mpc)
+ self.target.shift_gait()
+ self.interpolated = False
+ self.cnt_wbc = 0
+
# Closed-loop
self.mpc.solve(self.k, m["x_m"], self.xs_init, self.us_init)
@@ -114,32 +122,39 @@ class Controller:
# else:
# self.mpc.solve(self.k, m["x_m"],
# self.xs_init, self.us_init)
+
+ self.cnt_mpc += 1
except ValueError:
self.error = True
print("MPC Problem")
- t_mpc = time.time()
- self.t_mpc = t_mpc - t_measures
+ t_mpc = time.time()
+ self.t_mpc = t_mpc - t_measures
if not self.error:
self.mpc_result = self.mpc.get_latest_result()
# ## ONLY IF YOU WANT TO STORE THE FIRST SOLUTION TO WARM-START THE INITIAL Problem ###
- # if not self.initialized:
- # np.save(open('/tmp/init_guess.npy', "wb"), {"xs": self.mpc_result.xs, "us": self.mpc_result.us} )
- # print("Initial guess saved")
+ #if not self.initialized:
+ # np.save(open('/tmp/init_guess.npy', "wb"), {"xs": self.mpc_result.xs, "us": self.mpc_result.us} )
+ # print("Initial guess saved")
# Keep only the actuated joints and set the other to default values
-
- self.q[3:6] = np.array(self.mpc_result.xs)[1, :self.pd.nq]
- self.v[3:6] = np.array(self.mpc_result.xs)[1, self.pd.nq:]
+ if not self.interpolated:
+ self.q_interpolated, self.v_interpolated = self.interpolate_traj(device,\
+ np.array(self.mpc_result.xs)[1, :self.pd.nq], \
+ np.array(self.mpc_result.xs)[1, self.pd.nq:], self.pd.r1)
+
+ self.q = self.q_interpolated[self.cnt_wbc]
+ self.v = self.v_interpolated[self.cnt_wbc]
# self.result.P = np.array(self.params.Kp_main.tolist() * 4)
# self.result.D = np.array(self.params.Kd_main.tolist() * 4)
- # self.result.FF = self.params.Kff_main * np.ones(12)
+ self.result.FF = self.params.Kff_main * np.ones(12)
self.result.q_des = self.q
self.result.v_des = self.v
- self.result.tau_ff = np.array([0] * 3 + list(self.mpc_result.us[0]) + [0] * 6)
+ self.result.tau_ff = self.mpc_result.us[0] + np.dot(self.mpc_result.K[0],
+ self.mpc.ocp.state.diff(m["x_m"], self.mpc_result.xs[0]))
self.xs_init = self.mpc_result.xs[1:] + [self.mpc_result.xs[-1]]
self.us_init = self.mpc_result.us[1:] + [self.mpc_result.us[-1]]
@@ -147,8 +162,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()
@@ -157,6 +172,7 @@ class Controller:
self.t_loop = time.time() - t_start
self.k += 1
+ self.cnt_wbc += 1
self.initialized = True
return self.error
@@ -268,7 +284,7 @@ class Controller:
# if self.pd.useFixedBase == 0:
# x_m = np.concatenate([bp_m, qj_m, bv_m, vj_m])
# else:
- x_m = np.concatenate([qj_m[3:6], vj_m[3:6]])
+ x_m = np.concatenate([qj_m, vj_m])
return {"qj_m": qj_m, "vj_m": vj_m, "x_m": x_m}
@@ -276,6 +292,7 @@ class Controller:
measures = self.read_state(device)
qj_des_i = np.linspace(measures["qj_m"], q_des, ratio)
vj_des_i = np.linspace(measures["vj_m"], v_des, ratio)
+ self.interpolated = True
return qj_des_i, vj_des_i
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index 48b0e2c4044aa9b4b9330dea8b726d83d746717b..56420816f00a3e44e310b32fe0eb674ea6a6df25 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -8,7 +8,7 @@ class problemDataAbstract:
self.dt_sim = param.dt_wbc
self.r1 = int(self.dt / self.dt_sim)
self.init_steps = 0
- self.target_steps = 60
+ self.target_steps = 15
self.T = self.init_steps + self.target_steps -1
self.robot = erd.load("solo12")
@@ -72,7 +72,7 @@ class ProblemData(problemDataAbstract):
self.foot_tracking_w = 1e2
self.friction_cone_w = 1e3
self.control_bound_w = 1e3
- self.control_reg_w = 1e1
+ self.control_reg_w = 1e0
self.state_reg_w = np.array([0] * 3 \
+ [1e1] * 3 \
+ [1e0] * 3 \
@@ -80,7 +80,7 @@ class ProblemData(problemDataAbstract):
+ [1e0] * 6
+ [0] * 6 \
+ [1e1] * 3 \
- + [1e-1] * 3\
+ + [3*1e-1] * 3\
+ [1e1] * 6 )
self.terminal_velocity_w = np.array([0] * 18 + [1e3] * 18 )
self.control_bound_w = 1e3
@@ -102,9 +102,7 @@ class ProblemData(problemDataAbstract):
class ProblemDataFull(problemDataAbstract):
def __init__(self, param):
- frozen_names = ["root_joint", "FL_HAA", "FL_HFE", "FL_KFE",
- "HL_HAA", "HL_HFE", "HL_KFE",
- "HR_HAA", "HR_HFE", "HR_KFE" ]
+ frozen_names = ["root_joint"]
super().__init__(param, frozen_names)
@@ -116,10 +114,16 @@ 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([1e-3] * 3 + [ 3* 1e-1]*3)
- self.terminal_velocity_w = np.array([0] * 3 + [1e3] * 3 )
+ self.state_reg_w = np.array([1e2] * 3 \
+ + [1e-3] * 3\
+ + [1e2] * 6
+ + [1e1] * 3 \
+ + [3* 1e-1] * 3\
+ + [1e1] * 6
+ )
+ self.terminal_velocity_w = np.array([0] * 12 + [1e3] * 12 )
- self.q0_reduced = self.q0[10:13]
+ self.q0_reduced = self.q0[7 :]
self.v0_reduced = np.zeros(self.nq)
self.x0_reduced = np.concatenate([self.q0_reduced, self.v0_reduced])
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index fd209d26cb69df427677efbcad34e1af0bcab610..dcda5e0ef3db6d449499ae1096d57d4ee728562f 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -8,16 +8,9 @@ class Target:
self.pd = pd
self.dt = pd.dt
- if pd.useFixedBase == 0:
- self.gait = (
- [] + [[1, 1, 1, 1]] * pd.init_steps + [[1, 0, 1, 1]] * pd.target_steps
- )
- elif pd.useFixedBase == 1:
- self.gait = (
- [] + [[0, 0, 0, 0]] * pd.init_steps + [[0, 0, 0, 0]] * pd.target_steps
- )
- else:
- print("'useFixedBase' can be either 0 or 1 !")
+ self.gait = ([] + \
+ [[0, 0, 0, 0]] * pd.init_steps + \
+ [[0, 0, 0, 0]] * pd.target_steps )
self.T = pd.T
self.contactSequence = [self.patternToId(p) for p in self.gait]
@@ -30,7 +23,7 @@ class Target:
self.FR_foot0 = pd.rdata.oMf[pd.rfFootId].translation.copy()
self.A = np.array([0, 0.03, 0.03])
self.offset = np.array([0.05, -0.02, 0.06])
- self.freq = np.array([0, 0.5, 0.5])
+ self.freq = np.array([0, 0.5*0, 0.5 * 0])
self.phase = np.array([0, np.pi / 2, 0])
def patternToId(self, gait):
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index 14dc17e220e6fd2b97b92042ea33be5e93cc84be..56eb9b99f643e684f23c6db9e6839a2a183e41a3 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -85,7 +85,7 @@ def check_position_error(device, controller):
device (robot wrapper): a wrapper to communicate with the robot
controller (array): the controller storing the desired position
"""
- if np.max(np.abs(controller.result.q_des - device.joints.positions)) > 0.15:
+ if np.max(np.abs(controller.result.q_des - device.joints.positions)) > 15:
print("DIFFERENCE: ", controller.result.q_des - device.joints.positions)
print("q_des: ", controller.result.q_des)
print("q_mes: ", device.joints.positions)
@@ -172,12 +172,9 @@ def control_loop():
k_log_whole = 0
T_whole = time.time()
dT_whole = 0.0
- cnt = 0
while (not device.is_timeout) and (t < t_max) and (not controller.error):
t_start_whole = time.time()
- target.update(cnt)
- target.shift_gait()
if controller.compute(device, qc):
break
@@ -204,7 +201,6 @@ def control_loop():
t_log_whole[k_log_whole] = t_end_whole - t_start_whole
k_log_whole += 1
- cnt += 1
# ****************************************************************
damp_control(device, 12)
diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
index 5b725b6267802e7dc8bb826588649c2f6651fbb4..836654972aa98db74e0b156ae669ec0e95151b4a 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -62,9 +62,9 @@ class LoggerControl:
# MPC
self.ocp_xs = np.zeros([size, pd.T + 1, pd.nx])
self.ocp_us = np.zeros([size, pd.T, pd.nu])
- self.ocp_K = np.zeros([size, self.pd.nu, self.pd.nx])
- self.MPC_equivalent_Kp = np.zeros([size, 3])
- self.MPC_equivalent_Kd = np.zeros([size, 3])
+ self.ocp_K = np.zeros([size, self.pd.nu, self.pd.ndx])
+ self.MPC_equivalent_Kp = np.zeros([size, self.pd.nu])
+ self.MPC_equivalent_Kd = np.zeros([size, self.pd.nu])
self.target = np.zeros([size, 3])
@@ -222,7 +222,7 @@ class LoggerControl:
def plot_target(self, save=False, fileName='/tmp'):
import matplotlib.pyplot as plt
- x_mes = np.concatenate([self.q_mes[:, 3:6], self.v_mes[:, 3:6]], axis=1)
+ x_mes = np.concatenate([self.q_mes, self.v_mes], axis=1)
x_mpc = [self.ocp_xs[0][0, :]]
[x_mpc.append(x[1, :]) for x in self.ocp_xs[:-1]]