From e6b83dd26143b093bad54b914e7f0b3abec290cf Mon Sep 17 00:00:00 2001
From: Ale <alessandroassirell98@gmail.com>
Date: Mon, 8 Aug 2022 15:53:37 +0200
Subject: [PATCH] added riccatio gains
---
config/walk_parameters.yaml | 2 +-
.../quadruped_reactive_walking/Controller.py | 22 +++++++++----------
2 files changed, 12 insertions(+), 12 deletions(-)
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 50d4e5ca..5e51ab10 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: 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 1b1b1315..7986e453 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -414,17 +414,17 @@ class Controller:
"""
Compute the feedforward torque using ricatti gains
"""
- # x_diff = np.concatenate(
- # [
- # pin.difference(
- # self.pd.model,
- # 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 :],
- # ]
- # )
- x_diff = self.mpc_result.xs[0] - m["x_m"]
+ x_diff = np.concatenate(
+ [
+ pin.difference(
+ self.pd.model,
+ 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 :],
+ ]
+ )
+ # 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
--
GitLab