From 890a6f164d0153d97dbcb81dc47a71ea9cd96d1c Mon Sep 17 00:00:00 2001
From: Ale <alessandroassirell98@gmail.com>
Date: Thu, 18 Aug 2022 10:17:16 +0200
Subject: [PATCH] fix x_diff definition in torques
---
config/walk_parameters.yaml | 10 +++++-----
python/quadruped_reactive_walking/Controller.py | 4 ++--
.../quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py | 2 +-
3 files changed, 8 insertions(+), 8 deletions(-)
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 75b856de..ebe0cb2d 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
@@ -24,7 +24,7 @@ robot:
dt_wbc: 0.001 # Time step of the whole body control
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
+ 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
interpolation_type: 3 # 0,1,2,3 decide which kind of interpolation is used
@@ -32,13 +32,13 @@ robot:
# 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.4, 0.4, 0.4] # Derivative 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
N_periods: 1
- gait: [20, 1, 1, 1, 1,
- 100, 1, 0, 1, 1] # Initial gait matrix
+ gait: [10, 1, 1, 1, 1,
+ 20, 1, 0, 1, 1] # Initial gait matrix
# Parameters of Joystick
gp_alpha_vel: 0.003 #Â Coefficient of the low pass filter applied to gamepad velocity
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index cf414780..9aeaff20 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -500,10 +500,10 @@ class Controller:
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:]
]
)
- 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/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index 9291283c..82e3a683 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -12,7 +12,7 @@ from time import time
class OCP:
def __init__(self, pd: ProblemData, footsteps, gait):
self.pd = pd
- self.max_iter = 1
+ self.max_iter = 3
self.state = crocoddyl.StateMultibody(self.pd.model)
self.initialized = False
--
GitLab