From 5a89c9bc8800d27cbc776ad56b81d5192265cf81 Mon Sep 17 00:00:00 2001 From: Ophelie Date: Wed, 9 Oct 2024 18:29:59 +0400 Subject: [PATCH] Modification on FrocePlateData + add a code to take from qualisys and send to a sever [WIP] --- biosiglive/interfaces/qualisys_interface.py | 26 ++--- examples/sandox/CompuServeetQual.py | 117 ++++++++++++++++++++ 2 files changed, 127 insertions(+), 16 deletions(-) create mode 100644 examples/sandox/CompuServeetQual.py diff --git a/biosiglive/interfaces/qualisys_interface.py b/biosiglive/interfaces/qualisys_interface.py index 6350c61..b0ccb88 100644 --- a/biosiglive/interfaces/qualisys_interface.py +++ b/biosiglive/interfaces/qualisys_interface.py @@ -272,25 +272,19 @@ def get_force_plate_data( if get_frame: packet.framenumber headerf, forcesdata = packet.get_force() - - all_forces_data = np.empty([len(forcesdata),9,1]) + new_data=np.zeros([18,len(forcesdata[0][1])]) + all_forces_data = np.empty([18,1]) + channel_name = ['Force_x', 'Force_y', 'Force_z', 'Moment_x', 'Moment_y', 'Moment_z', 'CoP_x', 'CoP_y', 'CoP_z']; + unit = ['N', 'N', 'N', 'Nmm', 'Nmm', 'Nmm', 'mm', 'mm', 'mm'] if (forcesdata[0][0].force_count) != 0: for platenum in range(len(forcesdata)): PFForce = forcesdata[platenum][1] - new_data = np.zeros((9, len(PFForce))) - data_tmp = np.array(PFForce) - data_tmp = data_tmp.T - count = 0 - channel_name = ['Force_x','Force_y','Force_z','Moment_x','Moment_y','Moment_z','CoP_x','CoP_y','CoP_z']; - unit = ['N', 'N', 'N', 'Nmm', 'Nmm', 'Nmm', 'mm', 'mm', 'mm'] - for i in range(9): - new_data[count, :] = data_tmp[count] - count += 1 - if count == 9: - break - #allonepfdata = all_forces_data[platenum] - #allonepfdata + new_data - all_forces_data[platenum] = new_data + for subframe in range(len(PFForce)): + data_tmp=PFForce[subframe] + + new_data[9*platenum:9*platenum+9, subframe] = [data_tmp.x, data_tmp.y, data_tmp.z, data_tmp.x_m, data_tmp.y_m, data_tmp.z_m, data_tmp.x_a, data_tmp.y_a, data_tmp.z_a] + + all_forces_data = new_data return all_forces_data """ diff --git a/examples/sandox/CompuServeetQual.py b/examples/sandox/CompuServeetQual.py new file mode 100644 index 0000000..d32e781 --- /dev/null +++ b/examples/sandox/CompuServeetQual.py @@ -0,0 +1,117 @@ +""" +This example shows how to retrieve marker data from a Vicon Nexus interface. Please note that the Vicon interface is the only implemented method capable of retrieving marker data. +First, you need to create a ViconClient object. This object will be used to connect to the Vicon system and retrieve data. Next, you need to add a set of markers to the interface. For now, only one marker set can be added. +The marker set takes the following arguments: + - nb_markers : int + Number of markers. + - name : str + Name of the marker set. + - marker_names : Union [list, str] + List of marker names. + subject_name : str + Name of the subject. If None, the subject will be the first in Nexus. + rate: int + Rate of the camera used to record the marker trajectories. + unit : str + Unit of the marker trajectories ("mm" or "m"). +If you want to display the markers in a 3D scatter plot, you can add a Scatter3D plot to the interface. You can pass the size and color of the marker via size and color argument, respectively. Please see the Scatter3D documentation for more information. +Next, the data flow runs in a loop where the get_marker_set_data() function is used to retrieve the data from the interface. The data is then passed to the graph via the update() method through an array of (n_frame, n_markers, 3) where the plot parameters can be updated. +""" +from time import sleep, time +import importlib +import biosiglive +importlib.reload(biosiglive) +from biosiglive import (LivePlot, PlotType, QualisysClient) +from biosiglive import load, RealTimeProcessingMethod, InterfaceType, DeviceType, Server, InverseKinematicsMethods +import asyncio +import logging +import xml.etree.ElementTree as ET +import qtm_rt +import numpy as np +from collections import deque + +# Fonction pour détecter si Fz dépasse le seuil +def detect_start(previous_f_z, current_f_z, threshold=30): + # Détection du passage de inférieur à supérieur au seuil + return previous_f_z <= threshold < current_f_z + +class RealTimeDataProcessor: + def __init__(self, server_ip="192.168.0.1", port=7, + threshold=30, system_rate=100, device_rate=2000, nb_markers=4, nb_seconds=1): + # Initialisation du serveur + self.server = Server(server_ip, port) + self.server.start() + # Variables d'état + self.sending_started = False + self.previous_fz = 0 + self.threshold = threshold + + async def setup(self): + """ main function """ + # Connection to qualisys + self.interface = await QualisysClient.create(ip="192.168.0.2", system_rate=100, port=22224) + queue = asyncio.Queue() + + # Add info needed + n_markers = 4 + await self.interface.add_marker_set( + nb_markers=n_markers, data_buffer_size=1000, marker_data_file_key="markers", name="markers", rate=100, unit="mm" + ) + await self.interface.add_device( + nb_channels=12, + device_type="force_plate", + name="force_plate", + data_buffer_size=100, + rate=1000, + device_data_file_key="force_plate", + processing_method=None, + moving_average=True, + ) + + while True: + tic = asyncio.get_event_loop().time() + packet = await self.interface.Connect.get_current_frame(components=self.interface.component) + + #data recuperation + mark_tmp = self.interface.get_marker_set_data(packet=packet) + mark_tmp = mark_tmp + + dataforce = self.interface.get_force_plate_data(packet=packet) + + current_fz = np.mean(dataforce[2]) + + if not self.sending_started and detect_start(self.previous_fz, current_fz, self.threshold): + self.sending_started = True + print("Démarrage de l'envoi des données.") + + elif self.sending_started: + connection, message = self.server.client_listening() # Non-bloquant + if connection: + dataAll = { + "Force": dataforce, + "Markers": mark_tmp, + "MarkersNames": self.mks_name + } + # "Angle": Q[:, -1], + self.server.send_data(dataAll, connection, message) + + # Mettre à jour la valeur précédente de Fz + self.previous_fz = current_fz + + # time laps + loop_time = asyncio.get_event_loop().time() - tic + real_time_to_sleep = (1/100) - loop_time + if real_time_to_sleep > 0: + await asyncio.sleep(real_time_to_sleep) + + + +if __name__ == "__main__": + processor = RealTimeDataProcessor() + # processor.process_data() + asyncio.run(processor.setup()) + """" + loop = asyncio.get_event_loop() + asyncio.ensure_future(setup()) + loop.run_forever() + """ \ No newline at end of file