diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 0ec3b6671519f4b67e92ffdb6913181aab2b3a77..3e67324ee3711136faf748579873e70ae4c4ee84 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -13,7 +13,7 @@ robot:
predefined_vel: true # If we are using a predefined reference velocity (True) or a joystick (False)
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
+ enable_multiprocessing: true # 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
# General control parameters
@@ -25,9 +25,9 @@ robot:
dt_mpc: 0.001 # 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: [1, 1, 1] # Proportional gains for the PD+
+ Kp_main: [3, 3, 3] # Proportional gains for the PD+
# Kd_main: [0., 0., 0.] # Derivative gains for the PD+
- Kd_main: [1, 1, 1] # Derivative gains for the PD+
+ Kd_main: [0.3, 0.3, 0.3] # Derivative gains for the PD+
# Kff_main: 0.0 # Feedforward torques multiplier for the PD+
Kff_main: 1.0 # Feedforward torques multiplier for the PD+
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 47c0614edf1b1e90f76dc7d0e03e84f414fe5e4e..992d8a11c82b8b2341fd285c7fe1d5e00164f856 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -82,7 +82,7 @@ class Controller:
self.us_init = None
print("No initial guess\n")
- self.compute(device)
+ # self.compute(device)
def compute(self, device, qc=None):
"""
@@ -118,16 +118,17 @@ class Controller:
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")
- # self.result.P = np.array(self.params.Kp_main.tolist() * 4)
- self.result.P = np.array([5] * 3 + [1] * 3 + [5] * 6)
- # self.result.D = np.array(self.params.Kd_main.tolist() * 4)
- self.result.D = np.array([0.3] * 3 + [0.01] * 3 + [0.3] * 6)
- tauFF = self.mpc_result.u[0]
+ self.result.P = np.array(self.params.Kp_main.tolist() * 4)
+ # self.result.P = np.array([3] * 3 + [3] * 3 + [3] * 6)
+ self.result.D = np.array(self.params.Kd_main.tolist() * 4)
+ # self.result.D = np.array([0.3] * 3 + [0.01] * 3 + [0.3] * 6)
+ tauFF = self.mpc_result.us[0]
self.result.FF = self.params.Kff_main * np.array(
[0] * 3 + list(tauFF) + [0] * 6
)
@@ -135,15 +136,15 @@ class Controller:
# Keep only the actuated joints and set the other to default values
self.mpc_result.q = np.array([self.pd.q0] * (self.pd.T + 1))[:, 7:19]
self.mpc_result.v = np.array([self.pd.v0] * (self.pd.T + 1))[:, 6:]
- self.mpc_result.q[:, 3:6] = np.array(self.mpc_result.x)[:, : self.pd.nq]
- self.mpc_result.v[:, 3:6] = np.array(self.mpc_result.x)[:, self.pd.nq :]
+ self.mpc_result.q[:, 3:6] = np.array(self.mpc_result.xs)[:, : self.pd.nq]
+ self.mpc_result.v[:, 3:6] = np.array(self.mpc_result.xs)[:, self.pd.nq :]
self.result.q_des = self.mpc_result.q[1]
self.result.v_des = self.mpc_result.v[1]
self.result.tau_ff = np.zeros(12)
- self.xs_init = self.mpc_result.x[1:] + [self.mpc_result.x[-1]]
- self.us_init = self.mpc_result.u[1:] + [self.mpc_result.u[-1]]
+ 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]]
t_send = time.time()
self.t_send = t_send - t_mpc
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index 97409d1aa8b2e7649c6ff90e71c32935f3f2bec1..550e5503f68c29b9e40f1b55ea392b18593e3fec 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -126,7 +126,7 @@ class OCP:
return (
self.ddp.xs.tolist().copy(),
self.ddp.us.tolist().copy(),
- self.ddp.K.copy(),
+ self.ddp.K.tolist().copy(),
self.t_ddp,
)
diff --git a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
index a2ad642859be78423e5f60c1e8e1bbb9360ea011..6a2f9e6cafb7f3cd75c7dbc5c8fb9fc3bf90650c 100644
--- a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
+++ b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
@@ -13,9 +13,9 @@ import quadruped_reactive_walking as qrw
class Result:
def __init__(self, pd):
- self.xs = np.zeros((pd.T + 1, pd.nx))
- self.us = np.zeros((pd.T, pd.nu))
- self.K = None
+ self.xs = list(np.zeros((pd.T + 1, pd.nx)))
+ self.us = list(np.zeros((pd.T, pd.nu)))
+ self.K = list(np.zeros([pd.T, pd.nu, pd.nx]))
self.solving_duration = 0.0
@@ -39,11 +39,12 @@ class MPC_Wrapper:
self.running = Value("b", True)
self.in_k = Value("i", 0)
self.in_x0 = Array("d", [0] * pd.nx)
- self.in_xs = Array("d", [0] * pd.T + 1 * pd.nx)
- self.in_us = Array("d", [0] * pd.T * pd.nu)
- self.out_xs = Array("d", [0] * pd.T + 1 * pd.nx)
- self.out_us = Array("d", [0] * pd.T * pd.nu)
- self.out_k = Array("d", [0] * 0 * 0)
+ self.in_warm_start = Value("b", False)
+ self.in_xs = Array("d", [0] * ((pd.T + 1) * pd.nx))
+ self.in_us = Array("d", [0] * (pd.T * pd.nu))
+ self.out_xs = Array("d", [0] * ((pd.T + 1) * pd.nx))
+ self.out_us = Array("d", [0] * (pd.T * pd.nu))
+ self.out_k = Array("d", [0] * (pd.T * pd.nu * pd.nx))
self.out_solving_time = Value("d", 0.0)
else:
self.ocp = OCP(pd, target)
@@ -58,7 +59,6 @@ class MPC_Wrapper:
Args:
k (int): Number of inv dynamics iterations since the start of the simulation
"""
-
if self.multiprocessing:
self.run_MPC_asynchronous(k, x0, xs, us)
else:
@@ -101,6 +101,7 @@ class MPC_Wrapper:
Run the MPC (asynchronous version)
"""
if k == 0:
+ self.last_available_result.xs = [x0 for _ in range (self.pd.T + 1)]
p = Process(target=self.MPC_asynchronous)
p.start()
@@ -116,13 +117,13 @@ class MPC_Wrapper:
self.new_data.value = False
- k, x0, xs, us = self.decompress_dataIn(self.dataIn)
+ k, x0, xs, us = self.decompress_dataIn()
if k == 0:
loop_ocp = OCP(self.pd, self.target)
loop_ocp.solve(x0, xs, us)
- xs, us, K, solving_time = loop_ocp.get_latest_result()
+ xs, us, K, solving_time = loop_ocp.get_results()
self.compress_dataOut(xs, us, K, solving_time)
self.new_result.value = True
@@ -146,13 +147,20 @@ class MPC_Wrapper:
with self.in_k.get_lock():
self.in_k.value = k
with self.in_x0.get_lock():
- np.frombuffer(self.in_x0).reshape(self.pd.nx)[:] = x0
+ np.frombuffer(self.in_x0.get_obj()).reshape(self.pd.nx)[:] = x0
+
+ if xs is None or us is None:
+ self.in_warm_start.value = False
+ return
+
with self.in_xs.get_lock():
- np.frombuffer(self.in_xs).reshape((self.pd.T + 1, self.pd.nx))[:, :] = xs
+ np.frombuffer(self.in_xs.get_obj()).reshape((self.pd.T + 1, self.pd.nx))[
+ :, :
+ ] = np.array(xs)
with self.in_us.get_lock():
- np.frombuffer(self.in_us).reshape((self.pd.T, self.pd.nu))[:, :] = us
-
- return k, x0, xs, us
+ np.frombuffer(self.in_us.get_obj()).reshape((self.pd.T, self.pd.nu))[
+ :, :
+ ] = np.array(us)
def decompress_dataIn(self):
"""
@@ -160,13 +168,21 @@ class MPC_Wrapper:
retrieve data from the main control loop in the asynchronous MPC
"""
with self.in_k.get_lock():
- k = self.dataIn.k
+ k = self.in_k.value
with self.in_x0.get_lock():
- x0 = np.frombuffer(self.in_x0).reshape(self.pd.nx)
+ x0 = np.frombuffer(self.in_x0.get_obj()).reshape(self.pd.nx)
+
+ if not self.in_warm_start.value:
+ return k, x0, None, None
+
with self.in_xs.get_lock():
- xs = np.frombuffer(self.in_xs).reshape((self.pd.T + 1, self.pd.nx))
+ xs = list(
+ np.frombuffer(self.in_xs.get_obj()).reshape((self.pd.T + 1, self.pd.nx))
+ )
with self.in_us.get_lock():
- us = np.frombuffer(self.in_us).reshape((self.pd.T, self.pd.nu))
+ us = list(
+ np.frombuffer(self.in_us.get_obj()).reshape((self.pd.T, self.pd.nu))
+ )
return k, x0, xs, us
@@ -176,22 +192,35 @@ class MPC_Wrapper:
retrieve data in the main control loop from the asynchronous MPC
"""
with self.out_xs.get_lock():
- np.frombuffer(self.out_xs).reshape((self.pd.T + 1, self.pd.nx))[:, :] = xs
+ np.frombuffer(self.out_xs.get_obj()).reshape((self.pd.T + 1, self.pd.nx))[
+ :, :
+ ] = np.array(xs)
with self.out_us.get_lock():
- np.frombuffer(self.out_us).reshape((self.pd.T, self.pd.nu))[:, :] = us
+ np.frombuffer(self.out_us.get_obj()).reshape((self.pd.T, self.pd.nu))[
+ :, :
+ ] = np.array(us)
with self.out_k.get_lock():
- np.frombuffer(self.out_k).reshape((0, 0))[:, :] = K
+ np.frombuffer(self.out_k.get_obj()).reshape(
+ [self.pd.T, self.pd.nu, self.pd.nx]
+ )[:, :, :] = np.array(K)
self.out_solving_time.value = solving_time
+
def decompress_dataOut(self):
"""
Return the result of the asynchronous MPC (desired contact forces) that is
stored in the shared memory
"""
- xs = np.frombuffer(self.out_xs).reshape((self.pd.T + 1, self.pd.nx))
- us = np.frombuffer(self.out_us).reshape((self.pd.T, self.pd.nu))
- K = np.frombuffer(self.out_k).reshape((0, 0))
- solving_time = self.solving_time.value
+ xs = list(
+ np.frombuffer(self.out_xs.get_obj()).reshape((self.pd.T + 1, self.pd.nx))
+ )
+ us = list(np.frombuffer(self.out_us.get_obj()).reshape((self.pd.T, self.pd.nu)))
+ K = list(
+ np.frombuffer(self.out_k.get_obj()).reshape(
+ [self.pd.T, self.pd.nu, self.pd.nx]
+ )
+ )
+ solving_time = self.out_solving_time.value
return xs, us, K, solving_time
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index 654e65832b72a2adf0c4c5f46240433af6c58513..2aaddc49d55daf1de9b4c07e1f79bc738ad27604 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -209,12 +209,11 @@ def control_loop():
cnt += 1
# ****************************************************************
- finished = t >= t_max
damp_control(device, 12)
if params.enable_multiprocessing:
print("Stopping parallel process MPC")
- controller.mpc_wrapper.stop_parallel_loop()
+ controller.mpc.stop_parallel_loop()
# ****************************************************************
diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
index 79794913905440d570fb4f84b435fd985780fa4d..75bbc4e05fa6faf5b7547f49c4309ac326731045 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -102,7 +102,7 @@ class LoggerControl:
# Controller timings: MPC time, ...
self.target[self.i] = controller.point_target
- self.t_mpc[self.i] = controller.mpc.mpc_result.solver_time
+ self.t_mpc[self.i] = controller.t_mpc
self.t_send[self.i] = controller.t_send
self.t_loop[self.i] = controller.t_loop
self.t_measures[self.i] = controller.t_measures
@@ -115,11 +115,11 @@ class LoggerControl:
self.t_mpc[self.i] = controller.t_mpc
self.t_send[self.i] = controller.t_send
self.t_loop[self.i] = controller.t_loop
-
+
+ self.t_ocp_ddp[self.i] = controller.mpc_result.solving_duration
if not self.multiprocess_mpc:
self.t_ocp_update[self.i] = controller.mpc.ocp.t_update
self.t_ocp_warm_start[self.i] = controller.mpc.ocp.t_warm_start
- self.t_ocp_ddp[self.i] = controller.mpc.ocp.t_ddp
self.t_ocp_solve[self.i] = controller.mpc.ocp.t_solve
self.t_ocp_update_FK[self.i] = controller.mpc.ocp.t_FK
@@ -249,9 +249,10 @@ class LoggerControl:
plt.plot(t_range, self.t_mpc, "g+")
plt.plot(t_range, self.t_send, "b+")
plt.plot(t_range, self.t_loop, "+", color="violet")
+ plt.plot(t_range, self.t_ocp_ddp, "+", color="royalblue")
plt.axhline(y=0.001, color="grey", linestyle=":", lw=1.0)
plt.axhline(y=0.01, color="grey", linestyle=":", lw=1.0)
- lgd = ["Measures", "MPC", "Send", "Whole-loop"]
+ lgd = ["Measures", "MPC", "Send", "Whole-loop", "MPC solve"]
plt.legend(lgd)
plt.xlabel("Time [s]")
plt.ylabel("Time [s]")