From 7c67e8e7f726bca784f96d450a9a90948108f85c Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Thu, 5 Aug 2021 12:12:36 +0200
Subject: [PATCH] Remove unused variable x_f_wbc from main loop and
LoggerControl
---
scripts/Controller.py | 20 +-------------------
scripts/LoggerControl.py | 4 ----
2 files changed, 1 insertion(+), 23 deletions(-)
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 826382ea..08b5c36b 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -283,10 +283,6 @@ class Controller:
# Retrieve reference contact forces in horizontal frame
self.x_f_mpc = self.mpc_wrapper.get_latest_result()
- """if self.k == 0:
- self.x_save = self.x_f_mpc[12:, :].copy()
- else:
- self.x_f_mpc[12:, :] = self.x_save.copy()"""
t_mpc = time.time()
@@ -301,20 +297,6 @@ class Controller:
# Update pos, vel and acc references for feet
self.footTrajectoryGenerator.update(self.k, o_targetFootstep)
- # Target state for the whole body control
- self.x_f_wbc = (self.x_f_mpc[:24, 0]).copy()
- if not self.gait.getIsStatic():
- self.x_f_wbc[0] = self.dt_wbc * xref[6, 1]
- self.x_f_wbc[1] = self.dt_wbc * xref[7, 1]
- self.x_f_wbc[2] = self.h_ref
- self.x_f_wbc[3] = 0.0
- self.x_f_wbc[4] = 0.0
- self.x_f_wbc[5] = self.dt_wbc * xref[11, 1]
- else: # Sort of position control to avoid slow drift
- self.x_f_wbc[0:3] = self.planner.q_static[0:3, 0] # TODO: Adapt to new code
- self.x_f_wbc[3:6] = self.planner.RPY_static[:, 0]
- self.x_f_wbc[6:12] = xref[6:, 1]
-
# Whole Body Control
# If nothing wrong happened yet in the WBC controller
if (not self.error) and (not self.joystick.stop):
@@ -336,7 +318,7 @@ class Controller:
# Run InvKin + WBC QP
self.wbcWrapper.compute(self.q_wbc, self.b_v,
- self.x_f_wbc[12:], np.array([cgait[0, :]]),
+ (self.x_f_mpc[12:24, 0]).copy(), np.array([cgait[0, :]]),
self.feet_p_cmd,
self.feet_v_cmd,
self.feet_a_cmd)
diff --git a/scripts/LoggerControl.py b/scripts/LoggerControl.py
index 6a39d791..5ff6bb1a 100644
--- a/scripts/LoggerControl.py
+++ b/scripts/LoggerControl.py
@@ -79,7 +79,6 @@ class LoggerControl():
self.mpc_x_f = np.zeros([logSize, 24, statePlanner.getNSteps()])
# Whole body control
- self.wbc_x_f = np.zeros([logSize, 24]) # input vector of the WBC (next state + reference contact force)
self.wbc_P = np.zeros([logSize, 12]) # proportionnal gains of the PD+
self.wbc_D = np.zeros([logSize, 12]) # derivative gains of the PD+
self.wbc_q_des = np.zeros([logSize, 12]) # desired position of actuators
@@ -151,7 +150,6 @@ class LoggerControl():
self.mpc_x_f[self.i] = loop.x_f_mpc
# Logging from whole body control
- self.wbc_x_f[self.i] = loop.x_f_wbc
self.wbc_P[self.i] = loop.result.P
self.wbc_D[self.i] = loop.result.D
self.wbc_q_des[self.i] = loop.result.q_des
@@ -611,7 +609,6 @@ class LoggerControl():
mpc_x_f=self.mpc_x_f,
- wbc_x_f=self.wbc_x_f,
wbc_P=self.wbc_P,
wbc_D=self.wbc_D,
wbc_q_des=self.wbc_q_des,
@@ -696,7 +693,6 @@ class LoggerControl():
self.mpc_x_f = data["mpc_x_f"]
- self.wbc_x_f = data["wbc_x_f"]
self.wbc_P = data["wbc_P"]
self.wbc_D = data["wbc_D"]
self.wbc_q_des = data["wbc_q_des"]
--
GitLab