From e9608409f245b27a56fc9fb99790f9317c3560d6 Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Wed, 14 Sep 2022 09:33:09 +0200
Subject: [PATCH] code for xps, logger prepared for ocp plots
---
config/walk_parameters.yaml | 4 +--
.../quadruped_reactive_walking/Controller.py | 34 +++++++++----------
.../tools/LoggerControl.py | 32 ++++++++---------
3 files changed, 35 insertions(+), 35 deletions(-)
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 66d4cf77..7d111baf 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -6,7 +6,7 @@ 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: true # Enable/disable PyBullet simulation or running on real robot
+ SIMULATION: false # 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
@@ -26,7 +26,7 @@ robot:
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
+ 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 5996921f..076a06c4 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -274,21 +274,21 @@ 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)
-
- self.result.q_des[3:6] = xs[1][:3]
- self.result.v_des[3:6] = xs[1][3:]
-
- # self.result.q_des[3:6] = q
- # self.result.v_des[3:6] = v
+ 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)
+
+ # self.result.q_des[3:6] = xs[1][:3]
+ # self.result.v_des[3:6] = xs[1][3:]
+
+ self.result.q_des[3:6] = q
+ self.result.v_des[3:6] = v
if self.axs is not None:
[
@@ -490,8 +490,8 @@ class Controller:
self.mpc_result.xs[0][self.pd.nq :] - m["x_m"][self.pd.nq :],
]
)
- # tau = self.mpc_result.us[0] + np.dot(self.mpc_result.K[0], x_diff)
- tau = self.mpc_result.us[0]
+ 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 f8a1ce36..067d49a1 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -269,22 +269,22 @@ class LoggerControl:
if save:
plt.savefig(fileName + "/target")
- legend = ['x', 'y', 'z']
- plt.figure(figsize=(12, 18), dpi = 90)
- all_ocp_feet_p_log[self.pd.rfFootId] = all_ocp_feet_p_log[self.pd.rfFootId][1:]
- for p in range(3):
- plt.subplot(3,1, p+1)
- plt.title('Free foot on ' + legend[p])
- for i in range(horizon-2):
- t = np.linspace(i*self.pd.dt, (self.pd.T+ i)*self.pd.dt, self.pd.T+1)
- y = all_ocp_feet_p_log[self.pd.rfFootId][i][:,p]
- for j in range(len(y) - 1):
- plt.plot(t[j:j+2], y[j:j+2], color='C0', linewidth = 3, marker='o', markersize = 5, alpha=max([1 - j/len(y), 0]))
- plt.plot(t_scale[:-10], self.target[10:, p], color="C1", linewidth=3)
- plt.plot(t_scale[:-10], m_feet_p_log[self.pd.rfFootId][10:, p], color="C2", linewidth=3)
- plt.xlim((-0.05, 0.2))
- if save:
- plt.savefig(fileName + "/ocp_predictions", format = "svg", dpi=300)
+ # legend = ['x', 'y', 'z']
+ # plt.figure(figsize=(12, 18), dpi = 90)
+ # all_ocp_feet_p_log[self.pd.rfFootId] = all_ocp_feet_p_log[self.pd.rfFootId][1:]
+ # for p in range(3):
+ # plt.subplot(3,1, p+1)
+ # plt.title('Free foot on ' + legend[p])
+ # for i in range(horizon-2):
+ # t = np.linspace(i*self.pd.dt, (self.pd.T+ i)*self.pd.dt, self.pd.T+1)
+ # y = all_ocp_feet_p_log[self.pd.rfFootId][i][:,p]
+ # for j in range(len(y) - 1):
+ # plt.plot(t[j:j+2], y[j:j+2], color='C0', linewidth = 3, marker='o', markersize = 5, alpha=max([1 - j/len(y), 0]))
+ # plt.plot(t_scale[:-10], self.target[10:, p], color="C1", linewidth=3)
+ # plt.plot(t_scale[:-10], m_feet_p_log[self.pd.rfFootId][10:, p], color="C2", linewidth=3)
+ # plt.xlim((-0.05, 0.2))
+ # if save:
+ # plt.savefig(fileName + "/ocp_predictions", format = "svg", dpi=300)
def plot_riccati_gains(self, n, save=False, fileName="/tmp"):
import matplotlib.pyplot as plt
--
GitLab