From a6bde952ae29e7bf19de2aaccc186e4e29737e55 Mon Sep 17 00:00:00 2001 From: Ophelie Date: Fri, 5 Apr 2024 15:06:46 +0400 Subject: [PATCH 01/23] WIP qualisys_interface - copy from vicon --- biosiglive/interfaces/qualysis_interface.py | 393 ++++++++++++++++++++ 1 file changed, 393 insertions(+) create mode 100644 biosiglive/interfaces/qualysis_interface.py diff --git a/biosiglive/interfaces/qualysis_interface.py b/biosiglive/interfaces/qualysis_interface.py new file mode 100644 index 0000000..871a0d0 --- /dev/null +++ b/biosiglive/interfaces/qualysis_interface.py @@ -0,0 +1,393 @@ +""" +This file contains a wrapper for the python Vicon SDK. +""" +from .param import * +from typing import Union +from .generic_interface import GenericInterface +from ..enums import InverseKinematicsMethods, InterfaceType + +try: + import asyncio + import qtm_rt +except ModuleNotFoundError: + pass + + +class QualysisClient(GenericInterface): + """ + Class for interfacing with the Vicon system. + """ + + def __init__(self, system_rate: int, ip: str = "127.0.0.1", port: int = 801, init_now=True): + """ + Initialize the ViconClient class. + + Parameters + ---------- + system_rate: int + Streaming rate of the nexus software. + ip: str + IP address of the nexus software. + port: int + Port of the nexus software. + init_now: bool + Whether to initialize the client now. + Usefull if you want to pickle the interface as the Vicon SDK is not pickable (swig). + """ + super(ViconClient, self).__init__(ip=ip, system_rate=system_rate, interface_type=InterfaceType.ViconClient) + self.address = f"{ip}:{port}" + + self.vicon_client = None + self.acquisition_rate = None + self.system_rate = system_rate + self.devices = [] + self.imu = [] + self.marker_sets = [] + self.is_frame = False + self.is_initialized = False + if init_now: + self._init_client() + + def _init_client(self): + """ + Initialize the Vicon client. + """ + print(f"Connection to ViconDataStreamSDK at : {self.address} ...") + self.vicon_client = VDS.Client() + self.vicon_client.Connect(self.address) + print("Connected to Vicon.") + self.is_initialized = True + + # Enable several data types + self.vicon_client.EnableSegmentData() + self.vicon_client.EnableDeviceData() + self.vicon_client.EnableMarkerData() + self.vicon_client.EnableUnlabeledMarkerData() + self.get_frame() + if self.system_rate != self.vicon_client.GetFrameRate(): + raise ValueError( + f"Vicon system rate ({self.vicon_client.GetFrameRate()}) does not match the system rate " + f"({self.system_rate})." + ) + + def add_device( + self, + nb_channels: int, + device_type: Union[DeviceType, str] = DeviceType.Emg, + 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 Vicon 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.vicon_client: + device_tmp.infos = self.vicon_client.GetDeviceOutputDetails(name) + else: + device_tmp.infos = None + device_tmp.data_windows = data_buffer_size + self.devices.append(device_tmp) + + 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 Vicon 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.vicon_client: + markers_tmp.subject_name = subject_name if subject_name else self.vicon_client.GetSubjectNames()[0] + markers_tmp.marker_names = ( + self.vicon_client.GetMarkerNames(markers_tmp.subject_name) if not marker_names else marker_names + ) + markers_tmp.marker_names = [name[0] for name in markers_tmp.marker_names] + if markers_tmp.nb_channels != len(markers_tmp.marker_names): + raise RuntimeError("Nb of marker not the same than markers on vicon.") + 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) + + @staticmethod + def get_force_plate_data(): + raise NotImplementedError("Force plate streaming is not implemented yet.") + + 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 Vicon. + + 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 Vicon system. + + Returns + ------- + device_data: list + All asked device data. + """ + if len(self.devices) == 0: + raise ValueError("No device has been added to the Vicon system.") + if not self.is_initialized: + raise RuntimeError("Vicon client is not initialized.") + if get_frame: + self.get_frame() + 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 = [] + 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.vicon_client.GetDeviceOutputValues(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 + + def get_marker_set_data( + self, subject_name: Union[str, list] = None, marker_names: Union[str, list] = None, get_frame: bool = True + ): + """ + Get the markers data from Vicon. + + 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 Vicon system.") + if not self.is_initialized: + raise RuntimeError("Vicon client is not initialized.") + if get_frame: + self.get_frame() + 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 + for m, marker_name in enumerate(markers.marker_names): + markers_data_tmp, occluded_tmp = self.vicon_client.GetMarkerGlobalTranslation( + markers.subject_name, marker_name + ) + markers.new_data[:, count, :] = np.array(markers_data_tmp)[:, np.newaxis] + occluded.append(occluded_tmp) + if marker_names: + markers_data = np.zeros((3, len(marker_names), markers.sample)) + for n, name in enumerate(markers.marker_names): + if name in marker_names: + markers_data[:, marker_names.index(name), :] = markers.new_data[:, n, :] + all_markers_data.append(markers_data) + else: + all_markers_data.append(markers.new_data) + + markers.append_data(markers.new_data) + all_occluded_data.append(occluded) + if len(all_markers_data) == 1: + return all_markers_data[0], all_occluded_data[0] + return all_markers_data, all_occluded_data + + def init_client(self): + """ + Initialize the Vicon client if it is not already initialized. + This function has to be called before get frame from interface. + """ + if self.is_initialized: + raise RuntimeError("Vicon client is already initialized.") + else: + self._init_client() + for d, device in enumerate(self.devices): + if not device.infos: + device.infos = self.vicon_client.GetDeviceOutputDetails(device.name) + for m, marker_set in enumerate(self.marker_sets): + if not marker_set.markers_names: + marker_set.markers_names = self.vicon_client.GetMarkerNames(marker_set.subject_name) + if not marker_set.subject_name: + marker_set.subject_name = self.vicon_client.GetSubjectNames()[0] + + def get_latency(self) -> float: + """ + Get the latency between the Vicon system and the Vicon SDK. + + Returns + ------- + latency: float + Latency between the Vicon system and the Vicon SDK. + """ + if not self.is_initialized: + raise RuntimeError("Vicon client is not initialized.") + return self.vicon_client.GetLatencyTotal() + + def get_frame(self) -> bool: + """ + Get a new frame from the Vicon system. + + Returns + ------- + bool + True if there is a frame, False otherwise. + """ + if not self.is_initialized: + raise RuntimeError("Vicon client is not initialized.") + self.is_frame = self.vicon_client.GetFrame() + while self.is_frame is not True: + self.is_frame = self.vicon_client.GetFrame() + return self.is_frame + + 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("Vicon client is not initialized.") + return self.vicon_client.GetFrameNumber() + + 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) From 004a8742f53248336a80ce63e6929aa83380a572 Mon Sep 17 00:00:00 2001 From: Ophelie Date: Tue, 9 Apr 2024 11:07:37 +0400 Subject: [PATCH 02/23] Adding the Qualisys client on interface type class --- biosiglive/enums.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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): """ From d15c2b1b49bd42fd26b84ef912e22ecb8b4368ba Mon Sep 17 00:00:00 2001 From: Ophelie Date: Tue, 9 Apr 2024 11:47:09 +0400 Subject: [PATCH 03/23] set up of qualisys client, client init, add device PF debut mise en place de queue --- biosiglive/interfaces/qualysis_interface.py | 51 +++++++++++---------- 1 file changed, 26 insertions(+), 25 deletions(-) diff --git a/biosiglive/interfaces/qualysis_interface.py b/biosiglive/interfaces/qualysis_interface.py index 871a0d0..c0b6d25 100644 --- a/biosiglive/interfaces/qualysis_interface.py +++ b/biosiglive/interfaces/qualysis_interface.py @@ -15,29 +15,29 @@ class QualysisClient(GenericInterface): """ - Class for interfacing with the Vicon system. + Class for interfacing with the Qualisys system. """ - def __init__(self, system_rate: int, ip: str = "127.0.0.1", port: int = 801, init_now=True): + def __init__(self, system_rate: int, ip: str = "192.168.254.1", port: int = 22224, init_now=True): """ - Initialize the ViconClient class. + Initialize the QualisysClient class. Parameters ---------- system_rate: int - Streaming rate of the nexus software. + Streaming rate of the Qualisys software. ip: str - IP address of the nexus software. + IP address of the Qualisys software. port: int - Port of the nexus software. + Port of the Qualisys software. init_now: bool Whether to initialize the client now. Usefull if you want to pickle the interface as the Vicon SDK is not pickable (swig). """ - super(ViconClient, self).__init__(ip=ip, system_rate=system_rate, interface_type=InterfaceType.ViconClient) - self.address = f"{ip}:{port}" - - self.vicon_client = None + super(QualysisClient, self).__init__(ip=ip, system_rate=system_rate, interface_type=InterfaceType.QualysisClient) + self.address =ip + self.port= None + self.qualysis_client = None self.acquisition_rate = None self.system_rate = system_rate self.devices = [] @@ -52,28 +52,29 @@ def _init_client(self): """ Initialize the Vicon client. """ - print(f"Connection to ViconDataStreamSDK at : {self.address} ...") - self.vicon_client = VDS.Client() - self.vicon_client.Connect(self.address) - print("Connected to Vicon.") + + print(f"Connection to Qualisys DataStreamSDK at : {self.address} ...") + self.qualysis_client.Connect = await qtm_rt.connect(self.address) + if self.qualysis_client.Connect is None: + self.qualysis_client._update_state("Error","Failed to connect") + return + + print("Connected to Qualisys") self.is_initialized = True - # Enable several data types - self.vicon_client.EnableSegmentData() - self.vicon_client.EnableDeviceData() - self.vicon_client.EnableMarkerData() - self.vicon_client.EnableUnlabeledMarkerData() - self.get_frame() - if self.system_rate != self.vicon_client.GetFrameRate(): +""" pour le moment je ne trouver pas commetn avoir la Fs de Qualisys + if self.system_rate != self.qualisys_client.get_frame_rate(): raise ValueError( - f"Vicon system rate ({self.vicon_client.GetFrameRate()}) does not match the system rate " + f"Vicon system rate ({self.qualisys_client.get_frame_rate()}) does not match the system rate " f"({self.system_rate})." ) +""" + def add_device( self, nb_channels: int, - device_type: Union[DeviceType, str] = DeviceType.Emg, + device_type: Union[DeviceType, str] = DeviceType.ForcePlate, data_buffer_size: int = None, name: str = None, rate: float = 2000, @@ -107,8 +108,8 @@ def add_device( nb_channels, device_type, name, rate, device_range, processing_method, **process_kwargs ) device_tmp.interface = self.interface_type - if self.vicon_client: - device_tmp.infos = self.vicon_client.GetDeviceOutputDetails(name) + if self.qualisys_client: + device_tmp.infos = self.qualisys.(name) else: device_tmp.infos = None device_tmp.data_windows = data_buffer_size From da304bb7109025c0a2abd1ba9eba9941268eeb1f Mon Sep 17 00:00:00 2001 From: OphelieL Date: Mon, 13 May 2024 17:31:56 +0400 Subject: [PATCH 04/23] Perform Qualisys interface Add device, force, maqueur --- biosiglive/interfaces/qualysis_interface.py | 84 ++++++++++++++++----- 1 file changed, 67 insertions(+), 17 deletions(-) diff --git a/biosiglive/interfaces/qualysis_interface.py b/biosiglive/interfaces/qualysis_interface.py index c0b6d25..a792902 100644 --- a/biosiglive/interfaces/qualysis_interface.py +++ b/biosiglive/interfaces/qualysis_interface.py @@ -13,7 +13,7 @@ pass -class QualysisClient(GenericInterface): +class QualisysClient(GenericInterface): """ Class for interfacing with the Qualisys system. """ @@ -34,10 +34,10 @@ def __init__(self, system_rate: int, ip: str = "192.168.254.1", port: int = 2222 Whether to initialize the client now. Usefull if you want to pickle the interface as the Vicon SDK is not pickable (swig). """ - super(QualysisClient, self).__init__(ip=ip, system_rate=system_rate, interface_type=InterfaceType.QualysisClient) + super(QualisysClient, self).__init__(ip=ip, system_rate=system_rate, interface_type=InterfaceType.QualysisClient) self.address =ip self.port= None - self.qualysis_client = None + self.qualisys_client = None self.acquisition_rate = None self.system_rate = system_rate self.devices = [] @@ -54,9 +54,9 @@ def _init_client(self): """ print(f"Connection to Qualisys DataStreamSDK at : {self.address} ...") - self.qualysis_client.Connect = await qtm_rt.connect(self.address) - if self.qualysis_client.Connect is None: - self.qualysis_client._update_state("Error","Failed to connect") + self.qualisys_client.Connect = await qtm_rt.connect(self.address) + if self.qualisys_client.Connect is None: + self.qualisys_client._update_state("Error","Failed to connect") return print("Connected to Qualisys") @@ -72,6 +72,52 @@ def _init_client(self): def add_device( + self, + nb_channels: int, + device_type: Union[DeviceType, str] = DeviceType.Emg, + 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 Vicon 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: + self.component.append('Analogue') + device_tmp.infos = None #Voir comment recuperer les info avec api qualisys + else: + device_tmp.infos = None + + device_tmp.data_windows = data_buffer_size + self.devices.append(device_tmp) + + def add_forceplate( self, nb_channels: int, device_type: Union[DeviceType, str] = DeviceType.ForcePlate, @@ -109,11 +155,15 @@ def add_device( ) device_tmp.interface = self.interface_type if self.qualisys_client: - device_tmp.infos = self.qualisys.(name) - else: - device_tmp.infos = None + self.component.append('Force') + device_tmp.infos = None #Voir comment recuperer les info avec api qualisys + else: + device_tmp.infos = None + device_tmp.data_windows = data_buffer_size self.devices.append(device_tmp) + + def add_marker_set( self, @@ -163,22 +213,22 @@ def add_marker_set( kinematics_method=kinematics_method, **kin_method_kwargs, ) - if self.vicon_client: - markers_tmp.subject_name = subject_name if subject_name else self.vicon_client.GetSubjectNames()[0] - markers_tmp.marker_names = ( - self.vicon_client.GetMarkerNames(markers_tmp.subject_name) if not marker_names else marker_names - ) - markers_tmp.marker_names = [name[0] for name in markers_tmp.marker_names] - if markers_tmp.nb_channels != len(markers_tmp.marker_names): - raise RuntimeError("Nb of marker not the same than markers on vicon.") + if self.qualisys_client: + markers_tmp.subject_name = subject_name #a changer quand je saurais comment recuperer nom du sujet avec qualisys + markers_tmp.marker_names = marker_names #a changer quand je saurais comment recuperer nom des marqueurs avec qualisys + self.component.append('3d') 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) + @staticmethod def get_force_plate_data(): + if qualisys_client + header, cameras = packet.get_2d_markers() + await headersPF, makers = get raise NotImplementedError("Force plate streaming is not implemented yet.") def get_device_data( From 01b4a603316d09b628997b77bf68b90f63f97c44 Mon Sep 17 00:00:00 2001 From: Ophelie Date: Thu, 16 May 2024 11:47:21 +0400 Subject: [PATCH 05/23] QTM data extraction needed for QualysisClient --- examples/sandox/TestQTM.py | 76 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 76 insertions(+) create mode 100644 examples/sandox/TestQTM.py diff --git a/examples/sandox/TestQTM.py b/examples/sandox/TestQTM.py new file mode 100644 index 0000000..0e5c093 --- /dev/null +++ b/examples/sandox/TestQTM.py @@ -0,0 +1,76 @@ +""" Example that takes control of QTM, streams data etc """ + +import asyncio +import logging +import xml.etree.ElementTree as ET +import qtm_rt +import time +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) + + 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 + + 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) + + + queue = asyncio.Queue() + + receiver_future = asyncio.ensure_future(package_receiver(queue)) + + await connection.stream_frames(components=["6d","3d","force"], on_packet=queue.put_nowait) + + +# asyncio.ensure_future(shutdown(30, connection, receiver_future, queue)) + + +if __name__ == "__main__": + loop = asyncio.get_event_loop() + asyncio.ensure_future(setup()) + loop.run_forever() From fa89a1416ef4221a99cd33fcc97ddee5a8a9f7e7 Mon Sep 17 00:00:00 2001 From: OphelieL Date: Fri, 17 May 2024 15:32:00 +0400 Subject: [PATCH 06/23] All def adaptation still some trouble --- biosiglive/interfaces/qualysis_interface.py | 161 +++++++++++++------- 1 file changed, 102 insertions(+), 59 deletions(-) diff --git a/biosiglive/interfaces/qualysis_interface.py b/biosiglive/interfaces/qualysis_interface.py index a792902..78a6ef7 100644 --- a/biosiglive/interfaces/qualysis_interface.py +++ b/biosiglive/interfaces/qualysis_interface.py @@ -1,10 +1,11 @@ """ -This file contains a wrapper for the python Vicon SDK. +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 try: import asyncio @@ -32,7 +33,7 @@ def __init__(self, system_rate: int, ip: str = "192.168.254.1", port: int = 2222 Port of the Qualisys software. init_now: bool Whether to initialize the client now. - Usefull if you want to pickle the interface as the Vicon SDK is not pickable (swig). + 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.QualysisClient) self.address =ip @@ -50,7 +51,7 @@ def __init__(self, system_rate: int, ip: str = "192.168.254.1", port: int = 2222 def _init_client(self): """ - Initialize the Vicon client. + Initialize the qualisys client. """ print(f"Connection to Qualisys DataStreamSDK at : {self.address} ...") @@ -62,15 +63,15 @@ def _init_client(self): print("Connected to Qualisys") self.is_initialized = True -""" pour le moment je ne trouver pas commetn avoir la Fs de Qualisys - if self.system_rate != self.qualisys_client.get_frame_rate(): + xml_general = await self.qualisys_client.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"Vicon system rate ({self.qualisys_client.get_frame_rate()}) does not match the system rate " + f"qualisys system rate ({self.qualisys_client.get_frame_rate()}) does not match the system rate " f"({self.system_rate})." ) -""" - - + def add_device( self, nb_channels: int, @@ -83,7 +84,7 @@ def add_device( **process_kwargs, ): """ - Add a device to the Vicon system. + Add a device to the qualisys system. Parameters ---------- @@ -109,8 +110,9 @@ def add_device( ) device_tmp.interface = self.interface_type if self.qualisys_client: - self.component.append('Analogue') - device_tmp.infos = None #Voir comment recuperer les info avec api qualisys + self.component.append('Analog') + xml_analog = await self.qualisys_client.Connect.get_parameters(parameters=["analog"]) + device_tmp.infos = ET.fromstring(xml_analog) else: device_tmp.infos = None @@ -119,8 +121,6 @@ def add_device( def add_forceplate( self, - nb_channels: int, - device_type: Union[DeviceType, str] = DeviceType.ForcePlate, data_buffer_size: int = None, name: str = None, rate: float = 2000, @@ -129,7 +129,7 @@ def add_forceplate( **process_kwargs, ): """ - Add a device to the Vicon system. + Add a device to the qualisys system. Parameters ---------- @@ -150,18 +150,19 @@ def add_forceplate( **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 + force_tmp = self._add_forceplate( + name, rate, device_range, processing_method, **process_kwargs ) - device_tmp.interface = self.interface_type + force_tmp.interface = self.interface_type if self.qualisys_client: - self.component.append('Force') - device_tmp.infos = None #Voir comment recuperer les info avec api qualisys + xml_force = await self.qualisys_client.Connect.get_parameters(parameters=["force"]) + force_tmp.infos = ET.fromstring(xml_force) + self.component.append('force') else: - device_tmp.infos = None + force_tmp.infos = None - device_tmp.data_windows = data_buffer_size - self.devices.append(device_tmp) + force_tmp.data_windows = data_buffer_size + self.force.append(force_tmp) @@ -178,7 +179,7 @@ def add_marker_set( **kin_method_kwargs, ): """ - Add markers set to stream from the Vicon system. + Add markers set to stream from the qualisys system. Parameters ---------- @@ -215,7 +216,9 @@ def add_marker_set( ) if self.qualisys_client: markers_tmp.subject_name = subject_name #a changer quand je saurais comment recuperer nom du sujet avec qualisys - markers_tmp.marker_names = marker_names #a changer quand je saurais comment recuperer nom des marqueurs avec qualisys + xlm_mks = await connection.get_parameters(parameters=['3d']) + MksInfo = ET.fromstring(xlm_mks) + markers_tmp.marker_names =[label.find('Name').text for label in MksInfo.findall(".//Label")] self.component.append('3d') else: markers_tmp.subject_name = subject_name @@ -225,17 +228,50 @@ def add_marker_set( @staticmethod - def get_force_plate_data(): + def get_force_plate_data(self, PF_name: Union[str, list] = "all", channel_idx: Union[int, list] = (), get_frame: bool = True): if qualisys_client - header, cameras = packet.get_2d_markers() - await headersPF, makers = get + headerf, forcespacket = self.packet.get_force() + PF_names=headerf #TODO change to be ok + if len(self.force) == 0: + raise ValueError("No PF 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 subject_name and isinstance(subject_name, list): + subject_name = [subject_name] + if PF_names and isinstance(PF_names, list): + PF_names = [PF_names] + all_force_data = [] + + + for PF in enumerate(forcespacket, 1): + forces.new_data = np.zeros((3, len(PF_names), self.packet.framenumber)) + count = 0 + for i, PF_name in enumerate(forcespacket, 1): + forces_data_tmp = forcespacket #TO CHECK + forces.new_data[:, count, :] = np.array(forces_data_tmp)[:, np.newaxis] + if PF_names: + forces_data = np.zeros((3, len(force_names), forces.sample)) + for n, name in enumerate(forces.force_names): + if name in force_names: + forces_data[:, force_names.index(name), :] = forces.new_data[:, n, :] + all_forces_data.append(forces_data) + else: + all_forces_data.append(forces.new_data) + + forces.append_data(forces.new_data) + if len(all_markers_data) == 1: + return all_markers_data[0] + return all_markers_data raise NotImplementedError("Force plate streaming is not implemented yet.") 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 Vicon. + Get the device data from qualisys. Parameters ---------- @@ -244,7 +280,7 @@ def get_device_data( channel_idx: Union[int, str] Index of the channel to return. get_frame: bool - Whether to get a new frame from the Vicon system. + Whether to get a new frame from the qualisys system. Returns ------- @@ -252,23 +288,24 @@ def get_device_data( All asked device data. """ if len(self.devices) == 0: - raise ValueError("No device has been added to the Vicon system.") + raise ValueError("No device has been added to the qualisys system.") if not self.is_initialized: - raise RuntimeError("Vicon client is not initialized.") + raise RuntimeError("Qualisys client is not initialized.") if get_frame: - self.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.vicon_client.GetDeviceOutputValues(device.name, output_name, channel_name) + 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 @@ -290,7 +327,7 @@ def get_marker_set_data( self, subject_name: Union[str, list] = None, marker_names: Union[str, list] = None, get_frame: bool = True ): """ - Get the markers data from Vicon. + Get the markers data from qualisys. Parameters ---------- @@ -307,18 +344,18 @@ def get_marker_set_data( All asked markers data. """ if len(self.marker_sets) == 0: - raise ValueError("No marker set has been added to the Vicon system.") + raise ValueError("No marker set has been added to the qualisys system.") if not self.is_initialized: - raise RuntimeError("Vicon client is not initialized.") + raise RuntimeError("qualisys client is not initialized.") if get_frame: - self.get_frame() + self.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 = [] + #occluded = [] all_markers_data = [] - all_occluded_data = [] + #all_occluded_data = [] if subject_name: marker_sets = [None] * len(subject_name) for s, marker_set in enumerate(self.marker_sets): @@ -333,11 +370,9 @@ def get_marker_set_data( markers.new_data = np.zeros((3, len(markers.marker_names), markers.sample)) count = 0 for m, marker_name in enumerate(markers.marker_names): - markers_data_tmp, occluded_tmp = self.vicon_client.GetMarkerGlobalTranslation( - markers.subject_name, marker_name - ) + header, markers_data_tmp= self.get_3d_markers() #TO check markers.new_data[:, count, :] = np.array(markers_data_tmp)[:, np.newaxis] - occluded.append(occluded_tmp) + #occluded.append(occluded_tmp) if marker_names: markers_data = np.zeros((3, len(marker_names), markers.sample)) for n, name in enumerate(markers.marker_names): @@ -355,50 +390,58 @@ def get_marker_set_data( def init_client(self): """ - Initialize the Vicon client if it is not already initialized. + 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("Vicon client is already initialized.") + raise RuntimeError("Qualisys client is already initialized.") else: self._init_client() + xlm_analog = await connection.get_parameters(parameters=['analog']) + AnalogInfo = ET.fromstring(xlm_analog) for d, device in enumerate(self.devices): if not device.infos: - device.infos = self.vicon_client.GetDeviceOutputDetails(device.name) + device.infos = self.qualisys_client.GetDeviceOutputDetails(device.name) #a changer + + xlm_3d = await connection.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 = self.vicon_client.GetMarkerNames(marker_set.subject_name) + marker_set.markers_names = [label.find('Name').text for label in MksInfo.findall(".//Label")] if not marker_set.subject_name: - marker_set.subject_name = self.vicon_client.GetSubjectNames()[0] + marker_set.subject_name = [label.find('Name').text for label in MksInfo.findall(".//Label")] def get_latency(self) -> float: """ - Get the latency between the Vicon system and the Vicon SDK. + Get the latency between the qualisys system and the qualisys SDK. Returns ------- latency: float - Latency between the Vicon system and the Vicon SDK. + Latency between the qualisys system and the qualisys SDK. """ if not self.is_initialized: - raise RuntimeError("Vicon client is not initialized.") - return self.vicon_client.GetLatencyTotal() + raise RuntimeError("qualisys client is not initialized.") + return self.qualisys_client=0 #TODO voir comment obtenir la latence def get_frame(self) -> bool: """ - Get a new frame from the Vicon system. + 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("Vicon client is not initialized.") - self.is_frame = self.vicon_client.GetFrame() + 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.vicon_client.GetFrame() + self.is_frame = self.qualisys_client.GetFrame() return self.is_frame + """ #TODO def get_frame_number(self) -> int: """ @@ -410,8 +453,8 @@ def get_frame_number(self) -> int: Last frame number. """ if not self.is_initialized: - raise RuntimeError("Vicon client is not initialized.") - return self.vicon_client.GetFrameNumber() + raise RuntimeError("qualisys client is not initialized.") + return self.packet.framenumber def get_kinematics_from_markers( self, From a2da35cb0510f65c3e5d663d1f31176ca7888153 Mon Sep 17 00:00:00 2001 From: Ophelie Date: Fri, 24 May 2024 12:20:58 +0400 Subject: [PATCH 07/23] Qualisys interface added --- biosiglive/__init__.py | 1 + 1 file changed, 1 insertion(+) 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 From fc26c51bdc957effe8d304b8bf33e9b439ef4844 Mon Sep 17 00:00:00 2001 From: Ophelie Date: Fri, 24 May 2024 12:21:33 +0400 Subject: [PATCH 08/23] Debug in process --- ...sis_interface.py => qualisys_interface.py} | 881 ++++++++++-------- 1 file changed, 487 insertions(+), 394 deletions(-) rename biosiglive/interfaces/{qualysis_interface.py => qualisys_interface.py} (57%) diff --git a/biosiglive/interfaces/qualysis_interface.py b/biosiglive/interfaces/qualisys_interface.py similarity index 57% rename from biosiglive/interfaces/qualysis_interface.py rename to biosiglive/interfaces/qualisys_interface.py index c0b6d25..fe3eedd 100644 --- a/biosiglive/interfaces/qualysis_interface.py +++ b/biosiglive/interfaces/qualisys_interface.py @@ -1,394 +1,487 @@ -""" -This file contains a wrapper for the python Vicon SDK. -""" -from .param import * -from typing import Union -from .generic_interface import GenericInterface -from ..enums import InverseKinematicsMethods, InterfaceType - -try: - import asyncio - import qtm_rt -except ModuleNotFoundError: - pass - - -class QualysisClient(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 Vicon SDK is not pickable (swig). - """ - super(QualysisClient, self).__init__(ip=ip, system_rate=system_rate, interface_type=InterfaceType.QualysisClient) - self.address =ip - self.port= None - self.qualysis_client = None - self.acquisition_rate = None - self.system_rate = system_rate - self.devices = [] - self.imu = [] - self.marker_sets = [] - self.is_frame = False - self.is_initialized = False - if init_now: - self._init_client() - - def _init_client(self): - """ - Initialize the Vicon client. - """ - - print(f"Connection to Qualisys DataStreamSDK at : {self.address} ...") - self.qualysis_client.Connect = await qtm_rt.connect(self.address) - if self.qualysis_client.Connect is None: - self.qualysis_client._update_state("Error","Failed to connect") - return - - print("Connected to Qualisys") - self.is_initialized = True - -""" pour le moment je ne trouver pas commetn avoir la Fs de Qualisys - if self.system_rate != self.qualisys_client.get_frame_rate(): - raise ValueError( - f"Vicon system rate ({self.qualisys_client.get_frame_rate()}) does not match the system rate " - f"({self.system_rate})." - ) -""" - - - 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 Vicon 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: - device_tmp.infos = self.qualisys.(name) - else: - device_tmp.infos = None - device_tmp.data_windows = data_buffer_size - self.devices.append(device_tmp) - - 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 Vicon 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.vicon_client: - markers_tmp.subject_name = subject_name if subject_name else self.vicon_client.GetSubjectNames()[0] - markers_tmp.marker_names = ( - self.vicon_client.GetMarkerNames(markers_tmp.subject_name) if not marker_names else marker_names - ) - markers_tmp.marker_names = [name[0] for name in markers_tmp.marker_names] - if markers_tmp.nb_channels != len(markers_tmp.marker_names): - raise RuntimeError("Nb of marker not the same than markers on vicon.") - 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) - - @staticmethod - def get_force_plate_data(): - raise NotImplementedError("Force plate streaming is not implemented yet.") - - 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 Vicon. - - 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 Vicon system. - - Returns - ------- - device_data: list - All asked device data. - """ - if len(self.devices) == 0: - raise ValueError("No device has been added to the Vicon system.") - if not self.is_initialized: - raise RuntimeError("Vicon client is not initialized.") - if get_frame: - self.get_frame() - 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 = [] - 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.vicon_client.GetDeviceOutputValues(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 - - def get_marker_set_data( - self, subject_name: Union[str, list] = None, marker_names: Union[str, list] = None, get_frame: bool = True - ): - """ - Get the markers data from Vicon. - - 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 Vicon system.") - if not self.is_initialized: - raise RuntimeError("Vicon client is not initialized.") - if get_frame: - self.get_frame() - 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 - for m, marker_name in enumerate(markers.marker_names): - markers_data_tmp, occluded_tmp = self.vicon_client.GetMarkerGlobalTranslation( - markers.subject_name, marker_name - ) - markers.new_data[:, count, :] = np.array(markers_data_tmp)[:, np.newaxis] - occluded.append(occluded_tmp) - if marker_names: - markers_data = np.zeros((3, len(marker_names), markers.sample)) - for n, name in enumerate(markers.marker_names): - if name in marker_names: - markers_data[:, marker_names.index(name), :] = markers.new_data[:, n, :] - all_markers_data.append(markers_data) - else: - all_markers_data.append(markers.new_data) - - markers.append_data(markers.new_data) - all_occluded_data.append(occluded) - if len(all_markers_data) == 1: - return all_markers_data[0], all_occluded_data[0] - return all_markers_data, all_occluded_data - - def init_client(self): - """ - Initialize the Vicon client if it is not already initialized. - This function has to be called before get frame from interface. - """ - if self.is_initialized: - raise RuntimeError("Vicon client is already initialized.") - else: - self._init_client() - for d, device in enumerate(self.devices): - if not device.infos: - device.infos = self.vicon_client.GetDeviceOutputDetails(device.name) - for m, marker_set in enumerate(self.marker_sets): - if not marker_set.markers_names: - marker_set.markers_names = self.vicon_client.GetMarkerNames(marker_set.subject_name) - if not marker_set.subject_name: - marker_set.subject_name = self.vicon_client.GetSubjectNames()[0] - - def get_latency(self) -> float: - """ - Get the latency between the Vicon system and the Vicon SDK. - - Returns - ------- - latency: float - Latency between the Vicon system and the Vicon SDK. - """ - if not self.is_initialized: - raise RuntimeError("Vicon client is not initialized.") - return self.vicon_client.GetLatencyTotal() - - def get_frame(self) -> bool: - """ - Get a new frame from the Vicon system. - - Returns - ------- - bool - True if there is a frame, False otherwise. - """ - if not self.is_initialized: - raise RuntimeError("Vicon client is not initialized.") - self.is_frame = self.vicon_client.GetFrame() - while self.is_frame is not True: - self.is_frame = self.vicon_client.GetFrame() - return self.is_frame - - 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("Vicon client is not initialized.") - return self.vicon_client.GetFrameNumber() - - 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) +""" +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 + +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.imu = [] + self.marker_sets = [] + self.is_frame = False + self.is_initialized = False + if init_now: + asyncio.create_task(self._init_client()) + + + async def _init_client(self): + """ + Initialize the qualisys client. + """ + print(f"Connection to Qualisys DataStreamSDK at : {self.address} ...") + self.qualisys_client.Connect = await qtm_rt.connect(self.address) + if self.qualisys_client.Connect is None: + print("Error","Failed to connect") + return + + print("Connected to Qualisys") + self.is_initialized = True + + xml_general = await self.qualisys_client.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 ({self.qualisys_client.get_frame_rate()}) does not match the system rate " + f"({self.system_rate})." + ) + + async def create(cls, ip="192.168.254.1", system_rate=100, port=22224): + self = cls(ip, system_rate, init_now=False) + await self._init_client() + return self + + async def add_device( + self, + nb_channels: int, + device_type: Union[DeviceType, str] = DeviceType.Emg, + 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: + self.component.append('Analog') + xml_analog = await self.qualisys_client.Connect.get_parameters(parameters=["analog"]) + device_tmp.infos = ET.fromstring(xml_analog) + 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 = 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. + """ + force_tmp = self._add_forceplate( + name, rate, device_range, processing_method, **process_kwargs + ) + force_tmp.interface = self.interface_type + if self.qualisys_client: + xml_force = await self.qualisys_client.Connect.get_parameters(parameters=["force"]) + force_tmp.infos = ET.fromstring(xml_force) + self.component.append('force') + else: + force_tmp.infos = None + + force_tmp.data_windows = data_buffer_size + self.force.append(force_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 connection.get_parameters(parameters=['3d']) + MksInfo = ET.fromstring(xlm_mks) + markers_tmp.marker_names =[label.find('Name').text for label in MksInfo.findall(".//Label")] + self.component.append('3d') + 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) + + + @staticmethod + async def get_force_plate_data(self, PF_name: Union[str, list] = "all", channel_idx: Union[int, list] = (), get_frame: bool = True): + if qualisys_client: + headerf, forcespacket = self.packet.get_force() + PF_names=headerf #TODO change to be ok + if len(self.force) == 0: + raise ValueError("No PF 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 subject_name and isinstance(subject_name, list): + subject_name = [subject_name] + if PF_names and isinstance(PF_names, list): + PF_names = [PF_names] + all_force_data = [] + + + for PF in enumerate(forcespacket, 1): + forces.new_data = np.zeros((3, len(PF_names), self.packet.framenumber)) + count = 0 + for i, PF_name in enumerate(forcespacket, 1): + forces_data_tmp = forcespacket[i][:] #TO CHECK + forces.new_data[:, count, :] = np.array(forces_data_tmp)[:, np.newaxis] + all_forces_data.append(forces.new_data) + forces.append_data(forces.new_data) + if len(all_forces_data) == 1: + return all_forces_data[0] + return all_forces_data + raise NotImplementedError("Force plate streaming is not implemented yet.") + + async 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 + ): + """ + 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. + """ + await self.connection.stream_frames(components=self.component, on_packet=queue.put_nowait) + 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: + self.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= self.get_3d_markers() #TO check + get_3d_markers_no_label + for m, marker_name in enumerate(markers.marker_names): + markers_data_tmp=allmarkers_data_tmp[m][:] + markers.new_data[:, count, :] = np.array(markers_data_tmp)[:, np.newaxis] + occluded.append(occluded_tmp) + if marker_names: + markers_data = np.zeros((3, len(marker_names), markers.sample)) + for n, name in enumerate(markers.marker_names): + if name in marker_names: + markers_data[:, marker_names.index(name), :] = markers.new_data[:, n, :] + all_markers_data.append(markers_data) + else: + all_markers_data.append(markers.new_data) + markers.append_data(markers.new_data) + all_occluded_data.append(occluded) + if len(all_markers_data) == 1: + return all_markers_data[0], all_occluded_data[0] + return all_markers_data, 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 connection.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 connection.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) From 98bfdf46e321df1f99d23c0ffb7b58c671e74c21 Mon Sep 17 00:00:00 2001 From: Ophelie Date: Fri, 24 May 2024 12:24:06 +0400 Subject: [PATCH 09/23] Test to of Qualisys client --- examples/sandox/Test_QualisysClient.py | 64 ++++++++++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 examples/sandox/Test_QualisysClient.py diff --git a/examples/sandox/Test_QualisysClient.py b/examples/sandox/Test_QualisysClient.py new file mode 100644 index 0000000..c9ac47c --- /dev/null +++ b/examples/sandox/Test_QualisysClient.py @@ -0,0 +1,64 @@ +""" +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 + + +async def setup(): + """ main function """ + interface = await QualisysClient.create(ip="192.168.254.1", system_rate=100, port=22224) + queue = asyncio.Queue() + n_markers = 4 + await interface.add_marker_set( + nb_markers=n_markers, data_buffer_size=100, marker_data_file_key="markers", name="markers", rate=100, unit="mm" + ) + + + marker_plot = LivePlot(name="markers", plot_type=PlotType.Scatter3D) + marker_plot.init() + time_to_sleep = 1 / 100 + offline_count = 0 + mark_to_plot = [] + + while True: + tic = time() + packet = await queue.get() + if packet is None: + break + self.packet = packet + mark_tmp, _ = await interface.get_marker_set_data() + marker_plot.update(mark_tmp[:, :, -1].T, size=0.03) + loop_time = time() - tic + real_time_to_sleep = time_to_sleep - loop_time + if real_time_to_sleep > 0: + sleep(real_time_to_sleep) + + +if __name__ == "__main__": + loop = asyncio.get_event_loop() + asyncio.ensure_future(setup()) + loop.run_forever() From 525ec25188e08982184c7072ece994c005437a80 Mon Sep 17 00:00:00 2001 From: Ophelie Date: Thu, 30 May 2024 09:09:20 +0400 Subject: [PATCH 10/23] Bug in add device data and get force data fix all excepted add_device_data works. Pb a delay in the stream --- biosiglive/interfaces/qualisys_interface.py | 206 +++++++++++--------- 1 file changed, 118 insertions(+), 88 deletions(-) diff --git a/biosiglive/interfaces/qualisys_interface.py b/biosiglive/interfaces/qualisys_interface.py index 5efd816..933eb75 100644 --- a/biosiglive/interfaces/qualisys_interface.py +++ b/biosiglive/interfaces/qualisys_interface.py @@ -36,16 +36,19 @@ def __init__(self, system_rate: int, ip: str = "192.168.254.1", port: int = 2222 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.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.create_task(self._init_client()) @@ -53,34 +56,43 @@ def __init__(self, system_rate: int, ip: str = "192.168.254.1", port: int = 2222 async def _init_client(self): """ Initialize the qualisys client. + """ - print(f"Connection to Qualisys DataStreamSDK at : {self.address} ...") - self.qualisys_client.Connect = await qtm_rt.connect(self.address) - if self.qualisys_client.Connect is None: + print(f"Connection to Qualisys DataStreamSDK at : {self.ip} ...") + 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.qualisys_client.Connect.get_parameters(parameters=["general"]) + 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(): + if self.system_rate != frame_rate: raise ValueError( - f"qualisys system rate ({self.qualisys_client.get_frame_rate()}) does not match the system rate " + f"qualisys system rate ({frame_rate}) does not match the system rate " f"({self.system_rate})." ) - async def create(cls, ip="192.168.254.1", system_rate=100, port=22224): - self = cls(ip, system_rate, init_now=False) + @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 + def _update_state(self, statuts, message): + print(f"Status: {status}, Message: {message}") + + def _display_info(self): + print(f"IP: {self.ip}") + async def add_device( self, nb_channels: int, - device_type: Union[DeviceType, str] = DeviceType.Emg, + device_type: Union[DeviceType, str] = DeviceType.ForcePlate, data_buffer_size: int = None, name: str = None, rate: float = 2000, @@ -115,25 +127,41 @@ async def add_device( ) device_tmp.interface = self.interface_type if self.qualisys_client: - self.component.append('Analog') - xml_analog = await self.qualisys_client.Connect.get_parameters(parameters=["analog"]) - device_tmp.infos = ET.fromstring(xml_analog) + 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) + 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 = 2000, - device_range: tuple = 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 @@ -154,22 +182,30 @@ async def add_forceplate( Method used to process the data. **process_kwargs Keyword arguments for the processing method. - """ - force_tmp = self._add_forceplate( - name, rate, device_range, processing_method, **process_kwargs - ) - force_tmp.interface = self.interface_type + """ + """ + 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.qualisys_client.Connect.get_parameters(parameters=["force"]) - force_tmp.infos = ET.fromstring(xml_force) + 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: - force_tmp.infos = None - - force_tmp.data_windows = data_buffer_size - self.force.append(force_tmp) - + 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, @@ -221,48 +257,48 @@ async def add_marker_set( ) 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 connection.get_parameters(parameters=['3d']) + 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.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) - - - @staticmethod - async def get_force_plate_data(self, PF_name: Union[str, list] = "all", channel_idx: Union[int, list] = (), get_frame: bool = True): - if qualisys_client: - headerf, forcespacket = self.packet.get_force() - PF_names=headerf #TODO change to be ok - if len(self.force) == 0: - raise ValueError("No PF 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 subject_name and isinstance(subject_name, list): - subject_name = [subject_name] - if PF_names and isinstance(PF_names, list): - PF_names = [PF_names] - all_force_data = [] - - - for PF in enumerate(forcespacket, 1): - forces.new_data = np.zeros((3, len(PF_names), self.packet.framenumber)) - count = 0 - for i, PF_name in enumerate(forcespacket, 1): - forces_data_tmp = forcespacket[i][:] #TO CHECK - forces.new_data[:, count, :] = np.array(forces_data_tmp)[:, np.newaxis] - all_forces_data.append(forces.new_data) - forces.append_data(forces.new_data) - if len(all_forces_data) == 1: - return all_forces_data[0] - return all_forces_data - raise NotImplementedError("Force plate streaming is not implemented yet.") + + async def get_force_plate_data( + self, forceplate_name: Union[str, list] = "all", get_frame: bool = True, packet=[] + ): + 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 = [] + forcedata = [] + if (forcesdata[0][0].force_count) != 0: + + Device.new_data = np.zeros((9, headerf.plate_count, packet.framenumber)) + for frame in range(forcesdata[0][0].force_count): + for platenum in range(headerf.plate_count): + forcedata = forcesdata[platenum][1][frame] + 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] + + Device.new_data[:, platenum, :] = np.array(forces_data_tmp)[:, np.newaxis] + + all_forces_data.append(Device.new_data) + #Device.append_data(Device.new_data) + + if len(all_forces_data) == 1: + return all_forces_data[0] + return all_forces_data + async def get_device_data( self, device_name: Union[str, list] = "all", channel_idx: Union[int, list] = (), get_frame: bool = True @@ -296,7 +332,7 @@ async def get_device_data( if channel_idx and not isinstance(channel_idx, list): channel_idx = [channel_idx] device_data = [] - headerdevice, Devicevalue = self.packet.get_analog + 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)) @@ -321,7 +357,7 @@ async def get_device_data( 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 + 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. @@ -340,13 +376,13 @@ async def get_marker_set_data( markers_data: list All asked markers data. """ - await self.connection.stream_frames(components=self.component, on_packet=queue.put_nowait) + 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: - self.packet.framenumber + packet.framenumber if subject_name and isinstance(subject_name, list): subject_name = [subject_name] if marker_names and isinstance(marker_names, list): @@ -367,25 +403,19 @@ async def get_marker_set_data( for markers in marker_sets: markers.new_data = np.zeros((3, len(markers.marker_names), markers.sample)) count = 0 - header, allmarkers_data_tmp= self.get_3d_markers() #TO check - get_3d_markers_no_label + 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[:, count, :] = np.array(markers_data_tmp)[:, np.newaxis] + markers.new_data[:, m, :] = np.array(markers_data_tmp)[:, np.newaxis] #occluded.append(occluded_tmp) - if marker_names: - markers_data = np.zeros((3, len(marker_names), markers.sample)) - for n, name in enumerate(markers.marker_names): - if name in marker_names: - markers_data[:, marker_names.index(name), :] = markers.new_data[:, n, :] - all_markers_data.append(markers_data) - else: - all_markers_data.append(markers.new_data) + + all_markers_data.append(markers.new_data) markers.append_data(markers.new_data) - all_occluded_data.append(occluded) + #all_occluded_data.append(occluded) if len(all_markers_data) == 1: - return all_markers_data[0], all_occluded_data[0] - return all_markers_data, all_occluded_data + return all_markers_data[0] #, all_occluded_data[0] + return all_markers_data #, all_occluded_data async def init_client(self): """ @@ -396,13 +426,13 @@ async def init_client(self): raise RuntimeError("Qualisys client is already initialized.") else: self._init_client() - xlm_analog = await connection.get_parameters(parameters=['analog']) + 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 connection.get_parameters(parameters=['3d']) + 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): From ff248e81d0b8a9147868f17bfc7f5f7c432cda56 Mon Sep 17 00:00:00 2001 From: Ophelie Date: Thu, 30 May 2024 09:11:35 +0400 Subject: [PATCH 11/23] An example to streamdata with QualisysClient --- examples/sandox/Test_QualisysClient.py | 37 +++++++++++++++++++++++--- 1 file changed, 34 insertions(+), 3 deletions(-) diff --git a/examples/sandox/Test_QualisysClient.py b/examples/sandox/Test_QualisysClient.py index c9ac47c..f09cf82 100644 --- a/examples/sandox/Test_QualisysClient.py +++ b/examples/sandox/Test_QualisysClient.py @@ -26,39 +26,70 @@ import logging import xml.etree.ElementTree as ET import qtm_rt +import numpy as np async def setup(): """ main function """ interface = await QualisysClient.create(ip="192.168.254.1", system_rate=100, port=22224) + interface._display_info() + queue = asyncio.Queue() n_markers = 4 await interface.add_marker_set( nb_markers=n_markers, data_buffer_size=100, 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", + rate=1000, + device_data_file_key="force_plate", + processing_method=None, + moving_average=True, + ) marker_plot = LivePlot(name="markers", plot_type=PlotType.Scatter3D) marker_plot.init() - time_to_sleep = 1 / 100 + force1_plot = LivePlot( + name="force", rate=100, plot_type=PlotType.Curve, nb_subplots=3 + ) + force1_plot.init(plot_windows=500, y_labels="Force (N)") + + force2_plot = LivePlot( + name="force", rate=100, plot_type=PlotType.Curve, nb_subplots=3 + ) + force2_plot.init(plot_windows=500, y_labels="Force (N)") + time_to_sleep = 1 / 1000 offline_count = 0 mark_to_plot = [] + await interface.Connect.stream_frames(components=interface.component, on_packet=queue.put_nowait) + while True: tic = time() packet = await queue.get() if packet is None: break - self.packet = packet - mark_tmp, _ = await interface.get_marker_set_data() + + mark_tmp = await interface.get_marker_set_data(packet=packet) + mark_tmp = mark_tmp / 1000 marker_plot.update(mark_tmp[:, :, -1].T, size=0.03) + force_tmp = await interface.get_force_plate_data(packet=packet) + if (len(force_tmp)) != 0: + force1_plot.update(np.array(force_tmp[0:3, 0, -1:])) + force2_plot.update(np.array(force_tmp[0:3, 1, -1:])) loop_time = time() - tic real_time_to_sleep = time_to_sleep - loop_time if real_time_to_sleep > 0: sleep(real_time_to_sleep) + if __name__ == "__main__": + #asyncio.run(setup()) + loop = asyncio.get_event_loop() asyncio.ensure_future(setup()) loop.run_forever() From 280d346c97bf920cf4f40045ebdb9aa52b0d503e Mon Sep 17 00:00:00 2001 From: Ophelie Date: Mon, 3 Jun 2024 13:13:34 +0400 Subject: [PATCH 12/23] Error fixed on QualisysClient and exemple --- biosiglive/interfaces/qualisys_interface.py | 40 +++++++-------- examples/sandox/Test_QualisysClient.py | 54 +++++++++++++-------- 2 files changed, 51 insertions(+), 43 deletions(-) diff --git a/biosiglive/interfaces/qualisys_interface.py b/biosiglive/interfaces/qualisys_interface.py index 933eb75..1d2225b 100644 --- a/biosiglive/interfaces/qualisys_interface.py +++ b/biosiglive/interfaces/qualisys_interface.py @@ -6,6 +6,7 @@ from .generic_interface import GenericInterface from ..enums import InverseKinematicsMethods, InterfaceType import xml.etree.ElementTree as ET +import numpy as np try: import asyncio @@ -50,7 +51,7 @@ def __init__(self, system_rate: int, ip: str = "192.168.254.1", port: int = 2222 self.is_initialized = False self.component = [] if init_now: - asyncio.create_task(self._init_client()) + asyncio.run(self._init_client()) async def _init_client(self): @@ -58,7 +59,7 @@ async def _init_client(self): Initialize the qualisys client. """ - print(f"Connection to Qualisys DataStreamSDK at : {self.ip} ...") + 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") @@ -83,12 +84,6 @@ async def create(cls, system_rate=100, ip="192.168.254.1", port=22224): await self._init_client() return self - def _update_state(self, statuts, message): - print(f"Status: {status}, Message: {message}") - - def _display_info(self): - print(f"IP: {self.ip}") - async def add_device( self, nb_channels: int, @@ -136,7 +131,6 @@ async def add_device( 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') @@ -268,8 +262,8 @@ async def add_marker_set( 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=[] + 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.") @@ -278,29 +272,29 @@ async def get_force_plate_data( if get_frame: packet.framenumber headerf, forcesdata = packet.get_force() + all_forces_data = [] - forcedata = [] if (forcesdata[0][0].force_count) != 0: Device.new_data = np.zeros((9, headerf.plate_count, packet.framenumber)) - for frame in range(forcesdata[0][0].force_count): - for platenum in range(headerf.plate_count): - forcedata = forcesdata[platenum][1][frame] - 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] + #for frame in range(forcesdata[0][0].force_count): + for platenum in range(headerf.plate_count): + forcedata = forcesdata[platenum][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] - Device.new_data[:, platenum, :] = np.array(forces_data_tmp)[:, np.newaxis] + Device.new_data[:, platenum, :] = np.array(forces_data_tmp)[:, np.newaxis] all_forces_data.append(Device.new_data) #Device.append_data(Device.new_data) - + #print(packet.timestamp) if len(all_forces_data) == 1: return all_forces_data[0] return all_forces_data - async def get_device_data( + def get_device_data( self, device_name: Union[str, list] = "all", channel_idx: Union[int, list] = (), get_frame: bool = True ): """ @@ -356,7 +350,7 @@ async def get_device_data( return all_device_data[0] return all_device_data - async def get_marker_set_data( + def get_marker_set_data( self, subject_name: Union[str, list] = None, marker_names: Union[str, list] = None, get_frame: bool = True, packet = None ): """ @@ -414,6 +408,8 @@ async def get_marker_set_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] #, all_occluded_data[0] return all_markers_data #, all_occluded_data diff --git a/examples/sandox/Test_QualisysClient.py b/examples/sandox/Test_QualisysClient.py index f09cf82..e0422cb 100644 --- a/examples/sandox/Test_QualisysClient.py +++ b/examples/sandox/Test_QualisysClient.py @@ -27,22 +27,25 @@ 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.254.1", system_rate=100, port=22224) - interface._display_info() - queue = asyncio.Queue() + + # Add info needed n_markers = 4 await interface.add_marker_set( - nb_markers=n_markers, data_buffer_size=100, marker_data_file_key="markers", name="markers", rate=100, unit="mm" + 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, @@ -50,46 +53,55 @@ async def setup(): ) + # Plot init marker_plot = LivePlot(name="markers", plot_type=PlotType.Scatter3D) marker_plot.init() force1_plot = LivePlot( - name="force", rate=100, plot_type=PlotType.Curve, nb_subplots=3 + name="force", rate=1000, plot_type=PlotType.Curve, nb_subplots=3 ) - force1_plot.init(plot_windows=500, y_labels="Force (N)") - + force1_plot.init(plot_windows=1000, y_labels="Force (N)") force2_plot = LivePlot( - name="force", rate=100, plot_type=PlotType.Curve, nb_subplots=3 + name="force", rate=1000, plot_type=PlotType.Curve, nb_subplots=3 ) - force2_plot.init(plot_windows=500, y_labels="Force (N)") - time_to_sleep = 1 / 1000 + force2_plot.init(plot_windows=1000, 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)) - await interface.Connect.stream_frames(components=interface.component, on_packet=queue.put_nowait) while True: - tic = time() - packet = await queue.get() - if packet is None: - break + tic = asyncio.get_event_loop().time() + packet = await interface.Connect.get_current_frame(components=interface.component) - mark_tmp = await interface.get_marker_set_data(packet=packet) + #data recuperation + mark_tmp = interface.get_marker_set_data(packet=packet) mark_tmp = mark_tmp / 1000 - marker_plot.update(mark_tmp[:, :, -1].T, size=0.03) - force_tmp = await interface.get_force_plate_data(packet=packet) + 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(np.array(force_tmp[0:3, 0, -1:])) force2_plot.update(np.array(force_tmp[0:3, 1, -1:])) - loop_time = time() - tic + + # 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: - sleep(real_time_to_sleep) + await asyncio.sleep(real_time_to_sleep) if __name__ == "__main__": - #asyncio.run(setup()) - + asyncio.run(setup()) + """" loop = asyncio.get_event_loop() asyncio.ensure_future(setup()) loop.run_forever() + """ \ No newline at end of file From df64b05154c02d9bfc8a703efb0ed552a72f8055 Mon Sep 17 00:00:00 2001 From: Ophelie Date: Thu, 29 Aug 2024 16:06:47 +0400 Subject: [PATCH 13/23] WIP Perform force data saving --- biosiglive/interfaces/qualisys_interface.py | 33 ++++++++++++++++----- examples/sandox/Test_QualisysClient.py | 11 +++---- 2 files changed, 32 insertions(+), 12 deletions(-) diff --git a/biosiglive/interfaces/qualisys_interface.py b/biosiglive/interfaces/qualisys_interface.py index 1d2225b..6350c61 100644 --- a/biosiglive/interfaces/qualisys_interface.py +++ b/biosiglive/interfaces/qualisys_interface.py @@ -273,25 +273,44 @@ def get_force_plate_data( packet.framenumber headerf, forcesdata = packet.get_force() - all_forces_data = [] + all_forces_data = np.empty([len(forcesdata),9,1]) 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 + return all_forces_data + """ Device.new_data = np.zeros((9, headerf.plate_count, packet.framenumber)) #for frame in range(forcesdata[0][0].force_count): for platenum in range(headerf.plate_count): - forcedata = forcesdata[platenum][1][-1] - forces_data_tmp = [forcedata.x, forcedata.y, forcedata.z, + if forcesdata[platenum][0].force_count!= 0: + forcedata = forcesdata[platenum][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] - Device.new_data[:, platenum, :] = np.array(forces_data_tmp)[:, np.newaxis] + Device.new_data[:, platenum, :] = np.array(forces_data_tmp)[:, np.newaxis] + + all_forces_data.append(Device.new_data) - all_forces_data.append(Device.new_data) - #Device.append_data(Device.new_data) - #print(packet.timestamp) if len(all_forces_data) == 1: return all_forces_data[0] return all_forces_data + """ def get_device_data( diff --git a/examples/sandox/Test_QualisysClient.py b/examples/sandox/Test_QualisysClient.py index e0422cb..056b947 100644 --- a/examples/sandox/Test_QualisysClient.py +++ b/examples/sandox/Test_QualisysClient.py @@ -59,11 +59,11 @@ async def setup(): force1_plot = LivePlot( name="force", rate=1000, plot_type=PlotType.Curve, nb_subplots=3 ) - force1_plot.init(plot_windows=1000, y_labels="Force (N)") + 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=1000, y_labels="Force (N)") + force2_plot.init(plot_windows=10000, y_labels="Force (N)") time_to_sleep = 1 / 200 @@ -80,15 +80,16 @@ async def setup(): #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) + 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(np.array(force_tmp[0:3, 0, -1:])) - force2_plot.update(np.array(force_tmp[0:3, 1, -1:])) + force1_plot.update(force_tmp[0][:3, -1:]) + force2_plot.update(force_tmp[1][:3, -1:]) # time laps loop_time = asyncio.get_event_loop().time() - tic From d9fb689d98cd58ce0cdf97076cc2b19a4f2ed5f9 Mon Sep 17 00:00:00 2001 From: Ophelie Date: Thu, 29 Aug 2024 16:08:00 +0400 Subject: [PATCH 14/23] A code to send stim during walking propulsive phase --- examples/sandox/WalkStim_FapPositif.py | 87 ++++++++++++++++++++++++++ 1 file changed, 87 insertions(+) create mode 100644 examples/sandox/WalkStim_FapPositif.py 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() From 5a89c9bc8800d27cbc776ad56b81d5192265cf81 Mon Sep 17 00:00:00 2001 From: Ophelie Date: Wed, 9 Oct 2024 18:29:59 +0400 Subject: [PATCH 15/23] 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 From 4eb6954f713d59166668614252f613e9cd52e80a Mon Sep 17 00:00:00 2001 From: Ophelie Date: Thu, 17 Oct 2024 16:10:09 +0400 Subject: [PATCH 16/23] add get makers names --- biosiglive/interfaces/qualisys_interface.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/biosiglive/interfaces/qualisys_interface.py b/biosiglive/interfaces/qualisys_interface.py index b0ccb88..cf4ac0a 100644 --- a/biosiglive/interfaces/qualisys_interface.py +++ b/biosiglive/interfaces/qualisys_interface.py @@ -254,6 +254,7 @@ async def add_marker_set( 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: @@ -423,7 +424,7 @@ def get_marker_set_data( if len(all_markers_data) == 1: #print(packet.framenumber) #print(packet.timestamp) - return all_markers_data[0] #, all_occluded_data[0] + return all_markers_data[0], self.marker_name #, all_occluded_data[0] return all_markers_data #, all_occluded_data async def init_client(self): From 71cbff9b4a0fb5656842f4c0dc5316235ab9641e Mon Sep 17 00:00:00 2001 From: Ophelie Date: Thu, 17 Oct 2024 18:15:57 +0400 Subject: [PATCH 17/23] Gestion of makers names --- biosiglive/interfaces/qualisys_interface.py | 4 ++-- examples/sandox/CompuServeetQual.py | 15 ++++++++++++--- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/biosiglive/interfaces/qualisys_interface.py b/biosiglive/interfaces/qualisys_interface.py index cf4ac0a..0e5912f 100644 --- a/biosiglive/interfaces/qualisys_interface.py +++ b/biosiglive/interfaces/qualisys_interface.py @@ -424,8 +424,8 @@ def get_marker_set_data( if len(all_markers_data) == 1: #print(packet.framenumber) #print(packet.timestamp) - return all_markers_data[0], self.marker_name #, all_occluded_data[0] - return all_markers_data #, all_occluded_data + 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): """ diff --git a/examples/sandox/CompuServeetQual.py b/examples/sandox/CompuServeetQual.py index d32e781..734ff21 100644 --- a/examples/sandox/CompuServeetQual.py +++ b/examples/sandox/CompuServeetQual.py @@ -35,6 +35,10 @@ 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): @@ -46,6 +50,10 @@ def __init__(self, server_ip="192.168.0.1", port=7, self.previous_fz = 0 self.threshold = threshold + def load_markers_names(self): + tmp = load("C:\\Users\\irisse-q\\Desktop\\Florian\\DATA\\LAO_01\\Venue2\\AQM\\LAO_01_Cond0007.qtm") + return tmp['makers_names'].data[0:self.nb_markers].tolist() + async def setup(self): """ main function """ # Connection to qualisys @@ -73,8 +81,8 @@ async def setup(self): 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 + mark_tmp, mks_name = self.interface.get_marker_set_data(packet=packet) + dataforce = self.interface.get_force_plate_data(packet=packet) @@ -84,13 +92,14 @@ async def setup(self): 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 + "MarkersNames": mks_name, } # "Angle": Q[:, -1], self.server.send_data(dataAll, connection, message) From 088dffa055ab29c5caecff5941ac3fce856abf43 Mon Sep 17 00:00:00 2001 From: Ophelie Date: Fri, 18 Oct 2024 17:29:26 +0400 Subject: [PATCH 18/23] update for running with 49mksmodel --- examples/sandox/CompuServeetQual.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/sandox/CompuServeetQual.py b/examples/sandox/CompuServeetQual.py index 734ff21..861722f 100644 --- a/examples/sandox/CompuServeetQual.py +++ b/examples/sandox/CompuServeetQual.py @@ -61,7 +61,7 @@ async def setup(self): queue = asyncio.Queue() # Add info needed - n_markers = 4 + n_markers = 53 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" ) @@ -70,7 +70,7 @@ async def setup(self): device_type="force_plate", name="force_plate", data_buffer_size=100, - rate=1000, + rate=2000, device_data_file_key="force_plate", processing_method=None, moving_average=True, From 6010078588ac5a1804adad2efcbb81543d3c80f5 Mon Sep 17 00:00:00 2001 From: Ophelie Date: Mon, 2 Dec 2024 17:42:16 +0400 Subject: [PATCH 19/23] Improve get force data --- biosiglive/interfaces/qualisys_interface.py | 59 +++++++++------------ 1 file changed, 26 insertions(+), 33 deletions(-) diff --git a/biosiglive/interfaces/qualisys_interface.py b/biosiglive/interfaces/qualisys_interface.py index 0e5912f..34f27c9 100644 --- a/biosiglive/interfaces/qualisys_interface.py +++ b/biosiglive/interfaces/qualisys_interface.py @@ -263,7 +263,7 @@ async def add_marker_set( markers_tmp.data_windows = data_buffer_size self.marker_sets.append(markers_tmp) - def get_force_plate_data( + async def get_force_plate_data( self, forceplate_name: Union[str, list] = "all", get_frame: bool = True, packet=None ): if len(self.forces) == 0: @@ -272,40 +272,33 @@ def get_force_plate_data( raise RuntimeError("Qualisys client is not initialized.") if get_frame: packet.framenumber - headerf, forcesdata = packet.get_force() - 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] - 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 - - """ - Device.new_data = np.zeros((9, headerf.plate_count, packet.framenumber)) - #for frame in range(forcesdata[0][0].force_count): - for platenum in range(headerf.plate_count): - if forcesdata[platenum][0].force_count!= 0: - forcedata = forcesdata[platenum][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] - Device.new_data[:, platenum, :] = np.array(forces_data_tmp)[:, np.newaxis] - - all_forces_data.append(Device.new_data) - - if len(all_forces_data) == 1: - return all_forces_data[0] + 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 = len(PFForce) # Nombre de frames pour cette plaque + # Collecte des données + for platenum in range(nb_pf): + + # Temporaire pour cette plaque + plate_data =np.empty((9, nb_frames)) + + 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( From ae715c924d5cb4f47a0229aebb18fbe1258d6703 Mon Sep 17 00:00:00 2001 From: Ophelie Date: Mon, 2 Dec 2024 17:43:08 +0400 Subject: [PATCH 20/23] A server between Qualisys and another PC --- examples/CompuServeetQual.py | 103 +++++++++++++++++++++++ examples/sandox/CompuServeetQual.py | 126 ---------------------------- 2 files changed, 103 insertions(+), 126 deletions(-) create mode 100644 examples/CompuServeetQual.py delete mode 100644 examples/sandox/CompuServeetQual.py diff --git a/examples/CompuServeetQual.py b/examples/CompuServeetQual.py new file mode 100644 index 0000000..cb75f83 --- /dev/null +++ b/examples/CompuServeetQual.py @@ -0,0 +1,103 @@ +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() + + async def setup_interface(self): + self.interface = await QualisysClient.create(ip="192.168.0.2", 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=100, + unit="mm" + ) + + await self.interface.add_device( + nb_channels=18, + device_type="force_plate", + name="force_plate", + data_buffer_size=2000, + 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 = 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.mean(dataforce[2]) + print(current_fz) + 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/CompuServeetQual.py b/examples/sandox/CompuServeetQual.py deleted file mode 100644 index 861722f..0000000 --- a/examples/sandox/CompuServeetQual.py +++ /dev/null @@ -1,126 +0,0 @@ -""" -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 - - def load_markers_names(self): - tmp = load("C:\\Users\\irisse-q\\Desktop\\Florian\\DATA\\LAO_01\\Venue2\\AQM\\LAO_01_Cond0007.qtm") - return tmp['makers_names'].data[0:self.nb_markers].tolist() - - 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 = 53 - 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=2000, - 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, mks_name = self.interface.get_marker_set_data(packet=packet) - - - 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": 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 From 9b1571cb15b237140d911bf4358bfe4e0e2074e9 Mon Sep 17 00:00:00 2001 From: Ophelie Date: Wed, 18 Dec 2024 16:50:30 +0400 Subject: [PATCH 21/23] resolution pb pf --- biosiglive/interfaces/qualisys_interface.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/biosiglive/interfaces/qualisys_interface.py b/biosiglive/interfaces/qualisys_interface.py index 34f27c9..1c81b29 100644 --- a/biosiglive/interfaces/qualisys_interface.py +++ b/biosiglive/interfaces/qualisys_interface.py @@ -282,7 +282,7 @@ async def get_force_plate_data( nb_frames = len(PFForce) # 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)) @@ -357,7 +357,7 @@ def get_device_data( return all_device_data[0] return all_device_data - def get_marker_set_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 ): """ From a9596ea9ba29a0c80e297dc736f9e6973ba7ff98 Mon Sep 17 00:00:00 2001 From: Ophelie Date: Mon, 13 Jan 2025 12:21:14 +0400 Subject: [PATCH 22/23] upgrade pf data storage --- biosiglive/interfaces/qualisys_interface.py | 8 ++++++-- examples/CompuServeetQual.py | 20 +++++++++++++------- 2 files changed, 19 insertions(+), 9 deletions(-) diff --git a/biosiglive/interfaces/qualisys_interface.py b/biosiglive/interfaces/qualisys_interface.py index 1c81b29..34db716 100644 --- a/biosiglive/interfaces/qualisys_interface.py +++ b/biosiglive/interfaces/qualisys_interface.py @@ -279,13 +279,15 @@ async def get_force_plate_data( 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 = len(PFForce) # Nombre de frames pour cette 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] = [ @@ -298,6 +300,8 @@ async def get_force_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) + print(all_forces_data) + print("newdata") return all_forces_data diff --git a/examples/CompuServeetQual.py b/examples/CompuServeetQual.py index cb75f83..a0e7973 100644 --- a/examples/CompuServeetQual.py +++ b/examples/CompuServeetQual.py @@ -37,14 +37,19 @@ 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.0.2", system_rate=100, port=22224) + 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=100, + nb_markers=self.nb_markers, + data_buffer_size=1000, + marker_data_file_key="markers", + name="markers", + rate=self.system_rate, unit="mm" ) @@ -52,7 +57,7 @@ async def setup_interface(self): nb_channels=18, device_type="force_plate", name="force_plate", - data_buffer_size=2000, + data_buffer_size=20000, rate=2000, device_data_file_key="force_plate", processing_method=None, @@ -67,14 +72,14 @@ async def process_data(self): 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 = 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.mean(dataforce[2]) - print(current_fz) + 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.") @@ -90,6 +95,7 @@ async def process_data(self): # 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: From 58162da60968f14e4417a51927092d90707007df Mon Sep 17 00:00:00 2001 From: Ophelie Date: Mon, 20 Jan 2025 14:23:33 +0400 Subject: [PATCH 23/23] code state on the 20th of January (Charbie) --- biosiglive/interfaces/qualisys_interface.py | 3 +- examples/sandox/Example_P24.py | 22 ++++ examples/sandox/Getcurrentframe.py | 67 +++++++++++ examples/sandox/IK_Biosiglive.py | 121 ++++++++++++++++++++ examples/sandox/TMP.py | 20 ++++ examples/sandox/TestQTM.py | 103 +++++++++++++++-- examples/sandox/Test_QualisysClient.py | 8 +- 7 files changed, 330 insertions(+), 14 deletions(-) create mode 100644 examples/sandox/Example_P24.py create mode 100644 examples/sandox/Getcurrentframe.py create mode 100644 examples/sandox/IK_Biosiglive.py create mode 100644 examples/sandox/TMP.py diff --git a/biosiglive/interfaces/qualisys_interface.py b/biosiglive/interfaces/qualisys_interface.py index 34db716..fa7e4f4 100644 --- a/biosiglive/interfaces/qualisys_interface.py +++ b/biosiglive/interfaces/qualisys_interface.py @@ -300,8 +300,7 @@ async def get_force_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) - print(all_forces_data) - print("newdata") + return all_forces_data 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 index 0e5c093..f12b59f 100644 --- a/examples/sandox/TestQTM.py +++ b/examples/sandox/TestQTM.py @@ -5,11 +5,13 @@ 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 """ + Asynchronous function that processes queue until None is posted in queue LOG.info("Entering package_receiver") while True: packet = await queue.get() @@ -18,10 +20,10 @@ async def package_receiver(queue): 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) @@ -37,8 +39,10 @@ async def package_receiver(queue): for force in forces: LOG.info("\t%s", force) - LOG.info("Exiting package_receiver") + marker_plot.update(mks[:, :, -1].T, size=0.1) + LOG.info("Exiting package_receiver") +""" async def setup(): @@ -49,6 +53,7 @@ async def setup(): 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) @@ -58,19 +63,101 @@ async def setup(): 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)) - queue = asyncio.Queue() + await connection.stream_frames(components=["6d", "3d", "force"], on_packet=queue.put_nowait) - receiver_future = asyncio.ensure_future(package_receiver(queue)) + 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 - await connection.stream_frames(components=["6d","3d","force"], on_packet=queue.put_nowait) + 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 index 056b947..a514d5f 100644 --- a/examples/sandox/Test_QualisysClient.py +++ b/examples/sandox/Test_QualisysClient.py @@ -21,7 +21,7 @@ import importlib import biosiglive importlib.reload(biosiglive) -from biosiglive import LivePlot, PlotType, QualisysClient +from biosiglive import (LivePlot, PlotType, QualisysClient) import asyncio import logging import xml.etree.ElementTree as ET @@ -33,7 +33,7 @@ async def setup(): """ main function """ # Connection to qualisys - interface = await QualisysClient.create(ip="192.168.254.1", system_rate=100, port=22224) + interface = await QualisysClient.create(ip="192.168.0.2", system_rate=100, port=22224) queue = asyncio.Queue() # Add info needed @@ -88,8 +88,8 @@ async def setup(): 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[1][:3, -1:]) + 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