diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index c96eb023a1132d4c488a6cbcb89e70276bb96819..51368c8546d2aa4b6b3c01cfb925507289f11b15 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,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
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
+ interpolation_type: 0 # 0,1,2,3 decide which kind of interpolation is used
# Kp_main: [0.0, 0.0, 0.0] # Proportional gains for the PD+
Kp_main: [1, 1, 1] # 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 a34c03de9429f9f16fb09ce024cf8e1b08f8e738..8b1e93b96f206d8cddc8749ab6f6cd7380f5a176 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -197,7 +197,13 @@ class Controller:
if not self.error:
self.mpc_result = self.mpc.get_latest_result()
+ if self.params.enable_multiprocessing:
+ if self.mpc_result.new_result:
+ print("new result! at iter: ", str(self.cnt_wbc))
+ print(self.mpc_result.xs[1], "\n")
+ self.cnt_wbc = 0
+
# ## 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} )
@@ -219,7 +225,7 @@ class Controller:
q, v = self.interpolator.interpolate(
(self.cnt_wbc + 1) * self.pd.dt_wbc)
- self.interpolator.plot_interpolation(self.pd.r1, self.pd.dt_wbc)
+ #self.interpolator.plot_interpolation(self.pd.r1, self.pd.dt_wbc)
else:
q, v = self.integrate_x(m)
@@ -232,8 +238,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()
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index 9767505bb48172abcb880bc8d35a8309737c9684..155afa63223fc0279232267b5361d07a1acbea20 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -12,7 +12,7 @@ class OCP:
def __init__(self, pd: ProblemData, target: Target):
self.pd = pd
self.target = target
- self.max_iter=1
+ self.max_iter=1000
self.state = crocoddyl.StateMultibody(self.pd.model)
self.initialized = False
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index d06cc0178201f9f6bb7814a67316be2f8db9ab4b..1a9a3ba87165c4305ac80f4bdc6359124695419b 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -23,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/WB_MPC_Wrapper.py b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
index 6a2f9e6cafb7f3cd75c7dbc5c8fb9fc3bf90650c..820e39b78c2ccf670861555a6f3fb2faaaf7dc06 100644
--- a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
+++ b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
@@ -17,6 +17,8 @@ class Result:
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
+ self.new_result = False
+
class MPC_Wrapper:
@@ -80,6 +82,7 @@ class MPC_Wrapper:
self.last_available_result.K,
self.last_available_result.solving_duration,
) = self.decompress_dataOut()
+ self.last_available_result.new_result = True
else:
self.initialized = True
return self.last_available_result
@@ -93,7 +96,7 @@ class MPC_Wrapper:
self.last_available_result.xs,
self.last_available_result.us,
self.last_available_result.K,
- self.last_available_result.solving_duration,
+ self.last_available_result.solving_duration
) = self.ocp.get_results()
def run_MPC_asynchronous(self, k, x0, xs, us):
@@ -104,7 +107,7 @@ class MPC_Wrapper:
self.last_available_result.xs = [x0 for _ in range (self.pd.T + 1)]
p = Process(target=self.MPC_asynchronous)
p.start()
-
+ self.last_available_result.new_result = False
self.add_new_data(k, x0, xs, us)
def MPC_asynchronous(self):
@@ -125,7 +128,6 @@ class MPC_Wrapper:
loop_ocp.solve(x0, xs, us)
xs, us, K, solving_time = loop_ocp.get_results()
self.compress_dataOut(xs, us, K, solving_time)
-
self.new_result.value = True
def add_new_data(self, k, x0, xs, us):