Remove updateState function that has been replaced by a c++ version

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