From b9bc218991761d41d98e02d5d16375ae818a9fe4 Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Tue, 10 Aug 2021 12:19:29 +0200
Subject: [PATCH] Plot to check modulo of the 15Hz filter
---
scripts/Controller.py | 55 +++++++++++++++++++++++--------------------
1 file changed, 30 insertions(+), 25 deletions(-)
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 089b7201..ead676aa 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -248,32 +248,37 @@ class Controller:
self.gait.updateGait(self.k, self.k_mpc, self.joystick.joystick_code)
# self.q[3:7, 0] = np.array([0, 0, 0.3826834, 0.9238795])
- """state = np.array([0.0, 0.0, 45.0 / 180 * np.pi])
- log = np.zeros((600, 3))
- for i in range(600):
- print(i)
- if i < 300:
- state += np.array([-np.pi/180, 0.0, 0.0])
- else:
- state -= np.array([-np.pi/180, 0.0, 0.0])
- for j in range(3):
+ state = np.array([0.0, 0.0, 45.0 / 180 * np.pi])
+ log = np.zeros((600, 3))
+ log_state = np.zeros((600, 3))
+ for i in range(600):
+ print(i)
+ if i < 300:
+ state += np.array([-np.pi/180, -1.5*np.pi/180, np.pi/180])
+ else:
+ state -= np.array([-np.pi/180, -1.5*np.pi/180, np.pi/180])
+ for j in range(3):
+ if state[j] > np.pi:
if state[j] > np.pi:
- state[j] -= 2*np.pi
- elif state[j] < -np.pi:
- state[j] += 2*np.pi
- self.q[3:7, 0] = pin.Quaternion(pin.rpy.rpyToMatrix(state)).coeffs()
- self.filter_mpc_q.filter(self.q[:7, 0:1])
- log[i, :] = self.filter_mpc_q.getFilt()[3:]
-
- from matplotlib import pyplot as plt
- plt.figure()
- for i in range(3):
- plt.plot(log[:, i], linewidth=3)
- plt.legend(["Roll", "Pitch", "Yaw"])
- plt.show()
-
- from IPython import embed
- embed()"""
+ if state[j] > np.pi:
+ state[j] -= 2*np.pi
+ elif state[j] < -np.pi:
+ state[j] += 2*np.pi
+ self.q[3:6, 0] = state.copy()
+ self.filter_mpc_q.filter(self.q[:6, 0:1], True)
+ log[i, :] = self.filter_mpc_q.getFilt()[3:]
+ log_state[i, :] = state.copy()
+
+ from matplotlib import pyplot as plt
+ plt.figure()
+ for i in range(3):
+ plt.plot(log[:, i], linewidth=3)
+ plt.plot(log_state[:, i], linewidth=3, linestyle='--')
+ plt.legend(["Roll", "Pitch", "Yaw"])
+ plt.show()
+
+ from IPython import embed
+ embed()
#Â Quantities go through a 1st order low pass filter with fc = 15 Hz
self.q_filt_mpc[:, 0] = self.filter_mpc_q.filter(self.q[:6, 0:1], True)
--
GitLab