From 65f6d3fb74b45eb213419fa368b85a25474b3f26 Mon Sep 17 00:00:00 2001
From: Ale <alessandroassirell98@gmail.com>
Date: Tue, 26 Jul 2022 14:55:44 +0200
Subject: [PATCH] timings quite good
---
config/walk_parameters.yaml | 4 ++--
python/quadruped_reactive_walking/Controller.py | 9 ++++++---
python/quadruped_reactive_walking/WB_MPC/ProblemData.py | 4 ++--
3 files changed, 10 insertions(+), 7 deletions(-)
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 9873654a..0ec3b667 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -7,11 +7,11 @@ robot:
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
- enable_pyb_GUI: True # Enable/disable PyBullet GUI
+ 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: 1000 # Number of simulated wbc time steps
+ N_SIMULATION: 5000 # 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
perfect_estimator: true # Enable/disable perfect estimator by using data directly from PyBullet
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 2d405c77..acafb1ea 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -72,7 +72,7 @@ class Controller:
device = DummyDevice()
device.joints.positions = q_init
try:
- # file = np.load('/tmp/init_guess.npy', allow_pickle=True).item()
+ file = np.load('/tmp/init_guess.npy', allow_pickle=True).item()
self.guess = {"xs": list(file["xs"]), "us": list(file["us"])}
print("\nInitial guess loaded\n")
except:
@@ -102,6 +102,9 @@ class Controller:
#else:
# self.mpc.solve(self.k, m["x_m"], self.guess)
+ ### ONLY IF YOU WANT TO STORE THE FIRST SOLUTION TO WARMSTART THE INITIAL Problem ###
+ #np.save(open('/tmp/init_guess.npy', "wb"), {"xs": self.mpc.ocp.get_results().x, "us": self.mpc.ocp.get_results().u} )
+
except ValueError:
self.error = True
print("MPC Problem")
@@ -138,7 +141,7 @@ class Controller:
self.t_send = t_send - t_mpc
self.clamp_result(device)
- #self.security_check(m)
+ self.security_check(m)
if self.error:
self.set_null_control()
@@ -175,7 +178,7 @@ class Controller:
print(m["qj_m"])
print(np.abs(m["qj_m"]) > self.q_security)
self.error = True
- elif (np.abs(m["vj_m"]) > 500 * np.pi / 180).any():
+ elif (np.abs(m["vj_m"]) > 1000 * np.pi / 180).any():
print("-- VELOCITY TOO HIGH ERROR --")
print(m["vj_m"])
print(np.abs(m["vj_m"]) > 500 * np.pi / 180)
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index bc7d0981..d2ae9801 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -113,11 +113,11 @@ class ProblemDataFull(problemDataAbstract):
# Cost function weights
self.mu = 0.7
- self.foot_tracking_w = 1e2
+ self.foot_tracking_w = 1e3
#self.friction_cone_w = 1e3 * 0
self.control_bound_w = 1e3
self.control_reg_w = 1e1
- self.state_reg_w = np.array([1e-3] * 3 + [3*1e-1]*3)
+ self.state_reg_w = np.array([1e-3] * 3 + [5*1e-1]*3)
self.terminal_velocity_w = np.array([0] * 3 + [1e3] * 3 )
self.q0_reduced = self.q0[10:13]
--
GitLab