From afa391ae5cc0031ed80e39db85279a7049e9ebf9 Mon Sep 17 00:00:00 2001
From: Fanny Risbourg <frisbourg@laas.fr>
Date: Tue, 30 Aug 2022 11:32:03 +0200
Subject: [PATCH] small stuff
---
config/walk_parameters.yaml | 11 ++++-------
.../WB_MPC/CrocoddylOCP.py | 2 +-
.../quadruped_reactive_walking/WB_MPC/ProblemData.py | 2 +-
python/quadruped_reactive_walking/WB_MPC/Target.py | 8 ++++----
.../tools/LoggerControl.py | 12 +++++-------
5 files changed, 15 insertions(+), 20 deletions(-)
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 9ab29a85..95273718 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -11,7 +11,7 @@ robot:
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: 5000 # 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
@@ -22,23 +22,20 @@ 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.015 # Time step of the model predictive control
+ dt_mpc: 0.01 # 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
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+
-# 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+
Kff_main: 1.0 # Feedforward torques multiplier for the PD+
# Parameters of Gait
N_periods: 1
- gait: [2, 1, 1, 1, 1,
- 30, 1, 0, 1, 1] # Initial gait matrix
+ gait: [5, 1, 1, 1, 1,
+ 45, 1, 0, 1, 1]
# 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/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index 72f2fffd..caa644d4 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -13,7 +13,7 @@ class OCP:
def __init__(self, pd: ProblemData, params, footsteps, gait):
self.pd = pd
self.params = params
- self.max_iter = 1
+ self.max_iter = 500 if params.save_guess else 1
self.state = crocoddyl.StateMultibody(self.pd.model)
self.initialized = False
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index 50818995..ca63488d 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -62,7 +62,7 @@ class ProblemData(problemDataAbstract):
# Cost function weights
self.mu = 0.7
- self.foot_tracking_w = 1e1
+ self.foot_tracking_w = 2. * 1e1
self.friction_cone_w = 1e4
self.control_bound_w = 1e3
self.control_reg_w = 1e0
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index 2821cd0d..ce90c06f 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -15,10 +15,10 @@ class Target:
)
if params.movement == "circle":
- self.A = np.array([0, 0.03, 0.04])
- self.offset = np.array([0, 0, 0.05])
- self.freq = np.array([0, 0.5, 0.5])
- self.phase = np.array([0, 0, -np.pi/2])
+ self.A = np.array([0.05, 0., 0.04])
+ self.offset = np.array([0.05, 0, 0.05])
+ self.freq = np.array([0.5, 0., 0.5])
+ self.phase = np.array([-np.pi/2-0.5, 0., -np.pi/2])
elif params.movement == "step":
self.initial = self.position[:, 1].copy()
self.target = self.position[:, 1].copy() + np.array([0.1, 0.0, 0.0])
diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
index 201c6ea2..0370937b 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -230,15 +230,13 @@ class LoggerControl:
}
# Target plot
+ _, axs = plt.subplots(3, sharex=True)
legend = ["x", "y", "z"]
-
- 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.feet_ids[1]][:, p])
- plt.legend(["Target", "Measured"])
+ axs[p].set_title("Free foot on " + legend[p])
+ axs[p].plot(self.target[:, p])
+ axs[p].plot(m_feet_p_log[self.pd.feet_ids[1]][:, p])
+ axs[p].legend(["Target", "Measured"])
# "Predicted"])
if save:
plt.savefig(fileName + "/target")
--
GitLab