diff --git a/biosiglive/__init__.py b/biosiglive/__init__.py index f14b3fb..a9e55e3 100644 --- a/biosiglive/__init__.py +++ b/biosiglive/__init__.py @@ -2,6 +2,7 @@ from .interfaces.pytrigno_interface import PytrignoClient from .interfaces.vicon_interface import ViconClient +from .interfaces.qualisys_interface import QualisysClient from .interfaces.generic_interface import GenericInterface from .interfaces.tcp_interface import TcpClient from .interfaces.param import Param, Device, MarkerSet diff --git a/biosiglive/enums.py b/biosiglive/enums.py index 5c103fa..d12ffb7 100644 --- a/biosiglive/enums.py +++ b/biosiglive/enums.py @@ -13,7 +13,7 @@ class InterfaceType(Enum): PytrignoClient = "pytrigno_client" TcpClient = "tcp_client" Custom = "custom" - + QualisysClient = "qualisys_client" class DeviceType(Enum): """ diff --git a/biosiglive/interfaces/qualisys_interface.py b/biosiglive/interfaces/qualisys_interface.py new file mode 100644 index 0000000..fa7e4f4 --- /dev/null +++ b/biosiglive/interfaces/qualisys_interface.py @@ -0,0 +1,523 @@ +""" +This file contains a wrapper for the python qualisys SDK. +""" +from .param import * +from typing import Union +from .generic_interface import GenericInterface +from ..enums import InverseKinematicsMethods, InterfaceType +import xml.etree.ElementTree as ET +import numpy as np + +try: + import asyncio + import qtm_rt +except ModuleNotFoundError: + pass + + +class QualisysClient(GenericInterface): + """ + Class for interfacing with the Qualisys system. + """ + + def __init__(self, system_rate: int, ip: str = "192.168.254.1", port: int = 22224, init_now=True): + """ + Initialize the QualisysClient class. + + Parameters + ---------- + system_rate: int + Streaming rate of the Qualisys software. + ip: str + IP address of the Qualisys software. + port: int + Port of the Qualisys software. + init_now: bool + Whether to initialize the client now. + Usefull if you want to pickle the interface as the qualisys SDK is not pickable (swig). + """ + super(QualisysClient, self).__init__(ip=ip, system_rate=system_rate, interface_type=InterfaceType.QualisysClient) + self.address = ip + self.port = port + self.qualisys_client = None + self.acquisition_rate = None + self.system_rate = system_rate + self.devices = [] + self.forces = [] + self.imu = [] + self.marker_sets = [] + self.force_plates = [] + self.is_frame = False + self.is_initialized = False + self.component = [] + if init_now: + asyncio.run(self._init_client()) + + + async def _init_client(self): + """ + Initialize the qualisys client. + + """ + print(f"Connection to Qualisys DataStreamSDK at : {self.address} ...") + self.Connect = await qtm_rt.connect("192.168.254.1") + if self.Connect is None: + print("Error","Failed to connect") + return + + print("Connected to Qualisys") + self.is_initialized = True + self.qualisys_client = True + + xml_general = await self.Connect.get_parameters(parameters=["general"]) + generalinfo = ET.fromstring(xml_general) + frame_rate = int(generalinfo.find('.//Frequency').text) + if self.system_rate != frame_rate: + raise ValueError( + f"qualisys system rate ({frame_rate}) does not match the system rate " + f"({self.system_rate})." + ) + + @classmethod + async def create(cls, system_rate=100, ip="192.168.254.1", port=22224): + self = cls(system_rate, ip, port, init_now=False) + await self._init_client() + return self + + async def add_device( + self, + nb_channels: int, + device_type: Union[DeviceType, str] = DeviceType.ForcePlate, + data_buffer_size: int = None, + name: str = None, + rate: float = 2000, + device_range: tuple = None, + processing_method: Union[RealTimeProcessingMethod, OfflineProcessingMethod] = None, + **process_kwargs, + ): + """ + Add a device to the qualisys system. + + Parameters + ---------- + nb_channels: int + Number of channels of the device. + device_type: Union[DeviceType, str] + Type of the device. + data_buffer_size: int + Size of the buffer for the device. + name: str + Name of the device. + rate: float + Rate of the device. + device_range: tuple + Range of the device. + processing_method : Union[RealTimeProcessingMethod, OfflineProcessingMethod] + Method used to process the data. + **process_kwargs + Keyword arguments for the processing method. + """ + device_tmp = self._add_device( + nb_channels, device_type, name, rate, device_range, processing_method, **process_kwargs + ) + device_tmp.interface = self.interface_type + if self.qualisys_client: + if DeviceType.ForcePlate: + self.component.append('Force') + xml_force = await self.Connect.get_parameters(parameters=["force"]) + ForceInfo = ET.fromstring(xml_force) + device_tmp.infos = ForceInfo + device_tmp.data_windows = data_buffer_size + device_tmp.name = [label.find('Name').text for label in ForceInfo.findall(".//Plate")] + #device_tmp.infos.unit = ForceInfo.find('.//Unit_Force').text + device_tmp.rate = ForceInfo.find('.//Frequency').text + self.forces.append(device_tmp) + else: + self.component.append('Analog') + xml_analog = await self.Connect.get_parameters(parameters=["analog"]) + device_tmp.infos = ET.fromstring(xml_analog) + device_tmp.data_windows = data_buffer_size + self.devices.append(device_tmp) + else: + device_tmp.infos = None + device_tmp.data_windows = data_buffer_size + self.devices.append(device_tmp) + + """ + async def add_forceplate( + self, + data_buffer_size: int = None, + name: str = None, + rate: float = 1000, + unit: str = "N", + force_range: tuple = None, + processing_method: Union[RealTimeProcessingMethod, OfflineProcessingMethod] = None, + **process_kwargs, + ): + """ + """ + Add a device to the qualisys system. + + Parameters + ---------- + nb_channels: int + Number of channels of the device. + device_type: Union[DeviceType, str] + Type of the device. + data_buffer_size: int + Size of the buffer for the device. + name: str + Name of the device. + rate: float + Rate of the device. + device_range: tuple + Range of the device. + processing_method : Union[RealTimeProcessingMethod, OfflineProcessingMethod] + Method used to process the data. + **process_kwargs + Keyword arguments for the processing method. + """ + """ + forceplate_tmp = self._add_forceplate( + nb_forceplate=nb_forceplate, + name=name, + marker_names=plate_names, + rate=rate, + **kin_method_kwargs, + ) + + forceplate_tmp.interface = self.interface_type + if self.qualisys_client: + xml_force = await self.Connect.get_parameters(parameters=["force"]) + forceplate_tmp.infos = ET.fromstring(xml_force) + forceplate_tmp.unit_forceplate = ForceInfo.find('.//Unit_Force').text + forceplate_tmp.forces_names = [label.find('Name').text for label in ForceInfo.findall(".//Plate")] + self.component.append('force') + else: + forceplate_tmp.infos = None + forceplate_tmp.forces_names = name + + forceplate_tmp.data_windows = data_buffer_size + self.forces.append(forceplate_tmp) + """ + + async def add_marker_set( + self, + nb_markers: int, + name: str = None, + data_buffer_size: int = None, + marker_names: Union[str, list] = None, + rate: float = 100, + unlabeled: bool = False, + subject_name: str = None, + kinematics_method: InverseKinematicsMethods = None, + **kin_method_kwargs, + ): + """ + Add markers set to stream from the qualisys system. + + Parameters + ---------- + nb_markers: int + Number of markers. + name: str + Name of the markers set. + data_buffer_size: int + Size of the buffer for the markers set. + marker_names: Union[list, str] + List of markers names. + rate: int + Rate of the markers set. + unlabeled: bool + Whether the markers set is unlabeled. + subject_name: str + Name of the subject. If None, the subject will be the first one in Nexus. + kinematics_method: InverseKinematicsMethods + Method used to compute the kinematics. + **kin_method_kwargs + Keyword arguments for the kinematics method. + """ + if len(self.marker_sets) != 0: + raise ValueError("Only one marker set can be added for now.") + + markers_tmp = self._add_marker_set( + nb_markers=nb_markers, + name=name, + marker_names=marker_names, + rate=rate, + unlabeled=unlabeled, + kinematics_method=kinematics_method, + **kin_method_kwargs, + ) + if self.qualisys_client: + markers_tmp.subject_name = subject_name #a changer quand je saurais comment recuperer nom du sujet avec qualisys + xlm_mks = await self.Connect.get_parameters(parameters=['3d']) + MksInfo = ET.fromstring(xlm_mks) + markers_tmp.marker_names =[label.find('Name').text for label in MksInfo.findall(".//Label")] + self.marker_names=markers_tmp.marker_names + self.component.append('3d') + self.component.append('3dnolabels') + else: + markers_tmp.subject_name = subject_name + markers_tmp.marker_names = marker_names + markers_tmp.data_windows = data_buffer_size + self.marker_sets.append(markers_tmp) + + async def get_force_plate_data( + self, forceplate_name: Union[str, list] = "all", get_frame: bool = True, packet=None + ): + if len(self.forces) == 0: + raise ValueError("No force has been added to the qualisys system.") + if not self.is_initialized: + raise RuntimeError("Qualisys client is not initialized.") + if get_frame: + packet.framenumber + + headerf, forcesdata = packet.get_force() + all_forces_data=[] + # Initialisation des données dynamiques + nb_pf = len(forcesdata) # Nombre de plaques de force + collected_data = [] # Liste dynamique pour collecter les données valides + PFForce = forcesdata[0][1] # Données pour une plaque + + + nb_frames = max(len(forcesdata[0][1]), len(forcesdata[1][1])) # Nombre de frames pour cette plaque + # Collecte des données + for platenum in range(nb_pf): + PFForce = forcesdata[platenum][1] + # Temporaire pour cette plaque + plate_data =np.empty((9, nb_frames)) + plate_data[:]=np.nan + for frame_idx, data_tmp in enumerate(PFForce): + # Récupérer les données et remplir la matrice temporaire + plate_data[:, frame_idx] = [ + 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 + ] + + collected_data.append(plate_data) + + # Concaténation des données valides uniquement pour obtenir [9 * nb_pf, nb_frame] + all_forces_data = np.concatenate(collected_data, axis=0) + + return all_forces_data + + + def get_device_data( + self, device_name: Union[str, list] = "all", channel_idx: Union[int, list] = (), get_frame: bool = True + ): + """ + Get the device data from qualisys. + + Parameters + ---------- + device_name: str or list + Name of the device or list of devices names. + channel_idx: Union[int, str] + Index of the channel to return. + get_frame: bool + Whether to get a new frame from the qualisys system. + + Returns + ------- + device_data: list + All asked device data. + """ + if len(self.devices) == 0: + raise ValueError("No device has been added to the qualisys system.") + if not self.is_initialized: + raise RuntimeError("Qualisys client is not initialized.") + if get_frame: + self.packet.framenumber + all_device_data = [] + if not isinstance(device_name, list): + device_name = [device_name] + if channel_idx and not isinstance(channel_idx, list): + channel_idx = [channel_idx] + device_data = [] + headerdevice, Devicevalue = self.packet.get_analog() + for d, device in enumerate(self.devices): + if device_name[0] == "all" or device.name in device_name: + device.new_data = np.zeros((device.nb_channels, device.sample)) + count = 0 + for output_name, channel_name, unit in device.infos: + data_tmp, _ = self.packet.get_analog(device.name, output_name, channel_name) + device.new_data[count, :] = data_tmp + device.channel_names.append(channel_name) + count += 1 + if count == device.nb_channels: + break + if channel_idx: + device_data = np.zeros((len(channel_idx), device.sample)) + for idx in range(device.nb_channels): + if idx in channel_idx: + device_data[channel_idx.index(idx), :] = device.new_data[idx, :] + device_data = device_data if channel_idx else device.new_data + device.append_data(device.new_data) + all_device_data.append(device_data) + if len(all_device_data) == 1: + return all_device_data[0] + return all_device_data + + async def get_marker_set_data( + self, subject_name: Union[str, list] = None, marker_names: Union[str, list] = None, get_frame: bool = True, packet = None + ): + """ + Get the markers data from qualisys. + + Parameters + ---------- + subject_name: Union[str, list] + Name of the subject. If None, the subject will be the first one in Nexus. + marker_names: Union[str, list] + List of markers names. + get_frame: bool + Whether to get a new frame or not. + + Returns + ------- + markers_data: list + All asked markers data. + """ + + if len(self.marker_sets) == 0: + raise ValueError("No marker set has been added to the qualisys system.") + if not self.is_initialized: + raise RuntimeError("qualisys client is not initialized.") + if get_frame: + packet.framenumber + if subject_name and isinstance(subject_name, list): + subject_name = [subject_name] + if marker_names and isinstance(marker_names, list): + marker_names = [marker_names] + #occluded = [] + all_markers_data = [] + #all_occluded_data = [] + if subject_name: + marker_sets = [None] * len(subject_name) + for s, marker_set in enumerate(self.marker_sets): + if marker_set.subject_name in subject_name: + marker_sets[subject_name.index(marker_set.subject_name)] = marker_set + if marker_sets == [None]: + raise RuntimeError("No subject of this name.") + else: + marker_sets = self.marker_sets + + for markers in marker_sets: + markers.new_data = np.zeros((3, len(markers.marker_names), markers.sample)) + count = 0 + header, allmarkers_data_tmp= packet.get_3d_markers() #TO check + #headernolabel, allmarkersnolabel_data_tmp= packet.get_3d_markers_no_label + for m, marker_name in enumerate(markers.marker_names): + markers_data_tmp=allmarkers_data_tmp[m][:] + markers.new_data[:, m, :] = np.array(markers_data_tmp)[:, np.newaxis] + #occluded.append(occluded_tmp) + + all_markers_data.append(markers.new_data) + markers.append_data(markers.new_data) + #all_occluded_data.append(occluded) + if len(all_markers_data) == 1: + #print(packet.framenumber) + #print(packet.timestamp) + return all_markers_data[0], self.marker_names #, all_occluded_data[0] + return all_markers_data.marker_names #, all_occluded_data + + async def init_client(self): + """ + Initialize the Qualisys client if it is not already initialized. + This function has to be called before get frame from interface. + """ + if self.is_initialized: + raise RuntimeError("Qualisys client is already initialized.") + else: + self._init_client() + xlm_analog = await self.Connect.get_parameters(parameters=['analog']) + AnalogInfo = ET.fromstring(xlm_analog) + for d, device in enumerate(self.devices): + if not device.infos: + device.infos = self.qualisys_client.GetDeviceOutputDetails(device.name) #a changer + + xlm_3d = await self.Connect.get_parameters(parameters=['3d']) + MksInfo = ET.fromstring(xlm_3d) + nom_mks = [label.find('Name').text for label in MksInfo.findall(".//Label")] + for m, marker_set in enumerate(self.marker_sets): + if not marker_set.markers_names: + marker_set.markers_names = [label.find('Name').text for label in MksInfo.findall(".//Label")] + if not marker_set.subject_name: + marker_set.subject_name = [label.find('Name').text for label in MksInfo.findall(".//Label")] + + async def get_latency(self) -> float: + """ + Get the latency between the qualisys system and the qualisys SDK. + + Returns + ------- + latency: float + Latency between the qualisys system and the qualisys SDK. + """ + if not self.is_initialized: + raise RuntimeError("qualisys client is not initialized.") + self.qualisys_client = 0 #voir comment obtenir la latence + return self.qualisys_client + + async def get_frame(self) -> bool: + """ + Get a new frame from the qualisys system. + + Returns + ------- + bool + True if there is a frame, False otherwise. + """ + """ + if not self.is_initialized: + raise RuntimeError("qualisys client is not initialized.") + self.is_frame = self.qualisys_client.GetFrame() + while self.is_frame is not True: + self.is_frame = self.qualisys_client.GetFrame() + return self.is_frame + """ #TODO + + async def get_frame_number(self) -> int: + """ + Get the last frame number. + + Returns + ------- + frame_number: int + Last frame number. + """ + if not self.is_initialized: + raise RuntimeError("qualisys client is not initialized.") + return self.packet.framenumber + + async def get_kinematics_from_markers( + self, + marker_set_name: str, + model_path: str = None, + method: Union[InverseKinematicsMethods, str] = InverseKinematicsMethods.BiorbdLeastSquare, + custom_func: callable = None, + **kwargs, + ): + """ + Get the kinematics from markers. + + Parameters + ---------- + marker_set_name: str + name of the markerset. + model_path: str + biorbd model of the kinematics. + method: str + Method to use to get the kinematics. Can be "kalman" or "custom". + custom_func: function + Custom function to get the kinematics. + + Returns + ------- + kinematics: list + List of kinematics. + """ + marker_set_idx = [i for i, m in enumerate(self.marker_sets) if m.name == marker_set_name][0] + return self.marker_sets[marker_set_idx].get_kinematics(model_path, method, custom_func=custom_func, **kwargs) diff --git a/examples/CompuServeetQual.py b/examples/CompuServeetQual.py new file mode 100644 index 0000000..a0e7973 --- /dev/null +++ b/examples/CompuServeetQual.py @@ -0,0 +1,109 @@ +import asyncio +from biosiglive import load, RealTimeProcessingMethod, InterfaceType, DeviceType, Server, InverseKinematicsMethods +import numpy as np +import time +from biosiglive import QualisysClient + + +# 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, data_path="example\\walkAll_LAO01_Cond10.bio", + model_path="example\\LAO.bioMod", + threshold=30, system_rate=100, device_rate=2000, nb_markers=53, nb_seconds=1): + # Initialisation du serveur + self.server = Server(server_ip, port) + self.server.start() + + # Paramètres + self.threshold = threshold + self.system_rate = system_rate + self.device_rate = device_rate + self.nb_markers = nb_markers + self.nb_seconds = nb_seconds + + # Variables d'état + self.sending_started = False + self.previous_fz = 0 # Valeur initiale de Fz + + # Chargement des noms des marqueurs + #self.mks_name = self.load_marker_names() + + def load_marker_names(self): + # Chargement des noms des marqueurs à partir du fichier + tmp = load("walkAll_LAO01_Cond10.bio") + return tmp['markers_names'].data[0:self.nb_markers].tolist() + return tmp['markers_names'].data[0:self.nb_markers].tolist() + + async def setup_interface(self): + self.interface = await QualisysClient.create(ip="192.168.254.1", system_rate=100, port=22224) + + # Configuration du jeu de marqueurs + + await self.interface.add_marker_set( + nb_markers=self.nb_markers, + data_buffer_size=1000, + marker_data_file_key="markers", + name="markers", + rate=self.system_rate, + unit="mm" + ) + + await self.interface.add_device( + nb_channels=18, + device_type="force_plate", + name="force_plate", + data_buffer_size=20000, + rate=2000, + device_data_file_key="force_plate", + processing_method=None, + moving_average=True, + ) + + async def process_data(self): + await self.setup_interface() + + while True: + tic = asyncio.get_event_loop().time() + packet = await self.interface.Connect.get_current_frame(components=self.interface.component) + + # data recuperation + mark_tmp = await self.interface.get_marker_set_data(packet=packet) + dataforce = await self.interface.get_force_plate_data(packet=packet) + + # Calcul de la force verticale moyenne actuelle + if dataforce is not []: + current_fz = np.nanmean(dataforce[2]) + current_fz2 = np.nanmean(dataforce[11]) + print(current_fz, current_fz2) + 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, + } + self.server.send_data(dataAll, connection, message) + + # Mettre à jour la valeur précédente de Fz + self.previous_fz = current_fz + + loop_time = time.perf_counter() - tic + real_time_to_sleep = max(0, (1 / self.system_rate) - loop_time) + if real_time_to_sleep > 0: + await asyncio.sleep(real_time_to_sleep) + + + + +if __name__ == "__main__": + processor = RealTimeDataProcessor() + asyncio.run(processor.process_data()) \ No newline at end of file diff --git a/examples/sandox/Example_P24.py b/examples/sandox/Example_P24.py new file mode 100644 index 0000000..32bb7a9 --- /dev/null +++ b/examples/sandox/Example_P24.py @@ -0,0 +1,22 @@ +import time +from pyScienceMode import Channel, Point, Device, Modes +from pyScienceMode import RehastimP24 as St + +""" +This example shows how to use the RehastimP24 device. +There are several commands divided into three levels (general, low and mid). +You can't call commands from different levels, you must first close the current level +to be able to use commands from another one. +""" +list_channels = [] +# Create all channels possible + +channel_1 = Channel("Single", no_channel=1, amplitude=8, pulse_width=250, frequency=25, name="Gastro",device_type=Device.Rehastimp24) +stimulator = St(port="COM5") +list_channels.append(channel_1) +stimulator.init_stimulation(list_channels=list_channels) + +stimulator.start_stimulation(upd_list_channels = list_channels, safety = True, stimulation_duration = 0.1) + + +stimulator.close_port() diff --git a/examples/sandox/Getcurrentframe.py b/examples/sandox/Getcurrentframe.py new file mode 100644 index 0000000..fa618b2 --- /dev/null +++ b/examples/sandox/Getcurrentframe.py @@ -0,0 +1,67 @@ +""" Example that takes control of QTM, streams data etc """ + +import asyncio +import logging +import xml.etree.ElementTree as ET +import qtm_rt +import time +import numpy as np +from biosiglive import LivePlot, PlotType, QualisysClient + +LOG = logging.getLogger("example") + + +async def setup(): + """ main function """ + + connection = await qtm_rt.connect("192.168.254.1") + + if connection is None: + return -1 + + + # Plot initialisation + marker_plot = LivePlot(name="markers", plot_type=PlotType.Scatter3D) + marker_plot.init() + + + all_mks_data = np.zeros((3, 4, 1)) + time_to_sleep = 1 / 100 + + while True: + tic = asyncio.get_event_loop().time() + packet = await connection.get_current_frame(components=['3d']) + headermks, mks = packet.get_3d_markers() + if mks is None: + break + + LOG.info("Framenumber %s", packet.framenumber) + + + # get_marker_data + + LOG.info("Component info: %s", headermks) + mks_currentframe = np.zeros((3, headermks.marker_count, 1)) + + for i, mks in enumerate(mks, 1): + LOG.info("Marqueur %d", i) + j = 0 + for marker in mks: + LOG.info("\t%s", marker) + mks_currentframe[j, i - 1, :] = marker / 1000 + j = j + 1 + + all_mks_data = np.append(all_mks_data, mks_currentframe, axis=2) + + + # plot actualisation + marker_plot.update(all_mks_data[:, :, -1].T, size=0.1) + loop_time = asyncio.get_event_loop().time() - tic + real_time_to_sleep = time_to_sleep - loop_time + if real_time_to_sleep > 0: + await asyncio.sleep(real_time_to_sleep) + + + +if __name__ == "__main__": + asyncio.run(setup()) diff --git a/examples/sandox/IK_Biosiglive.py b/examples/sandox/IK_Biosiglive.py new file mode 100644 index 0000000..0503511 --- /dev/null +++ b/examples/sandox/IK_Biosiglive.py @@ -0,0 +1,121 @@ +from biosiglive import QualisysClient +from biosiglive import load, RealTimeProcessingMethod, InterfaceType, DeviceType, Server, InverseKinematicsMethods +import numpy as np +import time +import asyncio + + +# Fonction pour détecter si Fz dépasse le seuil +async 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=50000, + model_path="example\\LAO.bioMod", + 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() + + # Initialisation de l'interface + + + self.model_path = model_path + + # Paramètres + self.threshold = threshold + self.system_rate = system_rate + self.device_rate = device_rate + self.nb_markers = nb_markers + self.nb_seconds = nb_seconds + + # Variables d'état + self.sending_started = False + self.previous_fz = 0 # Valeur initiale de Fz + + # Chargement des noms des marqueurs + #self.mks_name = self.load_marker_names() + + # Configuration de l'interface + self.setup_interface() + + + async def setup_interface(self): + # Configuration du jeu de marqueurs + await self.interface.add_marker_set( + nb_markers=self.nb_markers, + data_buffer_size=self.system_rate * self.nb_seconds, + processing_window=self.system_rate * self.nb_seconds, + marker_data_file_key="markers", + name="markers", + rate=self.system_rate, + kinematics_method=InverseKinematicsMethods.BiorbdKalman, + model_path=self.model_path, + unit="mm", + ) + + # Configuration du dispositif (tapis roulant) + await self.interface.add_device( + 18, + name="Treadmill", + device_type=DeviceType.Generic, + rate=self.device_rate, + data_buffer_size=int(self.device_rate * self.nb_seconds), + processing_window=int(self.device_rate * self.nb_seconds), + device_data_file_key="treadmill", + ) + + async def process_data(self): + self.interface = await QualisysClient.create(ip="192.168.254.1", system_rate=100, port=22224) + queue = asyncio.Queue() + try: + while True: + tic = time.perf_counter() + packet = await self.interface.Connect.get_current_frame(components=self.interface.component) + + dataforce = await self.interface.get_force_plate_data(packet=packet) + #Q, _, mark_tmp = self.interface.get_kinematics_from_markers(marker_set_name="markers", get_markers_data=True) + + # data recuperation + mark_tmp = interface.get_marker_set_data(packet=packet) + + # Calcul de la force verticale moyenne actuelle + 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 + loop_time = time.perf_counter() - tic + real_time_to_sleep = max(0, (1/self.system_rate) - loop_time) + if real_time_to_sleep > 0: + await asyncio.sleep(real_time_to_sleep) + + except KeyboardInterrupt: + print("Arrêt manuel du programme.") + except Exception as e: + print(f"Erreur rencontrée : {e}") + finally: + #self.server.stop() + print("Serveur arrêté proprement.") + + +if __name__ == "__main__": + processor = RealTimeDataProcessor() + #processor.process_data() + asyncio.run(processor.process_data()) \ No newline at end of file diff --git a/examples/sandox/TMP.py b/examples/sandox/TMP.py new file mode 100644 index 0000000..4c6596c --- /dev/null +++ b/examples/sandox/TMP.py @@ -0,0 +1,20 @@ + +#for d, device in enumerate(self.devices): +for platenum in range(len(forcesdata)): + PFForce = forcesdata[platenum][1] + device.new_data = np.zeros((9, len(PFForce))) + data_tmp = np.array(PFForce) + data_tmp = data_tmp.T + count = 0 + for output_name, channel_name, unit in device.infos: + device.new_data[count, :] = data_tmp[count] + device.channel_names.append(channel_name) + count += 1 + if count == device.nb_channels: + break + device_data = device_data if channel_idx else device.new_data + device.append_data(device.new_data) + all_device_data.append(device_data) +if len(all_device_data) == 1: + return all_device_data[0] +return all_device_data \ No newline at end of file diff --git a/examples/sandox/TestQTM.py b/examples/sandox/TestQTM.py new file mode 100644 index 0000000..f12b59f --- /dev/null +++ b/examples/sandox/TestQTM.py @@ -0,0 +1,163 @@ +""" Example that takes control of QTM, streams data etc """ + +import asyncio +import logging +import xml.etree.ElementTree as ET +import qtm_rt +import time +import numpy as np +from biosiglive import LivePlot, PlotType, QualisysClient +LOG = logging.getLogger("example") + +""" +async def package_receiver(queue): + Asynchronous function that processes queue until None is posted in queue + LOG.info("Entering package_receiver") + while True: + packet = await queue.get() + if packet is None: + break + + LOG.info("Framenumber %s", packet.framenumber) + + + latency = (packet.timestamp) - time.time() + LOG.info("Latency: %s", latency) + + + headermks, mks = packet.get_3d_markers() + LOG.info("Component info: %s", headermks) + for i, mks in enumerate(mks, 1): + LOG.info("Marqueur %d", i) + for marker in mks: + LOG.info("\t%s", marker) + + headerf, forces = packet.get_force() + LOG.info("Component info: %s", headerf) + for i, forces in enumerate(forces, 1): + LOG.info("Force %d", i) + for force in forces: + LOG.info("\t%s", force) + + marker_plot.update(mks[:, :, -1].T, size=0.1) + + LOG.info("Exiting package_receiver") +""" + + +async def setup(): + """ main function """ + + connection = await qtm_rt.connect("192.168.254.1") + + if connection is None: + return -1 + + #General information (~add_markerset & add_forceplate) + xlm_general = await connection.get_parameters(parameters=['general']) + GeneralInfo = ET.fromstring(xlm_general) + Fs = int(GeneralInfo.find('.//Frequency').text) + + xlm_3d = await connection.get_parameters(parameters=['3d']) + MksInfo = ET.fromstring(xlm_3d) + nb_mks = MksInfo.find('.//Labels').text + nom_mks = [label.find('Name').text for label in MksInfo.findall(".//Label")] + print(Fs) + print(nom_mks) + + xlm_force = await connection.get_parameters(parameters=['force']) + ForceInfo = ET.fromstring(xlm_force) + unit_forceplate = ForceInfo.find('.//Unit_Force').text + nom_forceplate = [label.find('Name').text for label in ForceInfo.findall(".//Plate")] + print(nom_forceplate) + print(unit_forceplate) + + # Plot initialisation + marker_plot = LivePlot(name="markers", plot_type=PlotType.Scatter3D) + marker_plot.init() + """ + force1_plot = LivePlot( + name="force", rate=1000, plot_type=PlotType.Curve, nb_subplots=3 + ) + force1_plot.init(plot_windows=1000, y_labels="Force (N)") + + force2_plot = LivePlot( + name="force", rate=1000, plot_type=PlotType.Curve, nb_subplots=3 + ) + force2_plot.init(plot_windows=1000, y_labels="Force (N)") + """ + #Data extraction + queue = asyncio.Queue() + + #receiver_future = asyncio.ensure_future(package_receiver(queue)) + + await connection.stream_frames(components=["6d", "3d", "force"], on_packet=queue.put_nowait) + + all_forcedata = np.zeros((3, 2, 1)) + all_mks_data = np.zeros((3, 4, 1)) + time_to_sleep = 1 / 200 + while True: + tic=asyncio.get_event_loop().time() + packet = await queue.get() + if packet is None: + break + + LOG.info("Framenumber %s", packet.framenumber) + + """ + latency = (packet.timestamp) - time.time() + LOG.info("Latency: %s", latency) + """ + #get_marker_data + headermks, mks = packet.get_3d_markers() + LOG.info("Component info: %s", headermks) + mks_currentframe = np.zeros((3, headermks.marker_count, 1)) + + for i, mks in enumerate(mks, 1): + LOG.info("Marqueur %d", i) + j=0 + for marker in mks: + LOG.info("\t%s", marker) + mks_currentframe[j, i - 1, :] = marker/1000 + j=j+1 + + all_mks_data = np.append(all_mks_data, mks_currentframe, axis=2) + + """ + # get_force_data + headerf, forces = packet.get_force() + LOG.info("Component info: %s", headerf) + force_currentframe = np.zeros((9, headerf.plate_count, 1)) + for i, forces in enumerate(forces, 1): + LOG.info("Force %d", i) + for force in forces: + LOG.info("\t%s", force) + + forcedata = forces[i-1][1][1] + forces_data_tmp = [forcedata.x, forcedata.y, forcedata.z, + forcedata.x_m, forcedata.y_m, forcedata.z_m, + forcedata.x_a, forcedata.y_a, forcedata.z_a] + force_currentframe[:, i-1,:] = np.array(forces_data_tmp)[:, np.newaxis] + + all_forcedata = np.append(all_forcedata, force_currentframe, axis=2) + """ + #plot actualisation + marker_plot.update(all_mks_data[:, :, -1].T, size=0.1) + loop_time = asyncio.get_event_loop().time() - tic + real_time_to_sleep = time_to_sleep - loop_time + if real_time_to_sleep > 0: + await asyncio.sleep(real_time_to_sleep) + """ + force1_plot.update(np.array(all_forcedata[0:3, 0, -1:])) + force2_plot.update(np.array(all_forcedata[0:3, 1, -1:])) + """ +# asyncio.ensure_future(shutdown(30, connection, receiver_future, queue)) + + +if __name__ == "__main__": + asyncio.run(setup()) + """ + loop = asyncio.get_event_loop() + asyncio.ensure_future(setup()) + loop.run_forever() + """ \ No newline at end of file diff --git a/examples/sandox/Test_QualisysClient.py b/examples/sandox/Test_QualisysClient.py new file mode 100644 index 0000000..a514d5f --- /dev/null +++ b/examples/sandox/Test_QualisysClient.py @@ -0,0 +1,108 @@ +""" +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) +import asyncio +import logging +import xml.etree.ElementTree as ET +import qtm_rt +import numpy as np +from collections import deque + + +async def setup(): + """ main function """ + # Connection to qualisys + interface = await QualisysClient.create(ip="192.168.0.2", system_rate=100, port=22224) + queue = asyncio.Queue() + + # Add info needed + n_markers = 4 + await interface.add_marker_set( + nb_markers=n_markers, data_buffer_size=1000, marker_data_file_key="markers", name="markers", rate=100, unit="mm" + ) + await 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, + ) + + + # Plot init + marker_plot = LivePlot(name="markers", plot_type=PlotType.Scatter3D) + marker_plot.init() + force1_plot = LivePlot( + name="force", rate=1000, plot_type=PlotType.Curve, nb_subplots=3 + ) + force1_plot.init(plot_windows=10000, y_labels="Force (N)") + force2_plot = LivePlot( + name="force", rate=1000, plot_type=PlotType.Curve, nb_subplots=3 + ) + force2_plot.init(plot_windows=10000, y_labels="Force (N)") + + + time_to_sleep = 1 / 200 + mark_all = deque(maxlen=1000) + offline_count = 0 + mark_to_plot = [] + mark_all=np.zeros((3, 4, 1)) + + + while True: + tic = asyncio.get_event_loop().time() + packet = await interface.Connect.get_current_frame(components=interface.component) + + #data recuperation + mark_tmp = interface.get_marker_set_data(packet=packet) + mark_tmp = mark_tmp / 1000 + mark_all = np.append(mark_all,mark_tmp, axis=2) + + force_tmp = interface.get_force_plate_data(packet=packet) + + # data plot + marker_plot.update(mark_tmp[:, :, -1].T, size=0.1) + + if (len(force_tmp)) != 0: + force1_plot.update(force_tmp[0:3, -1:]) + force2_plot.update(force_tmp[9:12, -1:]) + + # time laps + loop_time = asyncio.get_event_loop().time() - tic + real_time_to_sleep = time_to_sleep - loop_time + if real_time_to_sleep > 0: + await asyncio.sleep(real_time_to_sleep) + + + +if __name__ == "__main__": + asyncio.run(setup()) + """" + loop = asyncio.get_event_loop() + asyncio.ensure_future(setup()) + loop.run_forever() + """ \ No newline at end of file diff --git a/examples/sandox/WalkStim_FapPositif.py b/examples/sandox/WalkStim_FapPositif.py new file mode 100644 index 0000000..12d7bb0 --- /dev/null +++ b/examples/sandox/WalkStim_FapPositif.py @@ -0,0 +1,87 @@ +from examples.custom_interface import MyInterface +from biosiglive import RealTimeProcessingMethod, InterfaceType, DeviceType, LivePlot, PlotType +from biosiglive.interfaces.qualisys_interface import QualisysClient +# Import Stimulator class +from pyScienceMode import Channel, Device +from pyScienceMode import RehastimP24 as St +import qtm_rt +import numpy as np +import multiprocessing as mp +from time import time, sleep +import asyncio +import logging +import xml.etree.ElementTree as ET +from collections import deque + +async def stim(): + interface = None + plot_curve = LivePlot( + name="curve", + plot_type=PlotType.Curve, + nb_subplots=2, + channel_names=["1", "2"], + ) + + plot_curve.init(plot_windows=1000, y_labels=["Strikes", "Force (N)"]) + interface_type = InterfaceType.QualisysClient + if interface_type == InterfaceType.Custom: + interface = MyInterface(system_rate=100, data_path="walk.bio") + elif interface_type == InterfaceType.QualisysClient: + interface = await QualisysClient.create(ip="192.168.254.1", system_rate=100, port=22224) + + nb_second = 10 + await 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, + ) + list_channels = [] + # Create all channels possible + channel_1 = Channel( + "Single", no_channel=1, amplitude=20, pulse_width=250, frequency=25, name="Gastro", + device_type=Device.Rehastimp24 + ) + stimulator = St(port="COM5") + list_channels.append(channel_1) + stimulator.init_stimulation(list_channels=list_channels) + time_to_sleep = 0.001 + sign = 1 + while True: + tic = time() + packet = await interface.Connect.get_current_frame(components=interface.component) + data = interface.get_force_plate_data(packet=packet) + if len(data) != 0: + force_ap_tmp = [data[0, 1, -10:]] + plot_curve.update(np.append(force_ap_tmp, force_ap_tmp, axis=0)) + if np.mean(force_ap_tmp) < -10: + new_sign = 0 + else: + new_sign = 1 + + if new_sign == 0 and sign != 0: + stimulator.start_stimulation(upd_list_channels=list_channels, safety=True, stimulation_duration=0.2) + print('send stim') + elif new_sign == 1 and sign != 1: + stimulator.pause_stimulation() + + sign = new_sign + loop_time = time() - tic + real_time_to_sleep = time_to_sleep - loop_time + if real_time_to_sleep > 0: + sleep(time_to_sleep - loop_time) + + + + + +if __name__ == "__main__": + asyncio.run(stim()) + + print("All processes complete.") + stimulator.end_stimulation() + stimulator.close_port()