From d6843b5cfecd91db73511215676df7c377365dd6 Mon Sep 17 00:00:00 2001
From: Pierre-Alexandre Leziart <paleziart@laas.fr>
Date: Wed, 9 Sep 2020 17:38:28 +0200
Subject: [PATCH] Fix OSQP C++ segfault when not in multiprocessing
---
MPC_Wrapper.py | 3 ++-
main.py | 11 ++++++++---
2 files changed, 10 insertions(+), 4 deletions(-)
diff --git a/MPC_Wrapper.py b/MPC_Wrapper.py
index f98a5947..f8610cbb 100644
--- a/MPC_Wrapper.py
+++ b/MPC_Wrapper.py
@@ -121,8 +121,9 @@ class MPC_Wrapper:
if self.mpc_type:
# OSQP MPC
+ # Replace NaN values by 0.0
fstep_planner.fsteps_mpc[np.isnan(fstep_planner.fsteps_mpc)] = 0.0
- self.mpc.run(np.int(k/self.k_mpc)-1, fstep_planner.xref.copy(), fstep_planner.fsteps_mpc.copy())
+ self.mpc.run(np.int(k), fstep_planner.xref.copy(), fstep_planner.fsteps_mpc.copy())
else:
# Crocoddyl MPC
self.mpc.solve(k, fstep_planner)
diff --git a/main.py b/main.py
index 0616a175..10365c79 100644
--- a/main.py
+++ b/main.py
@@ -60,7 +60,7 @@ def run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION
# Wrapper that makes the link with the solver that you want to use for the MPC
# First argument to True to have PA's MPC, to False to have Thomas's MPC
- enable_multiprocessing = True
+ enable_multiprocessing = False
mpc_wrapper = MPC_Wrapper.MPC_Wrapper(type_MPC, dt_mpc, fstep_planner.n_steps,
k_mpc, fstep_planner.T_gait, enable_multiprocessing)
@@ -125,8 +125,13 @@ def run_scenario(envID, velID, dt_mpc, k_mpc, t, n_periods, T_gait, N_SIMULATION
proc.process_mpc(k, k_mpc, interface, joystick, fstep_planner, mpc_wrapper,
dt_mpc, ID_deb_lines)
- # Check if the MPC has outputted a new result
- f_applied = mpc_wrapper.get_latest_result()
+ # Retrieve reference contact forces
+ if enable_multiprocessing or (k == 0):
+ # Check if the MPC has outputted a new result
+ f_applied = mpc_wrapper.get_latest_result()
+ else:
+ if (k % k_mpc) == 2: # Mimic a 4 ms delay
+ f_applied = mpc_wrapper.get_latest_result()
t_mpc = time.time() # To analyze the time taken by each step
--
GitLab