From 5d3d5d51e7f7bb62d6b3cccd93726409443876c2 Mon Sep 17 00:00:00 2001
From: Fanny Risbourg <frisbourg@laas.fr>
Date: Wed, 3 Aug 2022 17:56:56 +0200
Subject: [PATCH] try multiprocessing
---
config/walk_parameters.yaml | 2 +-
.../quadruped_reactive_walking/Controller.py | 20 +++++++------------
.../WB_MPC_Wrapper.py | 1 -
3 files changed, 8 insertions(+), 15 deletions(-)
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 8e74c7e6..3e8c9623 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
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 39cc2be2..d7ad22a5 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -24,22 +24,14 @@ class Result:
class Interpolator:
- def __init__(self, params):
+ def __init__(self, params, x0):
self.dt = params.dt_mpc
self.type = params.interpolation_type
- self.q0 = np.zeros(3)
- self.q1 = np.zeros(3)
- self.v0 = np.zeros(3)
- self.v1 = np.zeros(3)
-
if self.type == 3:
self.ts = np.repeat(np.linspace(0, 2 * self.dt, 3), 2)
- else:
- self.alpha = np.zeros(3)
- self.beta = np.zeros(3)
- self.gamma = np.zeros(3)
- self.delta = 1.0
+
+ self.update(x0, x0)
def update(self, x0, x1, x2=None):
self.q0 = x0[:3]
@@ -70,7 +62,7 @@ class Interpolator:
self.beta[i] = v0
self.gamma[i] = q0
self.delta = 2 * (q1 - q0) / (v1 + v0) / self.dt
- elif self.type == 3: # Spline interpolation
+ elif self.type == 3: # Spline interpolation
from scipy.interpolate import KroghInterpolator
if x2 is not None:
@@ -173,7 +165,9 @@ class Controller:
self.k_result = 0
self.k_solve = 0
if self.params.interpolate_mpc:
- self.interpolator = Interpolator(params)
+ self.interpolator = Interpolator(
+ params, np.concatenate([self.result.q_des[3:6], self.result.v_des[3:6]])
+ )
try:
file = np.load("/tmp/init_guess.npy", allow_pickle=True).item()
self.xs_init = list(file["xs"])
diff --git a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
index 3fd35a7f..b63932f4 100644
--- a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
+++ b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
@@ -104,7 +104,6 @@ class MPC_Wrapper:
"""
Run the MPC (asynchronous version)
"""
- print("Call to solve")
if k == 0:
self.last_available_result.xs = [x0 for _ in range(self.pd.T + 1)]
p = Process(target=self.MPC_asynchronous)
--
GitLab