rafaelcaricio/Open_Duck_Mini_Runtime
1
0
Fork 0
mirror of https://github.com/apirrone/Open_Duck_Mini_Runtime.git synced 2025-09-03 03:33:54 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

update

Browse source
This commit is contained in:
apirrone 2025-08-01 10:21:49 +02:00
parent 44bccb38c8
commit 0d71a47e31
2 changed files with 17 additions and 6 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

17
mini_bdx_runtime/mini_bdx_runtime/imu.py
View file

@ -164,9 +164,18 @@ if __name__ == "__main__":
imu = Imu(50, calibrate=False, upside_down=False)
# imu = Imu(50, upside_down=False)
while True:
data = imu.get_data()
print(data)
print("gyro", np.around(data["gyro"], 3))
print("orientation", np.around(data["orientation"], 3))
data = imu.get_data(as_mat=True)
# print(data)
# print("gyro", np.around(data["gyro"], 3))
# print("orientation", np.around(data["orientation"], 3))
mat = data["orientation"]
# gravity = np.array(data.site_xmat[self.get_site_id_from_name("imu")]).reshape(
# (3, 3)
# ).T @ np.array([0, 0, -1])
gravity = mat @ np.array([0, 0, -1])
print("gravity", np.around(gravity, 3))
print("---")
time.sleep(1 / 25)

6
scripts/imu_client.py
View file

@ -24,7 +24,7 @@ class IMUClient:
print(e)
time.sleep(0.5)
self.imu_queue = Queue(maxsize=1)
self.last_imu = [0, 0, 0, 0]
self.last_imu = {"orientation": [0, 0, 0, 0], "gyro": [0, 0, 0]}
Thread(target=self.imu_worker, daemon=True).start()
@ -63,7 +63,9 @@ if __name__ == "__main__":
pose[:3, 3] = [0.1, 0.1, 0.1]
try:
while True:
quat = client.get_imu()
data = client.get_imu()
quat = data["orientation"]
gyro = data["gyro"]
try:
rot_mat = R.from_quat(quat).as_matrix()
pose[:3, :3] = rot_mat