From ee9ffa69a96bcd6c742c6b009dc6dce05c7b6deb Mon Sep 17 00:00:00 2001
From: Fanny Risbourg <frisbourg@laas.fr>
Date: Fri, 5 Aug 2022 15:02:21 +0200
Subject: [PATCH] investigate tracking performance loss
---
config/walk_parameters.yaml | 12 ++++-----
.../quadruped_reactive_walking/Controller.py | 27 ++++++++++---------
.../tools/LoggerControl.py | 1 -
3 files changed, 21 insertions(+), 19 deletions(-)
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 69ba9cbd..9fd8366f 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: 10000 # Number of simulated wbc time steps
enable_corba_viewer: false # Enable/disable Corba Viewer
- enable_multiprocessing: true # Enable/disable running the MPC in another process in parallel of the main loop
+ enable_multiprocessing: false # 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
@@ -22,16 +22,16 @@ 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.001 # Time step of the whole body control
- dt_mpc: 0.01 # Time step of the model predictive control
+ 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
save_guess: false # true to interpolate the impedance quantities between nodes of the MPC
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
- Kp_main: [0.0, 0.0, 0.0] # Proportional gains for the PD+
- Kd_main: [0., 0., 0.] # Derivative gains for the PD+
+ # Kp_main: [0.0, 0.0, 0.0] # Proportional gains for the PD+
+ # Kd_main: [0., 0., 0.] # Derivative gains for the PD+
# Kff_main: 0.0 # Feedforward torques multiplier for the PD+
- # Kp_main: [3, 3, 3] # Proportional gains for the PD+
- # Kd_main: [0.3, 0.3, 0.3] # Derivative gains for the PD+
+ Kp_main: [3, 3, 3] # Proportional gains for the PD+
+ Kd_main: [0.3, 0.3, 0.3] # Derivative gains for the PD+
Kff_main: 1.0 # Feedforward torques multiplier for the PD+
# Parameters of Gait
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 7e95e253..83c259b8 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -250,16 +250,18 @@ class Controller:
self.result.FF = self.params.Kff_main * np.ones(12)
self.result.tau_ff[3:6] = self.compute_torque(m)[:]
- if self.params.interpolate_mpc:
- if self.mpc_result.new_result:
- if self.params.interpolation_type == 3:
- self.interpolator.update(xs[0], xs[1], xs[2])
- # self.interpolator.plot(self.pd.mpc_wbc_ratio, self.pd.dt_wbc)
-
- t = (self.k - self.k_solve + 1) * self.pd.dt_wbc
- q, v = self.interpolator.interpolate(t)
- else:
- q, v = self.integrate_x(m)
+ # if self.params.interpolate_mpc:
+ # if self.mpc_result.new_result:
+ # if self.params.interpolation_type == 3:
+ # self.interpolator.update(xs[0], xs[1], xs[2])
+ # # self.interpolator.plot(self.pd.mpc_wbc_ratio, self.pd.dt_wbc)
+
+ # t = (self.k - self.k_solve + 1) * self.pd.dt_wbc
+ # q, v = self.interpolator.interpolate(t)
+ # else:
+ # q, v = self.integrate_x(m)
+ q = xs[1][:3]
+ v = xs[1][3:]
self.result.q_des[3:6] = q[:]
self.result.v_des[3:6] = v[:]
@@ -422,8 +424,9 @@ class Controller:
# m["x_m"][self.pd.nq :] - self.mpc_result.xs[0][self.pd.nq :],
# ]
# )
- x_diff = self.mpc_result.xs[0] - m["x_m"]
- tau = self.mpc_result.us[0] + np.dot(self.mpc_result.K[0], x_diff)
+ # x_diff = self.mpc_result.xs[0] - m["x_m"]
+ # tau = self.mpc_result.us[0] + np.dot(self.mpc_result.K[0], x_diff)
+ tau = self.mpc_result.us[0]
return tau
def integrate_x(self, m):
diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
index 4acc50cc..ce900e6c 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -147,7 +147,6 @@ class LoggerControl:
self.plot_controller_times()
if not self.params.enable_multiprocessing:
self.plot_OCP_times()
- self.plot_OCP_update_times()
plt.show()
--
GitLab