From bac3ffd94d1881ac80e582629a25a051c471ccbf Mon Sep 17 00:00:00 2001
From: Ale <alessandroassirell98@gmail.com>
Date: Thu, 18 Aug 2022 10:21:32 +0200
Subject: [PATCH] Fix integration and plot
---
config/walk_parameters.yaml | 10 +++++-----
python/quadruped_reactive_walking/Controller.py | 12 ++++++------
.../tools/LoggerControl.py | 16 +++++++++-------
3 files changed, 20 insertions(+), 18 deletions(-)
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index ea783e70..bf5719a0 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -6,12 +6,12 @@ robot:
LOGGING: true # Enable/disable logging during the experiment
PLOTTING: true # Enable/disable automatic plotting at the end of the experiment
DEMONSTRATION: false # Enable/disable demonstration functionalities
- SIMULATION: false # Enable/disable PyBullet simulation or running on real robot
- enable_pyb_GUI: true # Enable/disable PyBullet GUI
+ SIMULATION: true # Enable/disable PyBullet simulation or running on real robot
+ enable_pyb_GUI: false # Enable/disable PyBullet GUI
envID: 0 # Identifier of the environment to choose in which one the simulation will happen
use_flat_plane: true # If True the ground is flat, otherwise it has bumps
predefined_vel: true # If we are using a predefined reference velocity (True) or a joystick (False)
- N_SIMULATION: 15000 # Number of simulated wbc time steps
+ 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
perfect_estimator: true # Enable/disable perfect estimator by using data directly from PyBullet
@@ -25,8 +25,8 @@ 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
save_guess: false # true to interpolate the impedance quantities between nodes of the MPC
- movement: "circle" # name of the movement to perform
- interpolate_mpc: true # true to interpolate the impedance quantities between nodes of the MPC
+ movement: "step" # name of the movement to perform
+ interpolate_mpc: false # 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+
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 6a78a1fc..0898af1b 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -169,7 +169,6 @@ class Controller:
self.result = Result(params)
self.result.q_des = self.pd.q0[7:].copy()
self.result.v_des = self.pd.v0[6:].copy()
- self.result.FF = self.params.Kff_main * np.ones(12)
self.target = Target(params)
self.footsteps = []
@@ -272,17 +271,19 @@ class Controller:
if not self.initialized and self.params.save_guess:
self.save_guess()
+ self.result.FF = self.params.Kff_main * np.ones(12)
+ self.result.tau_ff = 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)
+ # 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)
- self.result.tau_ff = self.compute_torque(m)[:]
self.result.q_des = q[:]
self.result.v_des = v[:]
@@ -483,11 +484,10 @@ class Controller:
m["x_m"][: self.pd.nq],
self.mpc_result.xs[0][: self.pd.nq],
),
- m["x_m"][self.pd.nq :] - self.mpc_result.xs[0][self.pd.nq :],
+ self.mpc_result.xs[0][self.pd.nq :] - m["x_m"][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)
+ tau = self.mpc_result.us[0] + np.dot(self.mpc_result.K[0], x_diff)
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 468c4d41..1fb199be 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -253,14 +253,15 @@ class LoggerControl:
# Target plot
legend = ["x", "y", "z"]
- plt.figure(figsize=(12, 18), dpi=90)
+
+ fig, axs = plt.subplots(3, sharex=True)
for p in range(3):
- plt.subplot(3, 1, p + 1)
- plt.title("Free foot on " + legend[p])
- plt.plot(self.target[:, p])
- plt.plot(m_feet_p_log[self.pd.rfFootId][:, p])
- plt.plot(feet_p_log[self.pd.rfFootId][:, p])
- plt.legend(["Target", "Measured", "Predicted"])
+ axs[p].set_title("Free foot on " + legend[p])
+ axs[p].plot(self.target[:, p], label="Target")
+ axs[p].plot(m_feet_p_log[self.pd.rfFootId][:, p], label="Measured")
+ axs[p].plot(feet_p_log[self.pd.rfFootId][:, p], label="Predicted")
+ axs[p].legend()
+
if save:
plt.savefig(fileName + "/target")
@@ -291,6 +292,7 @@ class LoggerControl:
plt.legend(["Stiffness", "Damping"])
plt.ylabel("Gains")
plt.xlabel("t")
+
if save:
plt.savefig(fileName + "/diagonal_Riccati_gains")
--
GitLab