From 6f6b8166dd53f133fbcf13fb9962485b204cc00e Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Thu, 28 Jul 2022 15:15:38 +0200
Subject: [PATCH] try experiment with closed-loop
---
python/quadruped_reactive_walking/Controller.py | 16 ++++++++--------
.../WB_MPC/ProblemData.py | 3 +--
.../tools/LoggerControl.py | 6 +++---
3 files changed, 12 insertions(+), 13 deletions(-)
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 7946a8fd..27b4ba04 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -44,7 +44,7 @@ class DummyDevice:
def __init__(self):
self.positions = np.zeros(12)
self.velocities = np.zeros(12)
-
+90
class Controller:
def __init__(self, pd, target, params, q_init, t):
@@ -103,15 +103,15 @@ class Controller:
if self.k % int(self.params.dt_mpc/self.params.dt_wbc) == 0:
try:
# Closed-loop
- # self.mpc.solve(self.k, m["x_m"], self.xs_init, self.us_init)
+ self.mpc.solve(self.k, m["x_m"], self.xs_init, self.us_init)
# Trajectory tracking
- if self.initialized:
- self.mpc.solve(
- self.k, self.mpc_result.xs[1], self.xs_init, self.us_init)
- else:
- self.mpc.solve(self.k, m["x_m"],
- self.xs_init, self.us_init)
+ # if self.initialized:
+ # self.mpc.solve(
+ # self.k, self.mpc_result.xs[1], self.xs_init, self.us_init)
+ # else:
+ # self.mpc.solve(self.k, m["x_m"],
+ # self.xs_init, self.us_init)
except ValueError:
self.error = True
print("MPC Problem")
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index 242d28f3..8064e805 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -5,8 +5,7 @@ import pinocchio as pin
class problemDataAbstract:
def __init__(self, param, frozen_names = []):
self.dt = param.dt_mpc # OCP dt
- self.dt_sim = 0.001
- self.dt_bldc = 0.0005
+ self.dt_sim = param.dt_wbc
self.r1 = int(self.dt / self.dt_sim)
self.init_steps = 0
self.target_steps = 60
diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
index e26af77a..4f5afe39 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -233,9 +233,9 @@ class LoggerControl:
plt.savefig(fileName + "/target")
self.plot_controller_times()
- if not self.params.enable_multiprocessing:
- self.plot_OCP_times()
- self.plot_OCP_update_times()
+ # if not self.params.enable_multiprocessing:
+ # self.plot_OCP_times()
+ # self.plot_OCP_update_times()
plt.show()
--
GitLab