From d396ce74ce21f3c3dcc459a37661e50012882381 Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Fri, 6 Aug 2021 17:18:28 +0200
Subject: [PATCH] Remove updateState function that has been replaced by a c++
version
---
scripts/Controller.py | 48 -------------------------------------------
1 file changed, 48 deletions(-)
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 56988095..98418191 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -394,51 +394,3 @@ class Controller:
self.t_mpc = t_mpc - t_planner
self.t_wbc = t_wbc - t_mpc
self.t_loop = time.time() - tic
-
- def updateState(self):
-
- # Update reference velocity vector
- self.v_ref[0:3, 0] = self.joystick.v_ref[0:3, 0] # TODO: Joystick velocity given in base frame and not
- self.v_ref[3:6, 0] = self.joystick.v_ref[3:6, 0] # in horizontal frame (case of non flat ground)
- self.v_ref[6:, 0] = 0.0
-
- # Update position and velocity state vectors
- if not self.gait.getIsStatic():
- # Integration to get evolution of perfect x, y and yaw
- Ryaw = np.array([[math.cos(self.yaw_estim), -math.sin(self.yaw_estim)],
- [math.sin(self.yaw_estim), math.cos(self.yaw_estim)]])
-
- self.q[0:2, 0:1] = self.q[0:2, 0:1] + Ryaw @ self.v_ref[0:2, 0:1] * self.dt_wbc
-
- # Mix perfect x and y with height measurement
- self.q[2, 0] = self.estimator.getQFilt()[2]
-
- # Mix perfect yaw with pitch and roll measurements
- self.yaw_estim += self.v_ref[5, 0] * self.dt_wbc
- self.q[3:7, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(self.estimator.getRPY()[0], self.estimator.getRPY()[1], self.yaw_estim)).coeffs()
-
- # Actuators measurements
- self.q[7:, 0] = self.estimator.getQFilt()[7:]
-
- # Velocities are the one estimated by the estimator
- self.v = self.estimator.getVFilt().reshape((-1, 1))
- hRb = pin.rpy.rpyToMatrix(self.estimator.getRPY()[0], self.estimator.getRPY()[1], 0.0)
-
- self.h_v[0:3, 0:1] = hRb @ self.v[0:3, 0:1]
- self.h_v[3:6, 0:1] = hRb @ self.v[3:6, 0:1]
-
- # self.v[:6, 0] = self.joystick.v_ref[:6, 0]
- else:
- quat = np.array([[0.0, 0.0, 0.0, 1.0]]).transpose()
- hRb = np.eye(3)
- pass
- # TODO: Adapt static mode to new version of the code
-
- # Transformation matrices between world and horizontal frames
- oRh = np.eye(3)
- c = math.cos(self.yaw_estim)
- s = math.sin(self.yaw_estim)
- oRh[0:2, 0:2] = np.array([[c, -s], [s, c]])
- oTh = np.array([[self.q[0, 0]], [self.q[1, 0]], [0.0]])
-
- return oRh, oTh
--
GitLab