From 899c94c7fc558f8a4c7902a56e797264c8958a7a Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Fri, 13 Jun 2025 22:30:21 +0100 Subject: [PATCH 01/27] Added MailBox and PostOffice. Working demo with gui_thread. --- Exoboot_Postal_Service.py | 12 +- dephyEB51.py | 5 +- src/utils/actuator_utils.py | 4 +- threading_demo.py | 234 ++++++++++++++++++------------------ 4 files changed, 131 insertions(+), 124 deletions(-) diff --git a/Exoboot_Postal_Service.py b/Exoboot_Postal_Service.py index ba9462a..67675a1 100644 --- a/Exoboot_Postal_Service.py +++ b/Exoboot_Postal_Service.py @@ -3,8 +3,12 @@ def Mail(sender, contents): + """ + Returns list of dictionaries with sender name (ex. gse/gui) + and contents (ex. "20") + """ return {"sender": sender, "contents": contents} - + class MailBox: """ @@ -34,7 +38,7 @@ def getmail_all(self): break return mail - + class PostOffice: """ @@ -87,9 +91,9 @@ def f(postoffice): # Get mail and add to mailcount mymail = which.mailbox.getmail_all() which.mailcount += len(mymail) - + sleep(1.0) - + # Create PostOffice with empty addressbook postoffice = PostOffice() diff --git a/dephyEB51.py b/dephyEB51.py index 4b3efd9..9f0c7f1 100644 --- a/dephyEB51.py +++ b/dephyEB51.py @@ -131,8 +131,9 @@ def ankle_angle(self) -> float: Ankle angle is in ° and is the angle of the ankle joint using the ankle encoder. Angles should range anywhere from 0° to 140°. """ + # TODO look over ank_ang/100 versus ank_ang*ENC_CLICKS_TO_DEG if self._data is not None: - ank_ang_in_deg = self.ank_ang/100 + ank_ang_in_deg = self.ank_ang * EB51_CONSTANTS.MOT_ENC_CLICKS_TO_DEG return float( (self.ank_enc_sign * ank_ang_in_deg) - self.tr_gen.get_offset() ) else: LOGGER.debug( @@ -149,7 +150,7 @@ def update(self): self.filter_temp() # update the actuator state - super().update() + super().update_allData() # update the gear ratio self.update_gear_ratio() diff --git a/src/utils/actuator_utils.py b/src/utils/actuator_utils.py index 32a11ab..cf8b7a4 100644 --- a/src/utils/actuator_utils.py +++ b/src/utils/actuator_utils.py @@ -88,8 +88,6 @@ def create_actuators(gear_ratio:float, baud_rate:int, freq:int, debug_level:int) CONSOLE_LOGGER.info(f" MOTOR SIGN: {actuator.motor_sign}") CONSOLE_LOGGER.info(f" ANKLE SIGN: {actuator.ank_enc_sign}") - CONSOLE_LOGGER.info(" ~~ FlexSEA connection initialized, streaming & exo actuators created ~~ ") - - CONSOLE_LOGGER.info(" ~~ FlexSEA connection initialized, streaming & exo actuators created ~~ ") + return actuators diff --git a/threading_demo.py b/threading_demo.py index 2dfbe17..bc541be 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -9,7 +9,16 @@ import queue -# TODO: add logging event +from src.utils.filing_utils import get_logging_info +from opensourceleg.logging import Logger, LogLevel +CONSOLE_LOGGER = Logger(enable_csv_logging=False, + log_path=get_logging_info(user_input_flag=False)[0], + stream_level = LogLevel.INFO, + log_format = "%(levelname)s: %(message)s" + ) + +from Exoboot_Postal_Service import PostOffice + class BaseWorkerThread(threading.Thread, ABC): """ Abstract base class for worker threads in the exoskeleton system. @@ -21,27 +30,28 @@ class BaseWorkerThread(threading.Thread, ABC): def __init__(self, quit_event:Type[threading.Event], pause_event:Type[threading.Event], - name:Optional[str]=None, + log_event:Type[threading.Event], + name:str, frequency:int=100)->None: super().__init__(name=name) self.quit_event = quit_event # event to signal quitting self.pause_event = pause_event # event to signal pausing + self.log_event = log_event # event to signal logging self.daemon = True self.frequency = int(frequency) self.rt_loop = FlexibleSleeper(dt=1/frequency) # create a soft real-time loop with the specified frequency - # set-up a logger for each thread/instance - logger_name = f"{name}_logger" - log_path = "./src/logs/" - self.data_logger = NonSingletonLogger( - log_path=log_path, - file_name=logger_name, - file_level=logging.DEBUG, - stream_level=logging.INFO - ) + # logger_name = f"{name}_logger" + # log_path = "./src/logs/" + # self.data_logger = NonSingletonLogger( + # log_path=log_path, + # file_name=logger_name, + # file_level=logging.DEBUG, + # stream_level=logging.INFO + # ) def run(self)->None: """ @@ -49,7 +59,7 @@ def run(self)->None: """ LOGGER.debug(f"[{self.name}] Thread running.") - while not self.quit_event.is_set(): # while not quitting + while self.quit_event.is_set(): # while not quitting self.pre_iterate() # run a pre-iterate method if not self.pause_event.is_set(): @@ -63,7 +73,6 @@ def run(self)->None: self.post_iterate() # run a post-iterate method self.rt_loop.pause() - LOGGER.debug(f"[{self.name}] Thread exiting.") @abstractmethod @@ -111,15 +120,18 @@ class ActuatorThread(BaseWorkerThread): This class handles the actuator's state updates and manages the actuator's control loop. """ def __init__(self, + name: str, actuator, - data_queue:queue, + post_office, quit_event: Type[threading.Event], pause_event: Type[threading.Event], - name: Optional[str] = None, + log_event: Type[threading.Event], frequency: int = 100) -> None: - self.actuator_queue = data_queue # Queue for inter-thread communication - super().__init__(quit_event, pause_event, name=name or actuator.side, frequency=frequency) + super().__init__(quit_event, pause_event, log_event, name, frequency=frequency) + + self.post_office = post_office + self.mailbox = None self.actuator = actuator @@ -149,7 +161,9 @@ def enqueue_actuator_states(self)-> None: }) def post_iterate(self)->None: - pass + # TODO add rest of stuff + if self.log_event.is_set(): + LOGGER.debug(f"[{self.name}] log_event True") def on_pause(self)->None: pass @@ -161,14 +175,17 @@ class GaitStateEstimatorThread(BaseWorkerThread): This class handles gait state estimation for BOTH exoskeletons/sides together. """ def __init__(self, - data_queue:queue, + post_office, quit_event:Type[threading.Event], pause_event:Type[threading.Event], + log_event: Type[threading.Event], name:Optional[str] = None, frequency:int=100)->None: - self.gse_queue = data_queue - super().__init__(quit_event, pause_event, name=name, frequency=frequency) + super().__init__(quit_event, pause_event, log_event, name=name, frequency=frequency) + + self.post_office = post_office + self.mailbox = None # instantiate the walking simulator self.walker = WalkingSimulator(stride_period=1.20) @@ -232,19 +249,43 @@ class GUICommunication(BaseWorkerThread): It then sends these setpoints to the actuators to handle. """ def __init__(self, - data_queue:queue, + post_office, quit_event:Type[threading.Event], pause_event:Type[threading.Event], + log_event: Type[threading.Event], name:Optional[str] = None, frequency:int=100)->None: - self.gui_queue = data_queue - super().__init__(quit_event, pause_event, name=name, frequency=frequency) + super().__init__(quit_event, pause_event, log_event, name=name, frequency=frequency) + + self.post_office = post_office + self.mailbox = None self.torque_setpoint = 20.0 def pre_iterate(self)->None: - pass + # read mail regardless of paused state + mail_list = self.mailbox.getmail_all() # return list of mail + CONSOLE_LOGGER.debug(f"mail received in GUICommunication thread {len(mail_list)}") + + # unpackage the mail + for mail in mail_list: + self.decode_message(mail) + print(f"torque setpoint is:{self.torque_setpoint}") + + def decode_message(self, mail:list) -> Optional[Any]: + """ + Decode a message from the mailbox. + """ + try: + # example contents {"peak_torque": 30.1, "stride_period": 1.3} + for key, value in mail["contents"].items(): + #TODO value validation (not None or something) + setattr(self, key, value) + + except Exception as err: + LOGGER.debug("Error decoding message:", err) + return None def iterate(self): """ @@ -254,14 +295,7 @@ def iterate(self): """ # Put the torque setpoint into the queue for the main thread - self.enqueue_GUI_input() - - def enqueue_GUI_input(self)-> None: - """ - Stores a dict of GUI inputs in the queue for the main thread. - This method is called to send the GUI input to the main thread. - """ - self.gui_queue.put({"torque_setpoint": self.torque_setpoint}) + pass def post_iterate(self)->None: pass @@ -279,22 +313,24 @@ def on_pause(self)->None: from opensourceleg.actuators.dephy import DEFAULT_CURRENT_GAINS class DephyExoboots(RobotBase[DephyEB51Actuator, SensorBase]): - def __init__(self, *args: Any, **kwargs: Any) -> None: + def __init__(self, tag, actuators, sensors, post_office) -> None: """ Exoboot Robot class that extends RobotBase. This class manages thread creation, data queue creation & management, as well as basic actuator control. It is designed to handle multiple actuators and their respective threads, as well as a Gait State Estimator (GSE) thread and a GUI communication thread. """ - super().__init__(*args, **kwargs) + super().__init__(tag=tag, actuators=actuators, sensors=sensors) self._threads = {} self._quit_event = threading.Event() # Event to signal threads to quit. self._pause_event = threading.Event() # Event to signal threads to pause. + self._log_event = threading.Event() # Event to signal threads to log + self.post_office = post_office # PostOffice class instance - self._pause_event.set() # set to un-paused by default - - # create a dictionary of queues for inter-thread communication - self.queues: Dict[str, queue.Queue] = {} + # Thread event inits + self._quit_event.set() # exo is running + self._pause_event.clear() # exo starts paused + self._log_event.clear() # exo starts not logging def start(self) -> None: """Start actuator threads""" @@ -308,7 +344,7 @@ def start(self) -> None: # creating and starting threads for each actuator self.initialize_actuator_thread(actuator) - LOGGER.debug(f"Started actuator thread for {actuator.side}") + CONSOLE_LOGGER.debug(f"Started actuator thread for {actuator.side}") # creating 1 thread for the Gait State Estimator self.initialize_GSE_thread() @@ -338,59 +374,21 @@ def create_interthread_queue(self, name:str, max_size:int=0) -> queue.Queue: return self.queues[name] - def decode_message(self, name: str, key_list: Optional[list[str]] = None) -> Optional[Any]: - """ - Try to receive a message from named queues. Names are: "gse_queue", "gui_queue", "left_exo_queue", "right_exo_queue". - If key_list is provided as a list of keys, return a tuple of those values (even if length 1). - If key_list is a single string, return just that value. - If key_list is None, return the full message dict. - - Args: - name (str): The name of the queue to receive from. - key_list (list[str] or None): List of keys to extract from the message dict. If None, return the full message. - Returns: - Any: The received value(s) or None if no message is available. - """ - # get the queue from the dictionary using the name - messenger_queue = self.queues.get(name) - - if messenger_queue is None: - raise ValueError(f"No messenger queue named {name}") - - # if queue name does exist, try to get a message from it - try: - msg = messenger_queue.get_nowait() - - # if key_list provided, check whether it is a list or single string - if key_list is not None: - if isinstance(key_list, str): - return msg.get(key_list) # return just that requested value - elif isinstance(key_list, list): - return tuple(msg.get(k) for k in key_list) # return a tuple of requested values - else: - return msg # if no key_list provided, return the full message dict - - except queue.Empty: - print(f"No new message in {name} queue.") - return None - def initialize_actuator_thread(self, actuator: DephyEB51Actuator) -> None: """ Create and start a thread for the specified actuator. This method is called to set up the actuator communication thread. """ - # create a FIFO queue with max size for inter-thread communication - actuator_queue = self.create_interthread_queue(actuator.side, max_size=0) actuator_thread = ActuatorThread(actuator=actuator, - data_queue=actuator_queue, quit_event=self._quit_event, pause_event=self._pause_event, + log_event=self._log_event, name=f"{actuator.side}", frequency=1, + post_office=self.post_office, ) - actuator_thread.start() LOGGER.debug(f"Started actuator thread for {actuator.side}") self._threads[actuator.side] = actuator_thread @@ -401,15 +399,13 @@ def initialize_GSE_thread(self) -> None: """ # create a FIFO queue with max size for inter-thread communication name = "gse" - gse_queue = self.create_interthread_queue(name, max_size=0) - gse_thread = GaitStateEstimatorThread(quit_event=self._quit_event, pause_event=self._pause_event, + log_event=self._log_event, name=name, frequency=1, - data_queue=gse_queue) + post_office=self.post_office) - gse_thread.start() LOGGER.debug(f"Started gse thread") self._threads[name] = gse_thread @@ -421,14 +417,12 @@ def initialize_GUI_thread(self) -> None: # create a FIFO queue with max size for inter-thread communication name = "gui" - gui_queue = self.create_interthread_queue(name, max_size=0) - gui_thread = GUICommunication(quit_event=self._quit_event, pause_event=self._pause_event, + log_event=self._log_event, name=name, frequency=1, - data_queue=gui_queue) - gui_thread.start() + post_office=self.post_office,) LOGGER.debug(f"Started gui thread") self._threads[name] = gui_thread @@ -452,6 +446,12 @@ def update(self)->None: """Required by RobotBase, but passing since ActuatorThread handles iterative exo state updates""" pass + def return_active_threads(self)->list: + """ + Return list of active thread addresses. + """ + return self._threads.values() + @property def left_exo_queue(self) -> Dict[str, queue.Queue]: """Get the queue for the left actuator""" @@ -475,12 +475,22 @@ def gui_queue(self) -> Dict[str, queue.Queue]: @property def left_thread(self) -> Optional[ActuatorThread]: """Get the left actuator thread""" - return self._actuator_threads.get("left") + return self._threads.get("left") @property def right_thread(self) -> Optional[ActuatorThread]: """Get the right actuator thread""" - return self._actuator_threads.get("right") + return self._threads.get("right") + + @property + def gse_thread(self) -> Optional[ActuatorThread]: + """Get the right actuator thread""" + return self._threads.get("gse") + + @property + def gui_thread(self) -> Optional[ActuatorThread]: + """Get the right actuator thread""" + return self._threads.get("gui") # Example main loop from opensourceleg.utilities import SoftRealtimeLoop @@ -489,39 +499,33 @@ def right_thread(self) -> Optional[ActuatorThread]: from src.utils.walking_simulator import WalkingSimulator if __name__ == '__main__': + + post_office = PostOffice() + actuators = create_actuators(1, EXO_SETUP_CONST.BAUD_RATE, EXO_SETUP_CONST.FLEXSEA_FREQ, EXO_SETUP_CONST.LOG_LEVEL) - exoboots = DephyExoboots(tag="exoboots", actuators=actuators, sensors={}) + exoboots = DephyExoboots(tag="exoboots", + actuators=actuators, + sensors={}, + post_office=post_office) clock = SoftRealtimeLoop(dt = 1 / 1) # Hz with exoboots: + # set-up addressbook for the PostOffice + post_office.setup_addressbook(*exoboots.return_active_threads()) + + # TODO add start_threads method to DephyExoboots class + # exoboots.left_thread.start() + exoboots.right_thread.start() + exoboots.gse_thread.start() + exoboots.gui_thread.start() + CONSOLE_LOGGER.debug("All threads started.") for t in clock: try: - # report current gait state using simulated walking in gse thread - gse_msg = exoboots.decode_message("gse", key_list=["time_in_stride", "percent_gc"]) - if gse_msg is not None: - time_in_stride, percent_gc = gse_msg # unpackage the message - print(f"Received gait state: time_in_stride={time_in_stride}, percent_gc={percent_gc}") - else: - print("No new gait state received.") - - # report PEAK torque setpoint using gui thread - torque = exoboots.decode_message("gui", key_list="torque_setpoint") - if torque is not None: - print(f"Received torque setpoint: {torque}") - else: - print("No new torque setpoint received.") - - # report actuator state from each actuator thread queue - for actuator in exoboots.actuators.values(): - actuator_msg = exoboots.decode_message(actuator.side, key_list=["motor_current", "temperature"]) - if actuator_msg is not None: - current_setpoint, temperature = actuator_msg - print(f"Received {actuator.side} actuator state: current_setpoint={current_setpoint}, temperature={temperature}") - else: - print(f"No new {actuator.side} actuator state received.") - + # send mail to the GUI thread + post_office.send(sender="main", recipient="gui", contents={"torque_setpoint": 30.0}) + print("Main sent") # TODO: pass reported values into AssistanceGenerator class -> method is part of Exoboots Robot class # TODO: determine appropriate torque setpoint given current gait state From b3086b08eb125dbf36c45f13e05ee0ed0060b4e7 Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Sat, 14 Jun 2025 19:53:30 +0100 Subject: [PATCH 02/27] added tests for Exboot_Postal_Service.py methods. Added docstrings, and type hints to file. Also changed Mail to a dataclass, added exceptions for edge cases, as well as console logger. Also added threading lock to prevent multiple threads from accessing the addressbook and a barrier to wait for all threads to start at the same time before sending messages. All of this is contained to the Postal_Service.py file --- Exoboot_Postal_Service.py | 232 ++++++++++++++++++++++++----------- Exoboot_Postal_Service_OG.py | 118 ++++++++++++++++++ exoboots.py | 3 +- tests/test_postal_service.py | 58 +++++++++ tests/test_system.py | 52 +------- threading_demo.py | 6 +- 6 files changed, 344 insertions(+), 125 deletions(-) create mode 100644 Exoboot_Postal_Service_OG.py create mode 100644 tests/test_postal_service.py diff --git a/Exoboot_Postal_Service.py b/Exoboot_Postal_Service.py index 67675a1..c9d2ab2 100644 --- a/Exoboot_Postal_Service.py +++ b/Exoboot_Postal_Service.py @@ -1,122 +1,214 @@ from queue import Queue, Empty -from threading import Thread, current_thread - - -def Mail(sender, contents): +from threading import Thread, current_thread, Lock, Barrier +from dataclasses import dataclass +from typing import Any, Dict, List, Optional +from time import sleep +from random import randint + +# Configure logging +from src.utils.filing_utils import get_logging_info +from opensourceleg.logging import Logger, LogLevel +CONSOLE_LOGGER = Logger(enable_csv_logging=False, + log_path=get_logging_info(user_input_flag=False)[0], + stream_level = LogLevel.INFO, + log_format = "%(levelname)s: %(message)s" + ) + + +@dataclass() +class Mail: """ - Returns list of dictionaries with sender name (ex. gse/gui) - and contents (ex. "20") + Represents a mail message with a sender and contents. """ - return {"sender": sender, "contents": contents} - + sender: str + contents: Any class MailBox: """ - TBD Description + MailBox receives and stores mail for a given address (thread). + The mailbox belongs to this thread and is used to recieve messages directed to it. - Receive mail from other addresses + Args: + address (str): The address of the mailbox, typically the thread name. """ - def __init__(self, address): - self.address = address - self.incoming_mail = Queue() + def __init__(self, address: str) -> None: + """ + Initialize a MailBox with a given address. + """ + self.address: str = address + self._incoming_mail: Queue = Queue() - def receive(self, mail): + def receive(self, mail: Mail) -> None: """ - Put mail in queue + Put mail in the mailbox queue. + Args: + mail (Mail): The mail object to be received. """ - self.incoming_mail.put_nowait(mail) + CONSOLE_LOGGER.info(f"Mail received at {self.address}: {mail}") + self._incoming_mail.put_nowait(mail) - def getmail_all(self): + def getmail_all(self) -> List[Mail]: """ - Return all mail in queue + Retrieve and remove all mail from the mailbox queue. + Returns: + List[Mail]: All mail currently in the mailbox. """ - mail = [] + mail: List[Mail] = [] + + # retrive all mail from queue until empty + # handles situation where data is continuously added to the queue while True: try: - mail.append(self.incoming_mail.get_nowait()) + mail.append(self._incoming_mail.get_nowait()) except Empty: break - + CONSOLE_LOGGER.info(f"All mail retrieved from {self.address}") return mail - class PostOffice: """ - TBD Description + This PostOffice class is used to coordinate inter-thread communication + via queues. It's primary function is to send messages (mail) from one thread + to another thread. - Send mail to other addresses(threads) + One PostOffice should be created and shared between all threads. + Each thread has its own mailbox, and the postoffice acts as a central hub + for sending and receiving messages between these mailboxes. """ - def __init__(self): - self.addressbook = {} + def __init__(self) -> None: + self._addressbook: Dict[str, MailBox] = {} + self._lock = Lock() # lock to ensure thread-safe access to the addressbook - def setup_addressbook(self, *threads): + def setup_addressbook(self, *threads: Thread) -> None: """ - Set up thread in addressbook with a corresponding mailbox + Set up threads in the addressbook with corresponding mailboxes. + To access threads' mailboxes, use the thread's `mailbox` attribute. + Args: + *threads (Thread): Threads to register in the addressbook. """ - for thread in threads: - mailbox = MailBox(thread.name) - thread.mailbox = mailbox - self.addressbook[thread.name] = mailbox - def send(self, sender, recipient, contents): + with self._lock: + for thread in threads: + + if hasattr(thread, 'mailbox') and thread.mailbox is not None: + # if thread already has a mailbox, skip + CONSOLE_LOGGER.info(f"Mailbox already exists for thread: {thread.name}") + continue + + elif not isinstance(thread, Thread): + # if thread is not a Thread instance, raise error + raise TypeError("Addressbook expected a Thread instance.") + + elif thread.name in self._addressbook: + # if thread name already exists in addressbook, skip + CONSOLE_LOGGER.warning(f"Thread name -- {thread.name} -- already exists in addressbook.") + continue + + elif not thread.name: + # if thread does not have a name, raise error + raise ValueError("Thread must have a name.") + + # instantiate a mailbox for each thread + mailbox = MailBox(thread.name) + thread.mailbox = mailbox + + # create dictionary mapping thread name to it's mailbox + self._addressbook[thread.name] = mailbox + CONSOLE_LOGGER.info(f"Mailbox set up for thread: {thread.name}") + + def send(self, sender: str, recipient: str, contents: Any) -> None: """ - Send mail to recipient - Mail needs to have a sender and contents to be complete + Send mail to a recipient's mailbox. + + Args: + sender (str): Name of the sender. + recipient (str): Name of the recipient thread. + contents (Any): The contents of the mail. + Raises: + KeyError: If the recipient does not exist in the addressbook. """ + + # package mail into a Mail object mail = Mail(sender, contents) - self.addressbook[recipient].receive(mail) + # to ensure that only one thread can access addressbook at a time + with self._lock: + # raise error if recipient is not in addressbook + if recipient not in self._addressbook: + raise KeyError(f"Recipient '{recipient}' not found in addressbook.") -if __name__ == "__main__": - from time import sleep - from random import randint + self._addressbook[recipient].receive(mail) + CONSOLE_LOGGER.info(f"Mail sent from {sender} to {recipient}: {mail.contents}") - def f(postoffice): - """ - Target function sends random amount of mail and counts amount of mail received at 1Hz - """ - which = current_thread() - which.mailcount = 0 +def f(postoffice, barrier): + """ + Target function sends random amount of mail and counts amount of mail received at 1Hz. + Waits at the barrier until all threads are ready before sending mail. + """ + which = current_thread() + which.mailcount = 0 + CONSOLE_LOGGER.info("Started " + which.name) - print("Started ", which.name) - while True: - # Send a random amount of mail to other addresses in the postoffice addressbook - for _ in range(randint(2, 5)): - try: - random_recipient = "thread" + str(randint(1, 3)) - postoffice.send(which.name, random_recipient, f"Hi from {which}") - except: - pass + # Wait for all threads to be ready before sending mail + barrier.wait() + + while True: + # Send a random amount of mail to other addresses in the postoffice addressbook + for _ in range(randint(2, 5)): + try: + random_recipient = "thread" + str(randint(1, 3)) - # Get mail and add to mailcount - mymail = which.mailbox.getmail_all() - which.mailcount += len(mymail) + # ensure that the recipient is not the same as the sender + if random_recipient == which.name: + continue - sleep(1.0) + postoffice.send(which.name, random_recipient, f"Hi from {which.name} to {random_recipient}!") + except: + pass + + # Get mail and add to mailcount + mymail = which.mailbox.getmail_all() + which.mailcount += len(mymail) + + sleep(1.0) + +if __name__ == "__main__": # Create PostOffice with empty addressbook postoffice = PostOffice() - # Create threads - thread1 = Thread(target=f, args=(postoffice,), daemon=True, name="thread1") - thread2 = Thread(target=f, args=(postoffice,), daemon=True, name="thread2") - thread3 = Thread(target=f, args=(postoffice,), daemon=True, name="thread3") + # Set the number of threads + num_threads = 3 + + # Create threads with unique names + thread_names = [f"thread{i+1}" for i in range(num_threads)] + threads = [] + for name in thread_names: + threads.append(Thread(target=f, args=(postoffice, None), daemon=True, name=name)) + + # Create a barrier for all threads + barrier = Barrier(num_threads) + + # Assign the barrier to each thread's args + for thread in threads: + thread._args = (postoffice, barrier) # Populate addressbook with threads - postoffice.setup_addressbook(thread1, thread2, thread3) + postoffice.setup_addressbook(*threads) - # Start threads - thread1.start() - thread2.start() - thread3.start() + # Start all threads + for thread in threads: + thread.start() # Print out mailflow summary every 2 seconds while True: try: - print("\nMail Summary") - for thread in [thread1, thread2, thread3]: - print(thread.name, thread.mailcount) + CONSOLE_LOGGER.info("\nMail Summary\n") + for thread in threads: + CONSOLE_LOGGER.info(f"{thread.name} {thread.mailcount}") thread.mailcount = 0 sleep(2.0) + except KeyboardInterrupt: + CONSOLE_LOGGER.info("Shutting down mail summary loop.") break diff --git a/Exoboot_Postal_Service_OG.py b/Exoboot_Postal_Service_OG.py new file mode 100644 index 0000000..ba9462a --- /dev/null +++ b/Exoboot_Postal_Service_OG.py @@ -0,0 +1,118 @@ +from queue import Queue, Empty +from threading import Thread, current_thread + + +def Mail(sender, contents): + return {"sender": sender, "contents": contents} + + +class MailBox: + """ + TBD Description + + Receive mail from other addresses + """ + def __init__(self, address): + self.address = address + self.incoming_mail = Queue() + + def receive(self, mail): + """ + Put mail in queue + """ + self.incoming_mail.put_nowait(mail) + + def getmail_all(self): + """ + Return all mail in queue + """ + mail = [] + while True: + try: + mail.append(self.incoming_mail.get_nowait()) + except Empty: + break + + return mail + + +class PostOffice: + """ + TBD Description + + Send mail to other addresses(threads) + """ + def __init__(self): + self.addressbook = {} + + def setup_addressbook(self, *threads): + """ + Set up thread in addressbook with a corresponding mailbox + """ + for thread in threads: + mailbox = MailBox(thread.name) + thread.mailbox = mailbox + self.addressbook[thread.name] = mailbox + + def send(self, sender, recipient, contents): + """ + Send mail to recipient + Mail needs to have a sender and contents to be complete + """ + mail = Mail(sender, contents) + self.addressbook[recipient].receive(mail) + + +if __name__ == "__main__": + from time import sleep + from random import randint + + def f(postoffice): + """ + Target function sends random amount of mail and counts amount of mail received at 1Hz + """ + which = current_thread() + which.mailcount = 0 + + print("Started ", which.name) + while True: + # Send a random amount of mail to other addresses in the postoffice addressbook + for _ in range(randint(2, 5)): + try: + random_recipient = "thread" + str(randint(1, 3)) + postoffice.send(which.name, random_recipient, f"Hi from {which}") + except: + pass + + # Get mail and add to mailcount + mymail = which.mailbox.getmail_all() + which.mailcount += len(mymail) + + sleep(1.0) + + # Create PostOffice with empty addressbook + postoffice = PostOffice() + + # Create threads + thread1 = Thread(target=f, args=(postoffice,), daemon=True, name="thread1") + thread2 = Thread(target=f, args=(postoffice,), daemon=True, name="thread2") + thread3 = Thread(target=f, args=(postoffice,), daemon=True, name="thread3") + + # Populate addressbook with threads + postoffice.setup_addressbook(thread1, thread2, thread3) + + # Start threads + thread1.start() + thread2.start() + thread3.start() + + # Print out mailflow summary every 2 seconds + while True: + try: + print("\nMail Summary") + for thread in [thread1, thread2, thread3]: + print(thread.name, thread.mailcount) + thread.mailcount = 0 + sleep(2.0) + except KeyboardInterrupt: + break diff --git a/exoboots.py b/exoboots.py index 1f67ecf..6b92835 100644 --- a/exoboots.py +++ b/exoboots.py @@ -56,7 +56,6 @@ def update(self) -> None: # print(f"Updating exoskeleton robot: {self.tag}") super().update() - def setup_control_modes(self) -> None: """ Call the setup_controller method for all actuators. @@ -288,7 +287,7 @@ def update_rt_plots(self, bertec_swing_flag, imu_activations) -> list: - Bertec In swing - Transmission Ratio - IMU estimator activations - + The data is collected from the exoboots object and returned as a list of arrays. This is done for each active actuator only. diff --git a/tests/test_postal_service.py b/tests/test_postal_service.py new file mode 100644 index 0000000..c142284 --- /dev/null +++ b/tests/test_postal_service.py @@ -0,0 +1,58 @@ +import pytest +from Exoboot_Postal_Service import Mail, MailBox, PostOffice + +class DummyThread: + def __init__(self, name: str): + self.name: str = name + self.mailbox: MailBox | None = None + +def test_mail_function() -> None: + """ + Test the Mail function. + Verifies that Mail returns a dictionary with the correct sender and contents. + """ + mail = Mail('sender1', 'hello') + assert mail == {'sender': 'sender1', 'contents': 'hello'} + +def test_mailbox_receive_and_getmail_all() -> None: + """ + Test MailBox.receive and MailBox.getmail_all. + Ensures that received mail is stored and getmail_all returns all mail, then empties the mailbox. + """ + mailbox = MailBox('address1') + mail1 = Mail('sender1', 'msg1') + mail2 = Mail('sender2', 'msg2') + mailbox.receive(mail1) + mailbox.receive(mail2) + all_mail = mailbox.getmail_all() + assert mail1 in all_mail + assert mail2 in all_mail + assert mailbox.getmail_all() == [] # Should be empty after reading + +def test_postoffice_send_and_setup_addressbook() -> None: + """ + Test PostOffice.setup_addressbook and PostOffice.send. + Checks that threads are added to the addressbook and mail is delivered to the correct mailbox. + """ + postoffice = PostOffice() + t1 = DummyThread('thread1') + t2 = DummyThread('thread2') + postoffice.setup_addressbook(t1, t2) + assert 'thread1' in postoffice.addressbook + assert 'thread2' in postoffice.addressbook + postoffice.send('thread1', 'thread2', 'test-message') + received = t2.mailbox.getmail_all() + assert received[0]['sender'] == 'thread1' + assert received[0]['contents'] == 'test-message' + assert t2.mailbox.getmail_all() == [] + +def test_postoffice_send_to_nonexistent() -> None: + """ + Test PostOffice.send error handling. + Ensures that sending mail to a nonexistent recipient raises a KeyError. + """ + postoffice = PostOffice() + t1 = DummyThread('thread1') + postoffice.setup_addressbook(t1) + with pytest.raises(KeyError): + postoffice.send('thread1', 'threadX', 'fail') diff --git a/tests/test_system.py b/tests/test_system.py index 1d3df8d..0405e35 100644 --- a/tests/test_system.py +++ b/tests/test_system.py @@ -1,6 +1,5 @@ import pytest import threading -import zmq import os import time from unittest import mock @@ -8,7 +7,7 @@ # Import modules to be tested from dephyEB51 import DephyEB51Actuator from non_singleton_logger import NonSingletonLogger -from threading_demo import ZMQManager, DephyExoboots +from threading_demo import DephyExoboots # --- Logging Tests --- @@ -56,53 +55,6 @@ def test_per_thread_logging(tmp_path): assert "Thread1 log" in f1.read() assert "Thread2 log" in f2.read() -# --- ZMQ Tests --- - -def test_zmq_manager_socket_setup_and_close(): - """ - Test that a ZMQManager can set up a SUB socket and close it properly. - Verifies that the socket is registered and the context is closed after cleanup. - """ - zmq_manager = ZMQManager() - zmq_manager.setup_sub_socket("test", "inproc://pytest") - assert "test" in zmq_manager.sockets - zmq_manager.close() - assert zmq_manager.context.closed - -def test_zmq_pub_sub_communication(): - """ - Test that a ZMQ PUB socket can send a message and a SUB socket can receive it. - Verifies inter-thread communication using ZeroMQ inproc transport. - """ - ctx = zmq.Context.instance() - pub = ctx.socket(zmq.PUB) - sub = ctx.socket(zmq.SUB) - addr = "inproc://pytest_comm" - pub.bind(addr) - sub.connect(addr) - sub.setsockopt_string(zmq.SUBSCRIBE, "") - time.sleep(0.1) # Allow sockets to connect - test_msg = {"foo": 123} - pub.send_pyobj(test_msg) - time.sleep(0.1) - try: - msg = sub.recv_pyobj(flags=zmq.NOBLOCK) - assert msg == test_msg - except zmq.Again: - pytest.fail("No message received on ZMQ SUB socket") - finally: - pub.close() - sub.close() - ctx.term() - -def test_zmq_manager_cleanup(): - """ - Test that the ZMQManager can be set up and closed without raising any exceptions. - Ensures resource cleanup works as expected. - """ - zmq_manager = ZMQManager() - zmq_manager.setup_sub_socket("test", "inproc://pytest_cleanup") - zmq_manager.close() # Should not raise any exceptions # --- EB51 Actuator Functionality Tests --- @@ -231,7 +183,7 @@ def __init__(self, tag=None, offline=True): self.tag = tag monkeypatch.setattr('dephyEB51.DephyEB51Actuator', DummyActuator) actuators = {"left": DummyActuator(tag="left", offline=True)} - exoboots = DephyExoboots(tag="pytest_exo", actuators=actuators, sensors={}) + exoboots = DephyExoboots(tag="pytest_exo", actuators=actuators, sensors={}, post_office=None) actuators["right"] = DummyActuator(tag="right", offline=True) exoboots.actuators = actuators assert "right" in exoboots.actuators diff --git a/threading_demo.py b/threading_demo.py index bc541be..cc04cd8 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -1,14 +1,14 @@ # Abstract Base class for threading import threading -import logging +import queue + from typing import Type, Dict, Any, Optional from abc import ABC, abstractmethod from src.utils.flexible_sleeper import FlexibleSleeper from non_singleton_logger import NonSingletonLogger from dephyEB51 import DephyEB51Actuator -import queue - +# logging setup from src.utils.filing_utils import get_logging_info from opensourceleg.logging import Logger, LogLevel CONSOLE_LOGGER = Logger(enable_csv_logging=False, From 62eebdc77845248bfce3281d417dfdf747777256 Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Sun, 15 Jun 2025 22:16:46 +0100 Subject: [PATCH 03/27] modified logging to better track Postal Service Performance. Seems like it is sending, receiving messages appropriately. --- Exoboot_Postal_Service.py | 67 +++++++++++++++----- Exoboot_Postal_Service_OG.py | 118 ----------------------------------- 2 files changed, 52 insertions(+), 133 deletions(-) delete mode 100644 Exoboot_Postal_Service_OG.py diff --git a/Exoboot_Postal_Service.py b/Exoboot_Postal_Service.py index c9d2ab2..b3750fb 100644 --- a/Exoboot_Postal_Service.py +++ b/Exoboot_Postal_Service.py @@ -2,6 +2,9 @@ from threading import Thread, current_thread, Lock, Barrier from dataclasses import dataclass from typing import Any, Dict, List, Optional + +import time +import random from time import sleep from random import randint @@ -14,14 +17,18 @@ log_format = "%(levelname)s: %(message)s" ) - @dataclass() class Mail: """ - Represents a mail message with a sender and contents. + Mail is a simple data structure that contains the sender and contents of a message. + + Args: + sender (str): The name of the sending thread. + contents (dict): The contents of the mail, typically a dictionary. """ sender: str - contents: Any + contents: Dict[str, Any] + class MailBox: """ @@ -44,7 +51,7 @@ def receive(self, mail: Mail) -> None: Args: mail (Mail): The mail object to be received. """ - CONSOLE_LOGGER.info(f"Mail received at {self.address}: {mail}") + CONSOLE_LOGGER.info(f" Mail put into {self.address} mailbox: {mail}") self._incoming_mail.put_nowait(mail) def getmail_all(self) -> List[Mail]: @@ -53,6 +60,7 @@ def getmail_all(self) -> List[Mail]: Returns: List[Mail]: All mail currently in the mailbox. """ + mail: List[Mail] = [] # retrive all mail from queue until empty @@ -65,6 +73,7 @@ def getmail_all(self) -> List[Mail]: CONSOLE_LOGGER.info(f"All mail retrieved from {self.address}") return mail + class PostOffice: """ This PostOffice class is used to coordinate inter-thread communication @@ -116,14 +125,14 @@ def setup_addressbook(self, *threads: Thread) -> None: self._addressbook[thread.name] = mailbox CONSOLE_LOGGER.info(f"Mailbox set up for thread: {thread.name}") - def send(self, sender: str, recipient: str, contents: Any) -> None: + def send(self, sender: str, recipient: str, contents: dict) -> None: """ Send mail to a recipient's mailbox. Args: sender (str): Name of the sender. recipient (str): Name of the recipient thread. - contents (Any): The contents of the mail. + contents (dict): The contents of the mail (must be a dictionary). Raises: KeyError: If the recipient does not exist in the addressbook. """ @@ -137,12 +146,17 @@ def send(self, sender: str, recipient: str, contents: Any) -> None: if recipient not in self._addressbook: raise KeyError(f"Recipient '{recipient}' not found in addressbook.") + + CONSOLE_LOGGER.info(f"{sender} sending to {recipient}") + CONSOLE_LOGGER.info(f" Message contents: {mail}") + + # send mail to recipient's mailbox self._addressbook[recipient].receive(mail) - CONSOLE_LOGGER.info(f"Mail sent from {sender} to {recipient}: {mail.contents}") + def f(postoffice, barrier): """ - Target function sends random amount of mail and counts amount of mail received at 1Hz. + This target function sends random amount of mail and counts amount of mail received at 1Hz. Waits at the barrier until all threads are ready before sending mail. """ which = current_thread() @@ -153,25 +167,48 @@ def f(postoffice, barrier): barrier.wait() while True: - # Send a random amount of mail to other addresses in the postoffice addressbook + # Random delay before sending mail to increase race condition likelihood + sleep(random.uniform(0, 0.05)) + for _ in range(randint(2, 5)): try: - random_recipient = "thread" + str(randint(1, 3)) + random_recipient = "thread" + str(randint(1, barrier.parties)) # ensure that the recipient is not the same as the sender if random_recipient == which.name: continue - postoffice.send(which.name, random_recipient, f"Hi from {which.name} to {random_recipient}!") - except: + # Create a unique message with a timestamp and message ID + elapsed = time.time() - START_TIME + unique_msg = { + "message": f"sending...", + "timestamp": f"{elapsed:0.2}", + "msg_id": f"{which.name}_{random_recipient}" + } + + # Random delay before sending + sleep(random.uniform(0, 0.02)) + + # send the mail to the thread's mailbox + postoffice.send(which.name, random_recipient, unique_msg) + except Exception as e: + CONSOLE_LOGGER.error(f"{which.name} failed to send to {random_recipient}: {e}") pass - # Get mail and add to mailcount + # After sending mail, it retrieves all mail from the current thread's mailbox mymail = which.mailbox.getmail_all() + for mail in mymail: + CONSOLE_LOGGER.info(f"unpacking {which.name} mailbox") + CONSOLE_LOGGER.info(f" {mail.sender} had sent this: {mail.contents}") which.mailcount += len(mymail) + CONSOLE_LOGGER.info(f"\n") sleep(1.0) + + +START_TIME = time.time() + if __name__ == "__main__": # Create PostOffice with empty addressbook @@ -200,12 +237,12 @@ def f(postoffice, barrier): for thread in threads: thread.start() - # Print out mailflow summary every 2 seconds + # Print out mail flow summary every 0.5 Hz (slower than threads) while True: try: CONSOLE_LOGGER.info("\nMail Summary\n") for thread in threads: - CONSOLE_LOGGER.info(f"{thread.name} {thread.mailcount}") + CONSOLE_LOGGER.info(f"{thread.name}, mailcount: {thread.mailcount}, time: {time.time() - START_TIME:0.2f} seconds") thread.mailcount = 0 sleep(2.0) diff --git a/Exoboot_Postal_Service_OG.py b/Exoboot_Postal_Service_OG.py deleted file mode 100644 index ba9462a..0000000 --- a/Exoboot_Postal_Service_OG.py +++ /dev/null @@ -1,118 +0,0 @@ -from queue import Queue, Empty -from threading import Thread, current_thread - - -def Mail(sender, contents): - return {"sender": sender, "contents": contents} - - -class MailBox: - """ - TBD Description - - Receive mail from other addresses - """ - def __init__(self, address): - self.address = address - self.incoming_mail = Queue() - - def receive(self, mail): - """ - Put mail in queue - """ - self.incoming_mail.put_nowait(mail) - - def getmail_all(self): - """ - Return all mail in queue - """ - mail = [] - while True: - try: - mail.append(self.incoming_mail.get_nowait()) - except Empty: - break - - return mail - - -class PostOffice: - """ - TBD Description - - Send mail to other addresses(threads) - """ - def __init__(self): - self.addressbook = {} - - def setup_addressbook(self, *threads): - """ - Set up thread in addressbook with a corresponding mailbox - """ - for thread in threads: - mailbox = MailBox(thread.name) - thread.mailbox = mailbox - self.addressbook[thread.name] = mailbox - - def send(self, sender, recipient, contents): - """ - Send mail to recipient - Mail needs to have a sender and contents to be complete - """ - mail = Mail(sender, contents) - self.addressbook[recipient].receive(mail) - - -if __name__ == "__main__": - from time import sleep - from random import randint - - def f(postoffice): - """ - Target function sends random amount of mail and counts amount of mail received at 1Hz - """ - which = current_thread() - which.mailcount = 0 - - print("Started ", which.name) - while True: - # Send a random amount of mail to other addresses in the postoffice addressbook - for _ in range(randint(2, 5)): - try: - random_recipient = "thread" + str(randint(1, 3)) - postoffice.send(which.name, random_recipient, f"Hi from {which}") - except: - pass - - # Get mail and add to mailcount - mymail = which.mailbox.getmail_all() - which.mailcount += len(mymail) - - sleep(1.0) - - # Create PostOffice with empty addressbook - postoffice = PostOffice() - - # Create threads - thread1 = Thread(target=f, args=(postoffice,), daemon=True, name="thread1") - thread2 = Thread(target=f, args=(postoffice,), daemon=True, name="thread2") - thread3 = Thread(target=f, args=(postoffice,), daemon=True, name="thread3") - - # Populate addressbook with threads - postoffice.setup_addressbook(thread1, thread2, thread3) - - # Start threads - thread1.start() - thread2.start() - thread3.start() - - # Print out mailflow summary every 2 seconds - while True: - try: - print("\nMail Summary") - for thread in [thread1, thread2, thread3]: - print(thread.name, thread.mailcount) - thread.mailcount = 0 - sleep(2.0) - except KeyboardInterrupt: - break From 9fc6577f21001fc606857a7b99f1514668e654af Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Sun, 15 Jun 2025 22:45:30 +0100 Subject: [PATCH 04/27] renamed postoffice and mailbox class to be something more descriptive --- ...tal_Service.py => Exoboot_Messenger_Hub.py | 89 ++++++++++--------- tests/test_postal_service.py | 2 +- threading_demo.py | 2 +- 3 files changed, 47 insertions(+), 46 deletions(-) rename Exoboot_Postal_Service.py => Exoboot_Messenger_Hub.py (73%) diff --git a/Exoboot_Postal_Service.py b/Exoboot_Messenger_Hub.py similarity index 73% rename from Exoboot_Postal_Service.py rename to Exoboot_Messenger_Hub.py index b3750fb..a25e25d 100644 --- a/Exoboot_Postal_Service.py +++ b/Exoboot_Messenger_Hub.py @@ -30,35 +30,35 @@ class Mail: contents: Dict[str, Any] -class MailBox: +class Inbox: """ - MailBox receives and stores mail for a given address (thread). - The mailbox belongs to this thread and is used to recieve messages directed to it. + Inbox receives and stores mail for a given address (thread). + The inbox belongs to this thread and is used to recieve messages directed to it. Args: - address (str): The address of the mailbox, typically the thread name. + address (str): The address of the inbox, typically the thread name. """ def __init__(self, address: str) -> None: """ - Initialize a MailBox with a given address. + Initialize a inbox with a given address. """ self.address: str = address self._incoming_mail: Queue = Queue() def receive(self, mail: Mail) -> None: """ - Put mail in the mailbox queue. + Put mail in the inbox queue. Args: mail (Mail): The mail object to be received. """ - CONSOLE_LOGGER.info(f" Mail put into {self.address} mailbox: {mail}") + CONSOLE_LOGGER.info(f" Mail put into {self.address} inbox: {mail}") self._incoming_mail.put_nowait(mail) - def getmail_all(self) -> List[Mail]: + def get_all_mail(self) -> List[Mail]: """ - Retrieve and remove all mail from the mailbox queue. + Retrieve and remove all mail from the inbox queue. Returns: - List[Mail]: All mail currently in the mailbox. + List[Mail]: All mail currently in the inbox. """ mail: List[Mail] = [] @@ -74,34 +74,35 @@ def getmail_all(self) -> List[Mail]: return mail -class PostOffice: +class MessageRouter: """ - This PostOffice class is used to coordinate inter-thread communication + This class is used to coordinate inter-thread communication via queues. It's primary function is to send messages (mail) from one thread to another thread. - One PostOffice should be created and shared between all threads. - Each thread has its own mailbox, and the postoffice acts as a central hub - for sending and receiving messages between these mailboxes. + One object should be created and shared between all threads. + Each thread has its own inbox, and the MessageRouter acts as a central hub + for sending and receiving messages between these inboxes. """ def __init__(self) -> None: - self._addressbook: Dict[str, MailBox] = {} + self._addressbook: Dict[str, Inbox] = {} self._lock = Lock() # lock to ensure thread-safe access to the addressbook def setup_addressbook(self, *threads: Thread) -> None: """ - Set up threads in the addressbook with corresponding mailboxes. - To access threads' mailboxes, use the thread's `mailbox` attribute. + Set up threads in the addressbook with corresponding inboxes. + To access threads' inboxes, use the thread's `inbox` attribute. Args: *threads (Thread): Threads to register in the addressbook. """ + # to ensure that only one thread can access addressbook at a time with self._lock: for thread in threads: - if hasattr(thread, 'mailbox') and thread.mailbox is not None: - # if thread already has a mailbox, skip - CONSOLE_LOGGER.info(f"Mailbox already exists for thread: {thread.name}") + if hasattr(thread, 'inbox') and thread.inbox is not None: + # if thread already has a inbox, skip + CONSOLE_LOGGER.info(f"Inbox already exists for thread: {thread.name}") continue elif not isinstance(thread, Thread): @@ -117,17 +118,17 @@ def setup_addressbook(self, *threads: Thread) -> None: # if thread does not have a name, raise error raise ValueError("Thread must have a name.") - # instantiate a mailbox for each thread - mailbox = MailBox(thread.name) - thread.mailbox = mailbox + # instantiate a inbox for each thread + inbox = Inbox(thread.name) + thread.inbox = inbox - # create dictionary mapping thread name to it's mailbox - self._addressbook[thread.name] = mailbox - CONSOLE_LOGGER.info(f"Mailbox set up for thread: {thread.name}") + # create dictionary mapping thread name to it's inbox + self._addressbook[thread.name] = inbox + CONSOLE_LOGGER.info(f"Inbox set up for thread: {thread.name}") def send(self, sender: str, recipient: str, contents: dict) -> None: """ - Send mail to a recipient's mailbox. + Send mail to a recipient's inbox. Args: sender (str): Name of the sender. @@ -150,17 +151,17 @@ def send(self, sender: str, recipient: str, contents: dict) -> None: CONSOLE_LOGGER.info(f"{sender} sending to {recipient}") CONSOLE_LOGGER.info(f" Message contents: {mail}") - # send mail to recipient's mailbox + # send mail to recipient's inbox self._addressbook[recipient].receive(mail) -def f(postoffice, barrier): +def f(msg_router, barrier): """ This target function sends random amount of mail and counts amount of mail received at 1Hz. Waits at the barrier until all threads are ready before sending mail. """ which = current_thread() - which.mailcount = 0 + which.msg_count = 0 CONSOLE_LOGGER.info("Started " + which.name) # Wait for all threads to be ready before sending mail @@ -189,18 +190,18 @@ def f(postoffice, barrier): # Random delay before sending sleep(random.uniform(0, 0.02)) - # send the mail to the thread's mailbox - postoffice.send(which.name, random_recipient, unique_msg) + # send the mail to the thread's inbox + msg_router.send(which.name, random_recipient, unique_msg) except Exception as e: CONSOLE_LOGGER.error(f"{which.name} failed to send to {random_recipient}: {e}") pass - # After sending mail, it retrieves all mail from the current thread's mailbox - mymail = which.mailbox.getmail_all() + # After sending mail, it retrieves all mail from the current thread's inbox + mymail = which.inbox.get_all_mail() for mail in mymail: - CONSOLE_LOGGER.info(f"unpacking {which.name} mailbox") + CONSOLE_LOGGER.info(f"unpacking {which.name} inbox") CONSOLE_LOGGER.info(f" {mail.sender} had sent this: {mail.contents}") - which.mailcount += len(mymail) + which.msg_count += len(mymail) CONSOLE_LOGGER.info(f"\n") sleep(1.0) @@ -212,7 +213,7 @@ def f(postoffice, barrier): if __name__ == "__main__": # Create PostOffice with empty addressbook - postoffice = PostOffice() + msg_router = MessageRouter() # Set the number of threads num_threads = 3 @@ -221,17 +222,17 @@ def f(postoffice, barrier): thread_names = [f"thread{i+1}" for i in range(num_threads)] threads = [] for name in thread_names: - threads.append(Thread(target=f, args=(postoffice, None), daemon=True, name=name)) + threads.append(Thread(target=f, args=(msg_router, None), daemon=True, name=name)) # Create a barrier for all threads barrier = Barrier(num_threads) # Assign the barrier to each thread's args for thread in threads: - thread._args = (postoffice, barrier) + thread._args = (msg_router, barrier) # Populate addressbook with threads - postoffice.setup_addressbook(*threads) + msg_router.setup_addressbook(*threads) # Start all threads for thread in threads: @@ -240,10 +241,10 @@ def f(postoffice, barrier): # Print out mail flow summary every 0.5 Hz (slower than threads) while True: try: - CONSOLE_LOGGER.info("\nMail Summary\n") + CONSOLE_LOGGER.info("\nMessage Summary\n") for thread in threads: - CONSOLE_LOGGER.info(f"{thread.name}, mailcount: {thread.mailcount}, time: {time.time() - START_TIME:0.2f} seconds") - thread.mailcount = 0 + CONSOLE_LOGGER.info(f"{thread.name}, msg count: {thread.msg_count}, time: {time.time() - START_TIME:0.2f} seconds") + thread.msg_count = 0 sleep(2.0) except KeyboardInterrupt: diff --git a/tests/test_postal_service.py b/tests/test_postal_service.py index c142284..c0a5b3b 100644 --- a/tests/test_postal_service.py +++ b/tests/test_postal_service.py @@ -1,5 +1,5 @@ import pytest -from Exoboot_Postal_Service import Mail, MailBox, PostOffice +from Exoboot_Messenger_Hub import Mail, MailBox, PostOffice class DummyThread: def __init__(self, name: str): diff --git a/threading_demo.py b/threading_demo.py index cc04cd8..909abed 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -17,7 +17,7 @@ log_format = "%(levelname)s: %(message)s" ) -from Exoboot_Postal_Service import PostOffice +from Exoboot_Messenger_Hub import PostOffice class BaseWorkerThread(threading.Thread, ABC): """ From 68c232365f5940f2a597aeb092964ca5f8de2d2b Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Sun, 15 Jun 2025 23:01:39 +0100 Subject: [PATCH 05/27] cleaned up exoboots.py so that errors from merging are resolved. also began a draft of threading_demo_updated.py to start a ThreadingManager class to initialize, start, stop and coordinate threads --- exoboots.py | 9 - threading_demo.py | 2 + threading_demo_updated.py | 582 ++++++++++++++++++++++++++++++++++++++ 3 files changed, 584 insertions(+), 9 deletions(-) create mode 100644 threading_demo_updated.py diff --git a/exoboots.py b/exoboots.py index 6b92835..d894678 100644 --- a/exoboots.py +++ b/exoboots.py @@ -174,12 +174,6 @@ def command_currents(self) -> None: for actuator in self.actuators.values(): current_setpoint = current_setpoints.get(actuator.side) - arguments: - current_setpoints: dict of currents for each active actuator. - key is the side of the actuator (left or right). - """ - - # TODO: ensure current_setpoints values are integers, no greater than max current limit, and are not None for actuator in self.actuators.values(): @@ -277,8 +271,6 @@ def initialize_rt_plots(self) -> list: return plot_config - def update_rt_plots(self, bertec_swing_flag, imu_activations)->list: - def update_rt_plots(self, bertec_swing_flag, imu_activations) -> list: """ Updates the real-time plots with current values for: @@ -308,7 +300,6 @@ def update_rt_plots(self, bertec_swing_flag, imu_activations) -> list: return data_to_plt - def track_variables_for_logging(self, logger: Logger) -> None: """ Track variables for each active actuator for logging to a single file diff --git a/threading_demo.py b/threading_demo.py index 909abed..6dc7722 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -492,6 +492,8 @@ def gui_thread(self) -> Optional[ActuatorThread]: """Get the right actuator thread""" return self._threads.get("gui") + + # Example main loop from opensourceleg.utilities import SoftRealtimeLoop from src.utils.actuator_utils import create_actuators diff --git a/threading_demo_updated.py b/threading_demo_updated.py new file mode 100644 index 0000000..850f795 --- /dev/null +++ b/threading_demo_updated.py @@ -0,0 +1,582 @@ +# Abstract Base class for threading +import threading +import queue + +from typing import Type, Dict, Any, Optional +from abc import ABC, abstractmethod +from src.utils.flexible_sleeper import FlexibleSleeper +from non_singleton_logger import NonSingletonLogger +from dephyEB51 import DephyEB51Actuator + +# logging setup +from src.utils.filing_utils import get_logging_info +from opensourceleg.logging import Logger, LogLevel +CONSOLE_LOGGER = Logger(enable_csv_logging=False, + log_path=get_logging_info(user_input_flag=False)[0], + stream_level = LogLevel.INFO, + log_format = "%(levelname)s: %(message)s" + ) + +from Exoboot_Messenger_Hub import PostOffice + +class BaseWorkerThread(threading.Thread, ABC): + """ + Abstract base class for worker threads in the exoskeleton system. + This class provides a template for creating threads that can be paused and quit. + It includes methods for thread lifecycle management and a run method that + handles the thread's main loop. + """ + + def __init__(self, + quit_event:Type[threading.Event], + pause_event:Type[threading.Event], + log_event:Type[threading.Event], + name:str, + frequency:int=100)->None: + + super().__init__(name=name) + self.quit_event = quit_event # event to signal quitting + self.pause_event = pause_event # event to signal pausing + self.log_event = log_event # event to signal logging + + self.daemon = True + self.frequency = int(frequency) + self.rt_loop = FlexibleSleeper(dt=1/frequency) # create a soft real-time loop with the specified frequency + + # set-up a logger for each thread/instance + # logger_name = f"{name}_logger" + # log_path = "./src/logs/" + # self.data_logger = NonSingletonLogger( + # log_path=log_path, + # file_name=logger_name, + # file_level=logging.DEBUG, + # stream_level=logging.INFO + # ) + + def run(self)->None: + """ + Main loop structure for each instantiated thread. + """ + LOGGER.debug(f"[{self.name}] Thread running.") + + while self.quit_event.is_set(): # while not quitting + self.pre_iterate() # run a pre-iterate method + + if not self.pause_event.is_set(): + self.on_pause() # if paused, run once + else: + try: + self.iterate() # call the iterate method to perform the thread's task + except Exception as e: + LOGGER.debug(f"Exception in thread {self.name}: {e}") + + self.post_iterate() # run a post-iterate method + self.rt_loop.pause() + + LOGGER.debug(f"[{self.name}] Thread exiting.") + + @abstractmethod + def pre_iterate(self): + """ + Override this abstract method in subclasses. + This method is called BEFORE the iterate method. + It can be used to perform any repetitive setup or initialization tasks. + """ + LOGGER.debug(f"[{self.name}] pre-iterate() called.") + pass + + @abstractmethod + def iterate(self): + """ + Override this abstract method in subclasses to do the thread's work. + This method contains the main logic for the thread's operation. + """ + LOGGER.debug(f"[{self.name}] iterate() called.") + pass + + @abstractmethod + def post_iterate(self): + """ + Override this abstract method in subclasses. + This method is called AFTER the iterate method. + It can be used to perform any repetitive cleanup or logging, or finalization tasks. + """ + LOGGER.debug(f"[{self.name}] post-iterate() called.") + pass + + @abstractmethod + def on_pause(self): + """ + Override this abstract method in subclasses. + Runs once before pausing threads. + """ + LOGGER.debug(f"[{self.name}] on_pre_pause() called.") + pass + + +class ActuatorThread(BaseWorkerThread): + """ + Threading class for EACH actuator. + This class handles the actuator's state updates and manages the actuator's control loop. + """ + def __init__(self, + name: str, + actuator, + post_office, + quit_event: Type[threading.Event], + pause_event: Type[threading.Event], + log_event: Type[threading.Event], + frequency: int = 100) -> None: + + super().__init__(quit_event, pause_event, log_event, name, frequency=frequency) + + self.post_office = post_office + self.mailbox = None + + self.actuator = actuator + + def pre_iterate(self)->None: + pass + + def iterate(self)->None: + """ + Main loop for the actuator thread. + This method is called repeatedly in the thread's run loop. + It handles the actuator's state updates. + """ + self.actuator.update() + self.data_logger.debug(f"motor position: {self.actuator.motor_position:.2f}") + + # create a queue that can send the actuator state to the DephyExoboots Robot class + self.enqueue_actuator_states() + + def enqueue_actuator_states(self)-> None: + """ + Stores a dict of actuators state in the queue for the main thread. + This method is called to send the actuator's state to the main thread. + """ + self.actuator_queue.put({ + "motor_current": self.actuator.motor_current, + "temperature": self.actuator.case_temperature, + }) + + def post_iterate(self)->None: + # TODO add rest of stuff + if self.log_event.is_set(): + LOGGER.debug(f"[{self.name}] log_event True") + + def on_pause(self)->None: + pass + + +class GaitStateEstimatorThread(BaseWorkerThread): + """ + Threading class for the Gait State Estimator. + This class handles gait state estimation for BOTH exoskeletons/sides together. + """ + def __init__(self, + post_office, + quit_event:Type[threading.Event], + pause_event:Type[threading.Event], + log_event: Type[threading.Event], + name:Optional[str] = None, + frequency:int=100)->None: + + super().__init__(quit_event, pause_event, log_event, name=name, frequency=frequency) + + self.post_office = post_office + self.mailbox = None + + # instantiate the walking simulator + self.walker = WalkingSimulator(stride_period=1.20) + + + # TODO: initialize GSE Bertec here + + def pre_iterate(self)->None: + pass + + def pre_iterate(self)->None: + pass + + def iterate(self): + """ + Main loop for the actuator thread. + This method is called repeatedly in the thread's run loop. + It handles the actuator's state updates. + """ + + self.time_in_stride = self.walker.update_time_in_stride() + self.ank_angle = self.walker.update_ank_angle() + + # TODO: do update_bertec_estimator.update() here + + # TODO: DON"T LOG HERE (make ligtweight) + self.data_logger.debug(f"Time in stride: {self.time_in_stride:.3f}s") + self.data_logger.debug(f"Ankle angle: {self.ank_angle:.2f} deg") + + # create a queue that can send the gait state to the DephyExoboots Robot class + self.enqueue_gait_states() + + def post_iterate(self)->None: + pass + + def enqueue_gait_states(self)-> None: + """ + Stores a dict of gait states in the queue for the main thread. + This method is called to send the gait state to the main thread. + """ + self.gse_queue.put({ + "time_in_stride": self.walker.time_in_stride, + "ank_angle": self.walker.ank_angle, + }) + + def post_iterate(self)->None: + pass + + def on_pause(self)->None: + pass + + +import sys +import select +class GUICommunication(BaseWorkerThread): + """ + Threading class to simulate GUI communication via gRPC. + This class handles GUI communication for BOTH exoskeletons/sides together. + + It constantly monitors for new user input specifying a desired torque setpoint. + It then sends these setpoints to the actuators to handle. + """ + def __init__(self, + post_office, + quit_event:Type[threading.Event], + pause_event:Type[threading.Event], + log_event: Type[threading.Event], + name:Optional[str] = None, + frequency:int=100)->None: + + + super().__init__(quit_event, pause_event, log_event, name=name, frequency=frequency) + + self.post_office = post_office + self.mailbox = None + self.torque_setpoint = 20.0 + + def pre_iterate(self)->None: + # read mail regardless of paused state + mail_list = self.mailbox.getmail_all() # return list of mail + CONSOLE_LOGGER.debug(f"mail received in GUICommunication thread {len(mail_list)}") + + # unpackage the mail + for mail in mail_list: + self.decode_message(mail) + print(f"torque setpoint is:{self.torque_setpoint}") + + def decode_message(self, mail:list) -> Optional[Any]: + """ + Decode a message from the mailbox. + """ + try: + # example contents {"peak_torque": 30.1, "stride_period": 1.3} + for key, value in mail["contents"].items(): + #TODO value validation (not None or something) + setattr(self, key, value) + + except Exception as err: + LOGGER.debug("Error decoding message:", err) + return None + + def iterate(self): + """ + Non-blocking: Only read user input if available. + Allow user to input a new desired torque setpoint. + If the user doesn't input a new value, the current setpoint is used. + """ + + # Put the torque setpoint into the queue for the main thread + pass + + def post_iterate(self)->None: + pass + + def on_pause(self)->None: + pass + + +# Mini Exoboots Robot Class for testing threading +from opensourceleg.robots.base import RobotBase +from opensourceleg.sensors.base import SensorBase +from dephyEB51 import DephyEB51Actuator +from opensourceleg.logging import LOGGER +from opensourceleg.actuators.base import CONTROL_MODES +from opensourceleg.actuators.dephy import DEFAULT_CURRENT_GAINS + +class DephyExoboots(RobotBase[DephyEB51Actuator, SensorBase]): + def __init__(self, tag, actuators, sensors, post_office) -> None: + """ + Exoboot Robot class that extends RobotBase. + This class manages thread creation, data queue creation & management, as well as basic actuator control. + It is designed to handle multiple actuators and their respective threads, as well as a + Gait State Estimator (GSE) thread and a GUI communication thread. + """ + super().__init__(tag=tag, actuators=actuators, sensors=sensors) + self._threads = {} + self._quit_event = threading.Event() # Event to signal threads to quit. + self._pause_event = threading.Event() # Event to signal threads to pause. + self._log_event = threading.Event() # Event to signal threads to log + self.post_office = post_office # PostOffice class instance + + # Thread event inits + self._quit_event.set() # exo is running + self._pause_event.clear() # exo starts paused + self._log_event.clear() # exo starts not logging + + def start(self) -> None: + """Start actuator threads""" + + for actuator in self.actuators.values(): + # start the actuator + actuator.start() + + # set-up control modes and gains here before starting the threads + self.set_actuator_mode_and_gains(actuator) + + # creating and starting threads for each actuator + self.initialize_actuator_thread(actuator) + CONSOLE_LOGGER.debug(f"Started actuator thread for {actuator.side}") + + # creating 1 thread for the Gait State Estimator + self.initialize_GSE_thread() + + # creating 1 thread for GUI communication + self.initialize_GUI_thread() + + def stop(self) -> None: + """Signal threads to quit and join them""" + # setting the quit event so all threads recieve the kill signal + self._quit_event.set() + super().stop() + + def create_interthread_queue(self, name:str, max_size:int=0) -> queue.Queue: + """ + Create a FIFO queue for inter-thread communication. + Queues stored in dictionary with the name as the key. + + Args: + name (str): A unique name for the queue, typically the side of the actuator or thread. + Returns: + queue.Queue: A FIFO queue for inter-thread communication. + """ + + # create a FIFO queue with max size for inter-thread communication & store to dictionary + self.queues[name] = queue.Queue(maxsize=max_size) + + return self.queues[name] + + def initialize_actuator_thread(self, actuator: DephyEB51Actuator) -> None: + """ + Create and start a thread for the specified actuator. + This method is called to set up the actuator communication thread. + """ + + actuator_thread = ActuatorThread(actuator=actuator, + quit_event=self._quit_event, + pause_event=self._pause_event, + log_event=self._log_event, + name=f"{actuator.side}", + frequency=1, + post_office=self.post_office, + ) + + LOGGER.debug(f"Started actuator thread for {actuator.side}") + self._threads[actuator.side] = actuator_thread + + def initialize_GSE_thread(self) -> None: + """ + Create and start the Gait State Estimator thread. + This method is called to set up the GSE communication thread. + """ + # create a FIFO queue with max size for inter-thread communication + name = "gse" + gse_thread = GaitStateEstimatorThread(quit_event=self._quit_event, + pause_event=self._pause_event, + log_event=self._log_event, + name=name, + frequency=1, + post_office=self.post_office) + + LOGGER.debug(f"Started gse thread") + self._threads[name] = gse_thread + + def initialize_GUI_thread(self) -> None: + """ + Create and start the GUI thread for user input. + This method is called to set up the GUI communication thread. + """ + + # create a FIFO queue with max size for inter-thread communication + name = "gui" + gui_thread = GUICommunication(quit_event=self._quit_event, + pause_event=self._pause_event, + log_event=self._log_event, + name=name, + frequency=1, + post_office=self.post_office,) + LOGGER.debug(f"Started gui thread") + self._threads[name] = gui_thread + + def set_actuator_mode_and_gains(self, actuator)-> None: + """ + Call the setup_controller method for all actuators. + This method selects current control mode and sets PID gains for each actuator. + """ + actuator.set_control_mode(CONTROL_MODES.CURRENT) + LOGGER.info("finished setting control mode") + + actuator.set_current_gains( + kp=DEFAULT_CURRENT_GAINS.kp, + ki=DEFAULT_CURRENT_GAINS.ki, + kd=DEFAULT_CURRENT_GAINS.kd, + ff=DEFAULT_CURRENT_GAINS.ff, + ) + LOGGER.info("finished setting gains") + + def update(self)->None: + """Required by RobotBase, but passing since ActuatorThread handles iterative exo state updates""" + pass + + def return_active_threads(self)->list: + """ + Return list of active thread addresses. + """ + return self._threads.values() + + @property + def left_exo_queue(self) -> Dict[str, queue.Queue]: + """Get the queue for the left actuator""" + return self.queues.get("left") + + @property + def right_exo_queue(self) -> Dict[str, queue.Queue]: + """Get the queue for the right actuator""" + return self.queues.get("right") + + @property + def gse_queue(self) -> Dict[str, queue.Queue]: + """Get the queue for the gait state estimator""" + return self.queues.get("gse") + + @property + def gui_queue(self) -> Dict[str, queue.Queue]: + """Get the queue for the GUI communication""" + return self.queues.get("gui") + + @property + def left_thread(self) -> Optional[ActuatorThread]: + """Get the left actuator thread""" + return self._threads.get("left") + + @property + def right_thread(self) -> Optional[ActuatorThread]: + """Get the right actuator thread""" + return self._threads.get("right") + + @property + def gse_thread(self) -> Optional[ActuatorThread]: + """Get the right actuator thread""" + return self._threads.get("gse") + + @property + def gui_thread(self) -> Optional[ActuatorThread]: + """Get the right actuator thread""" + return self._threads.get("gui") + + + +class ThreadManager: + """ + This class manages thread creation, communication and termination for the exoskeleton system. + """ + + def __init__(self, msg_router) -> None: + + self._threads = {} + self._quit_event = threading.Event() # Event to signal threads to quit. + self._pause_event = threading.Event() # Event to signal threads to pause. + self._log_event = threading.Event() # Event to signal threads to log + self.msg_router = msg_router # MessageRouter class instance + + # initialize thread events + self._quit_event.set() # exo is running + self._pause_event.clear() # exo starts paused + self._log_event.clear() # exo starts not logging + + def __enter__(self): + """ + Context manager enter method. + This method is called when the ThreadManager is used in a with statement. + It starts all threads. + """ + self.start() + return self + + def __exit__(self): + """ + Context manager exit method. + This method is called when the ThreadManager is used in a with statement. + It stops all threads and cleans up. + """ + self.stop() + + + +# Example main loop +from opensourceleg.utilities import SoftRealtimeLoop +from src.utils.actuator_utils import create_actuators +from src.settings.constants import EXO_SETUP_CONST +from src.utils.walking_simulator import WalkingSimulator + +if __name__ == '__main__': + + post_office = PostOffice() + + actuators = create_actuators(1, EXO_SETUP_CONST.BAUD_RATE, EXO_SETUP_CONST.FLEXSEA_FREQ, EXO_SETUP_CONST.LOG_LEVEL) + exoboots = DephyExoboots(tag="exoboots", + actuators=actuators, + sensors={}, + post_office=post_office) + clock = SoftRealtimeLoop(dt = 1 / 1) # Hz + + with exoboots: + # set-up addressbook for the PostOffice + post_office.setup_addressbook(*exoboots.return_active_threads()) + + # TODO add start_threads method to DephyExoboots class + # exoboots.left_thread.start() + exoboots.right_thread.start() + exoboots.gse_thread.start() + exoboots.gui_thread.start() + CONSOLE_LOGGER.debug("All threads started.") + + for t in clock: + try: + + # send mail to the GUI thread + post_office.send(sender="main", recipient="gui", contents={"torque_setpoint": 30.0}) + print("Main sent") + + # TODO: pass reported values into AssistanceGenerator class -> method is part of Exoboots Robot class + # TODO: determine appropriate torque setpoint given current gait state + + # TODO: send torque setpoints to each corresponding actuator + # TODO: determine appropriate current setpoint that matches the torque setpoint -> handled by DephyEB51Actuator class (within each actuator thread) + # TODO: command appropriate current setpoint using DephyExoboots class + + except KeyboardInterrupt: + print("KeyboardInterrupt received.") + break + + except Exception as err: + print("Unexpected error in executing main controller:", err) + break From 88ed8181e41c54e9cfae9f8900d54163806f8b53 Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Sun, 15 Jun 2025 23:44:10 +0100 Subject: [PATCH 06/27] fixed more merge mistakes in exoboots.py. also made a lil bit more progress on ThreadManager class in threading_demo_updated.py --- exoboots.py | 58 +++-- threading_demo_updated.py | 446 +++++++++++++++++++++++++++++--------- 2 files changed, 373 insertions(+), 131 deletions(-) diff --git a/exoboots.py b/exoboots.py index d894678..cd9b9b7 100644 --- a/exoboots.py +++ b/exoboots.py @@ -15,8 +15,6 @@ stream_level = LogLevel.INFO, log_format = "%(levelname)s: %(message)s" ) -from src.utils import CONSOLE_LOGGER -from opensourceleg.logging import Logger from src.utils.actuator_utils import create_actuators from src.settings.constants import ( @@ -27,14 +25,6 @@ class DephyExoboots(RobotBase[DephyEB51Actuator, SensorBase]): - """ - Bilateral Dephy EB51 Exoskeleton class derived from RobotBase. - - This class creates a DephyExoboots Robot, using the structure - provided by the RobotBase class. A robot is composed of a collection - of actuators and sensors. - - """ def start(self) -> None: """ @@ -49,6 +39,22 @@ def stop(self) -> None: super().stop() + def set_actuator_mode_and_gains(self, actuator)-> None: + """ + Call the setup_controller method for all actuators. + This method selects current control mode and sets PID gains for each actuator. + """ + actuator.set_control_mode(CONTROL_MODES.CURRENT) + LOGGER.info("finished setting control mode") + + actuator.set_current_gains( + kp=DEFAULT_CURRENT_GAINS.kp, + ki=DEFAULT_CURRENT_GAINS.ki, + kd=DEFAULT_CURRENT_GAINS.kd, + ff=DEFAULT_CURRENT_GAINS.ff, + ) + LOGGER.info("finished setting gains") + def update(self) -> None: """ Update the exoskeleton. @@ -148,48 +154,32 @@ def convert_torque_to_current_setpoints(self, torque_setpoint: float) -> dict: returns: - currents: dict of currents for each active actuator. + current_setpoints: dict of currents for each active actuator. key is the side of the actuator (left or right). """ for actuator in self.actuators.values(): - currents[actuator.side] = actuator.torque_to_current(torque_setpoint) + self.current_setpoints[actuator.side] = actuator.torque_to_current(torque_setpoint) CONSOLE_LOGGER.info(f"finished finding current setpoint for {actuator.side}") - return currents - - def command_currents(self, current_setpoints:dict) -> None: - self.current_setpoints[actuator.side] = actuator.torque_to_current(torque_setpoint) - # CONSOLE_LOGGER.info(f"finished finding current setpoint for {actuator.side}") + return self.current_setpoints def command_currents(self) -> None: """ - Commands current setpoints to each actuator. + Commands current setpoints to each actuator based on the current_setpoints dictionary. The setpoints can be unique. - - arguments: - current_setpoints: dict of currents for each active actuator. - key is the side of the actuator (left or right). """ - - for actuator in self.actuators.values(): - current_setpoint = current_setpoints.get(actuator.side) - # TODO: ensure current_setpoints values are integers, no greater than max current limit, and are not None for actuator in self.actuators.values(): - current_setpoint = current_setpoints.get(actuator.side) current_setpoint = self.current_setpoints.get(actuator.side) if current_setpoint is not None: actuator.set_motor_current(current_setpoint) - # CONSOLE_LOGGER.info(f"Finished setting current setpoint for {actuator.side}") + CONSOLE_LOGGER.info(f"Finished setting current setpoint for {actuator.side}") else: CONSOLE_LOGGER.warning(f"Unknown side '{actuator.side}' and unable to command current. Skipping.") - def initialize_rt_plots(self)->list: - CONSOLE_LOGGER.warning(f"Unknown side '{actuator.side}' and unable to command current. Skipping.") - def initialize_rt_plots(self) -> list: """ Initialize real-time plots for the exoskeleton robot. @@ -337,6 +327,12 @@ def right(self) -> DephyEB51Actuator: CONSOLE_LOGGER.error("Ankle actuator not found. Please check for `right` key in the actuators dictionary.") exit(1) + try: + return self.actuators["right"] + except KeyError: + CONSOLE_LOGGER.error("Ankle actuator not found. Please check for `right` key in the actuators dictionary.") + exit(1) + # DEMO: if __name__ == "__main__": # define dictionary of actuators & sensors diff --git a/threading_demo_updated.py b/threading_demo_updated.py index 850f795..3e866e6 100644 --- a/threading_demo_updated.py +++ b/threading_demo_updated.py @@ -17,7 +17,7 @@ log_format = "%(levelname)s: %(message)s" ) -from Exoboot_Messenger_Hub import PostOffice +from Exoboot_Messenger_Hub import MessageRouter class BaseWorkerThread(threading.Thread, ABC): """ @@ -311,38 +311,350 @@ def on_pause(self)->None: from opensourceleg.logging import LOGGER from opensourceleg.actuators.base import CONTROL_MODES from opensourceleg.actuators.dephy import DEFAULT_CURRENT_GAINS +from typing import Union, Dict +import time class DephyExoboots(RobotBase[DephyEB51Actuator, SensorBase]): - def __init__(self, tag, actuators, sensors, post_office) -> None: + + def start(self) -> None: """ - Exoboot Robot class that extends RobotBase. - This class manages thread creation, data queue creation & management, as well as basic actuator control. - It is designed to handle multiple actuators and their respective threads, as well as a - Gait State Estimator (GSE) thread and a GUI communication thread. + Start the Exoskeleton. """ - super().__init__(tag=tag, actuators=actuators, sensors=sensors) - self._threads = {} + super().start() + + def stop(self) -> None: + """ + Stop the Exoskeleton. + """ + + super().stop() + + def set_actuator_mode_and_gains(self, actuator)-> None: + """ + Call the setup_controller method for all actuators. + This method selects current control mode and sets PID gains for each actuator. + """ + actuator.set_control_mode(CONTROL_MODES.CURRENT) + LOGGER.info("finished setting control mode") + + actuator.set_current_gains( + kp=DEFAULT_CURRENT_GAINS.kp, + ki=DEFAULT_CURRENT_GAINS.ki, + kd=DEFAULT_CURRENT_GAINS.kd, + ff=DEFAULT_CURRENT_GAINS.ff, + ) + LOGGER.info("finished setting gains") + + def update(self) -> None: + """ + Update the exoskeleton. + """ + # print(f"Updating exoskeleton robot: {self.tag}") + super().update() + + def setup_control_modes(self) -> None: + """ + Call the setup_controller method for all actuators. + This method selects current control mode and sets PID gains for each actuator. + """ + + for actuator in self.actuators.values(): + actuator.set_control_mode(CONTROL_MODES.CURRENT) + CONSOLE_LOGGER.info("finished setting control mode") + + actuator.set_current_gains( + kp=DEFAULT_PID_GAINS.KP, + ki=DEFAULT_PID_GAINS.KI, + kd=DEFAULT_PID_GAINS.KD, + ff=DEFAULT_PID_GAINS.FF, + ) + CONSOLE_LOGGER.info("finished setting gains") + + def spool_belts(self): + """ + Spool the belts of both actuators. + This method is called to prepare the actuators for operation. + """ + for actuator in self.actuators.values(): + actuator.spool_belt() + CONSOLE_LOGGER.info(f"finished spooling belt of {actuator.side}") + + def set_to_transparent_mode(self): + """ + Set the exo currents to 0mA. + """ + self.update_current_setpoints(current_inputs=0, asymmetric=False) + self.command_currents() + + def detect_active_actuators(self) -> Union[start, list[str]]: + """ + Detect active actuators. + Returns a string if only one actuator is active, otherwise a list of strings. + """ + active_sides = list(self.actuators.keys()) + + if len(active_sides) == 1: + return active_sides[0] + + return active_sides + + def create_current_setpts_dict(self) -> None: + """ + create dictionary of current setpoints (in mA) corresponding to actuator side + """ + self.current_setpoints = {} + for actuator in self.actuators.values(): + self.current_setpoints[actuator.side] = 0.0 + + # TODO: generate test to determine if current_setpoints dict has the same keys as the actuators dict + + def update_current_setpoints(self, current_inputs: Union[int, Dict[str, int]], asymmetric:bool=False) -> None: + """ + Directly assign currents to the 'current_setpoints' dictionary for current control. + + If symmetric, the same current value is applied to both sides (with motor sign). + If asymmetric, the user must pass a dictionary specifying currents for each side. + + Args: + - current: int or dict. If symmetric=False, this should be a dict with 'left' and 'right' keys. + - asymmetric: bool. If True, use side-specific currents from the dictionary. + """ + # TODO: ensure that current_inputs matches the number of active sides + # TODO: if more than the number of active sides provided, trim to active one only + # TODO: handle missing sides + + # TODO: clip current setpoints to below max limit + + if asymmetric: + for side, current in current_inputs.items(): # assign different currents for each actuator + actuator = getattr(self, side) + self.current_setpoints[side] = int(current) * actuator.motor_sign + else: + for side in self.actuators.keys(): # assign the same current for both actuators + actuator = getattr(self, side) + self.current_setpoints[side] = int(current_inputs) * actuator.motor_sign + + def convert_torque_to_current_setpoints(self, torque_setpoint: float) -> dict: + """ + Find the appropriate current setpoint for the actuators. + This method is called to determine the current setpoint based on the torque setpoint. + + arguments: + torque_setpoint: float, the desired torque setpoint in Nm. + + + returns: + current_setpoints: dict of currents for each active actuator. + key is the side of the actuator (left or right). + """ + for actuator in self.actuators.values(): + self.current_setpoints[actuator.side] = actuator.torque_to_current(torque_setpoint) + CONSOLE_LOGGER.info(f"finished finding current setpoint for {actuator.side}") + + return self.current_setpoints + + def command_currents(self) -> None: + """ + Commands current setpoints to each actuator based on the current_setpoints dictionary. + The setpoints can be unique. + """ + # TODO: ensure current_setpoints values are integers, no greater than max current limit, and are not None + + for actuator in self.actuators.values(): + + current_setpoint = self.current_setpoints.get(actuator.side) + + if current_setpoint is not None: + actuator.set_motor_current(current_setpoint) + CONSOLE_LOGGER.info(f"Finished setting current setpoint for {actuator.side}") + else: + CONSOLE_LOGGER.warning(f"Unknown side '{actuator.side}' and unable to command current. Skipping.") + + def initialize_rt_plots(self) -> list: + """ + Initialize real-time plots for the exoskeleton robot. + Naming and plotting is flexible to each active actuator. + + The following time series are plotted: + - Current (A) + - Temperature (°C) + - Ankle Angle (°) + - Transmission Ratio + - Ankle Torque Setpoint (Nm) + + """ + # converting actuator dictionary keys to a list + active_sides_list = list(self.actuators.keys()) + + print("Active actuators:", active_sides_list) + + # pre-slice colors based on the number of active actuators + colors = ['r', 'b'][:len(active_sides_list)] + if len(active_sides_list) > len(colors): + raise ValueError("Not enough unique colors for the number of active actuators.") + + # repeat line styles and widths for each active actuator + line_styles = ['-' for _ in active_sides_list] + line_widths = [2 for _ in active_sides_list] + + current_plt_config = {'names' : active_sides_list, + 'colors' : colors, + 'line_style': line_styles, + 'title' : "Exo Current (A) vs. Sample", + 'ylabel': "Current (A)", + 'xlabel': "timestep", + 'line_width': line_widths, + 'yrange': [0,30] + } + + temp_plt_config = {'names' : active_sides_list, + 'colors' : colors, + 'line_style': line_styles, + 'title' : "Case Temperature (°C) vs. Sample", + 'ylabel': "Temperature (°C)", + 'xlabel': "timestep", + 'line_width': line_widths, + 'yrange': [20,60] + } + + in_swing_plt_config = {'names' : active_sides_list, + 'colors' : colors, + 'line_style': line_styles, + 'title' : "Bertec in-swing vs. Sample", + 'ylabel': "Bool", + 'xlabel': "timestep", + 'line_width': line_widths, + 'yrange': [0,150] + } + + TR_plt_config = {'names' : active_sides_list, + 'colors' : colors, + 'line_style': line_styles, + 'title' : "TR (°) vs. Sample", + 'ylabel': "N", + 'xlabel': "timestep", + 'line_width': line_widths, + 'yrange': [0,20] + } + + imu_plt_config = {'names' : active_sides_list, + 'colors' : colors, + 'line_style': line_styles, + 'title' : "Activations vs. Sample", + 'ylabel': "Bool", + 'xlabel': "timestep", + 'line_width': line_widths, + 'yrange': [0,50] + } + + plot_config = [current_plt_config, temp_plt_config, in_swing_plt_config, TR_plt_config, imu_plt_config] + + return plot_config + + def update_rt_plots(self, bertec_swing_flag, imu_activations) -> list: + """ + Updates the real-time plots with current values for: + - Current (A) + - Temperature (°C) + - Bertec In swing + - Transmission Ratio + - IMU estimator activations + + The data is collected from the exoboots object and returned as a list of arrays. + This is done for each active actuator only. + + Returns: + plot_data_array: A list of data arrays (for active actuators) for each plot. + """ + + data_to_plt = [] + + for actuator in self.actuators.values(): + data_to_plt.extend([ + abs(actuator.motor_current), # Motor current + actuator.case_temperature, # Case temperature + bertec_swing_flag, + actuator.gear_ratio, # Gear ratio + imu_activations + ]) + + return data_to_plt + + def track_variables_for_logging(self, logger: Logger) -> None: + """ + Track variables for each active actuator for logging to a single file + """ + + for actuator in self.actuators.values(): + dummy_grpc_value = 5.0 + dummy_ankle_torque_setpt = 20 + logger.track_variable(lambda: time.time(), "pitime") + logger.track_variable(lambda: dummy_grpc_value, "dollar_value") + logger.track_variable(lambda: dummy_ankle_torque_setpt, "torque_setpt_Nm") + + logger.track_variable(lambda: actuator.accelx, f"{actuator._tag}_accelx_mps2") + logger.track_variable(lambda: actuator.motor_current, f"{actuator._tag}_current_mA") + logger.track_variable(lambda: actuator.motor_position, f"{actuator._tag}_position_rad") + logger.track_variable(lambda: actuator.motor_encoder_counts, f"{actuator._tag}_encoder_counts") + logger.track_variable(lambda: actuator.case_temperature, f"{actuator._tag}_case_temp_C") + + tracked_vars = logger.get_tracked_variables() + print("Tracked variables:", tracked_vars) + + @property + def left(self) -> DephyEB51Actuator: + try: + return self.actuators["left"] + except KeyError: + CONSOLE_LOGGER.error("Ankle actuator not found. Please check for `left` key in the actuators dictionary.") + exit(1) + + @property + def right(self) -> DephyEB51Actuator: + try: + return self.actuators["right"] + except KeyError: + CONSOLE_LOGGER.error("Ankle actuator not found. Please check for `right` key in the actuators dictionary.") + exit(1) + + +class ThreadManager: + """ + This class manages thread creation, communication and termination for the exoskeleton system. + """ + + def __init__(self, msg_router, active_actuators_list) -> None: + + self.active_actuators = active_actuators_list # List of active actuators + self.msg_router = msg_router # MessageRouter class instance + + # create threading events common to all threads self._quit_event = threading.Event() # Event to signal threads to quit. self._pause_event = threading.Event() # Event to signal threads to pause. self._log_event = threading.Event() # Event to signal threads to log - self.post_office = post_office # PostOffice class instance - # Thread event inits + # initialize thread events self._quit_event.set() # exo is running self._pause_event.clear() # exo starts paused self._log_event.clear() # exo starts not logging - def start(self) -> None: - """Start actuator threads""" + # initialize dict of threads + self._threads = {} - for actuator in self.actuators.values(): - # start the actuator - actuator.start() + def stop(self) -> None: + """Signal threads to quit and join them""" - # set-up control modes and gains here before starting the threads - self.set_actuator_mode_and_gains(actuator) + # setting the quit event so all threads recieve the kill signal + self._quit_event.set() - # creating and starting threads for each actuator + def start(self) -> None: + """ + Creates and starts the following threads: + - for each ACTIVE actuator + - for the Gait State Estimator + - for the GUI + """ + for actuator in self.active_actuators: self.initialize_actuator_thread(actuator) CONSOLE_LOGGER.debug(f"Started actuator thread for {actuator.side}") @@ -352,28 +664,6 @@ def start(self) -> None: # creating 1 thread for GUI communication self.initialize_GUI_thread() - def stop(self) -> None: - """Signal threads to quit and join them""" - # setting the quit event so all threads recieve the kill signal - self._quit_event.set() - super().stop() - - def create_interthread_queue(self, name:str, max_size:int=0) -> queue.Queue: - """ - Create a FIFO queue for inter-thread communication. - Queues stored in dictionary with the name as the key. - - Args: - name (str): A unique name for the queue, typically the side of the actuator or thread. - Returns: - queue.Queue: A FIFO queue for inter-thread communication. - """ - - # create a FIFO queue with max size for inter-thread communication & store to dictionary - self.queues[name] = queue.Queue(maxsize=max_size) - - return self.queues[name] - def initialize_actuator_thread(self, actuator: DephyEB51Actuator) -> None: """ Create and start a thread for the specified actuator. @@ -426,31 +716,26 @@ def initialize_GUI_thread(self) -> None: LOGGER.debug(f"Started gui thread") self._threads[name] = gui_thread - def set_actuator_mode_and_gains(self, actuator)-> None: + def return_active_threads(self)->list: """ - Call the setup_controller method for all actuators. - This method selects current control mode and sets PID gains for each actuator. + Return list of active thread addresses. """ - actuator.set_control_mode(CONTROL_MODES.CURRENT) - LOGGER.info("finished setting control mode") - - actuator.set_current_gains( - kp=DEFAULT_CURRENT_GAINS.kp, - ki=DEFAULT_CURRENT_GAINS.ki, - kd=DEFAULT_CURRENT_GAINS.kd, - ff=DEFAULT_CURRENT_GAINS.ff, - ) - LOGGER.info("finished setting gains") + return self._threads.values() - def update(self)->None: - """Required by RobotBase, but passing since ActuatorThread handles iterative exo state updates""" - pass + def __enter__(self) -> None: + """ + Context manager enter method. + This method is called when the ThreadManager is used in a with statement. + """ + self.start() - def return_active_threads(self)->list: + def __exit__(self) -> None: """ - Return list of active thread addresses. + Context manager exit method. + This method is called when the ThreadManager is used in a with statement. + It stops all threads and cleans up. """ - return self._threads.values() + self.start() @property def left_exo_queue(self) -> Dict[str, queue.Queue]: @@ -493,44 +778,6 @@ def gui_thread(self) -> Optional[ActuatorThread]: return self._threads.get("gui") - -class ThreadManager: - """ - This class manages thread creation, communication and termination for the exoskeleton system. - """ - - def __init__(self, msg_router) -> None: - - self._threads = {} - self._quit_event = threading.Event() # Event to signal threads to quit. - self._pause_event = threading.Event() # Event to signal threads to pause. - self._log_event = threading.Event() # Event to signal threads to log - self.msg_router = msg_router # MessageRouter class instance - - # initialize thread events - self._quit_event.set() # exo is running - self._pause_event.clear() # exo starts paused - self._log_event.clear() # exo starts not logging - - def __enter__(self): - """ - Context manager enter method. - This method is called when the ThreadManager is used in a with statement. - It starts all threads. - """ - self.start() - return self - - def __exit__(self): - """ - Context manager exit method. - This method is called when the ThreadManager is used in a with statement. - It stops all threads and cleans up. - """ - self.stop() - - - # Example main loop from opensourceleg.utilities import SoftRealtimeLoop from src.utils.actuator_utils import create_actuators @@ -539,20 +786,19 @@ def __exit__(self): if __name__ == '__main__': - post_office = PostOffice() + post_office = MessageRouter() actuators = create_actuators(1, EXO_SETUP_CONST.BAUD_RATE, EXO_SETUP_CONST.FLEXSEA_FREQ, EXO_SETUP_CONST.LOG_LEVEL) exoboots = DephyExoboots(tag="exoboots", actuators=actuators, sensors={}, - post_office=post_office) + msg_router=post_office) clock = SoftRealtimeLoop(dt = 1 / 1) # Hz with exoboots: # set-up addressbook for the PostOffice post_office.setup_addressbook(*exoboots.return_active_threads()) - # TODO add start_threads method to DephyExoboots class # exoboots.left_thread.start() exoboots.right_thread.start() exoboots.gse_thread.start() From e69dddd38a3b541ea8552ee059d4a99bf2dafad9 Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Mon, 16 Jun 2025 04:59:03 +0100 Subject: [PATCH 07/27] fixed another merge error in the exoboots.py code. also added messaging to the GSE and Actuator threads with numerous to-dos to complete --- Exoboot_Messenger_Hub.py | 3 + exoboots.py | 26 +---- threading_demo_updated.py | 213 +++++++++++++++++++------------------- 3 files changed, 111 insertions(+), 131 deletions(-) diff --git a/Exoboot_Messenger_Hub.py b/Exoboot_Messenger_Hub.py index a25e25d..a6cd5d8 100644 --- a/Exoboot_Messenger_Hub.py +++ b/Exoboot_Messenger_Hub.py @@ -92,6 +92,9 @@ def setup_addressbook(self, *threads: Thread) -> None: """ Set up threads in the addressbook with corresponding inboxes. To access threads' inboxes, use the thread's `inbox` attribute. + + REMINDER: addressbook must be created prior to starting threads + Args: *threads (Thread): Threads to register in the addressbook. """ diff --git a/exoboots.py b/exoboots.py index cd9b9b7..d2b5651 100644 --- a/exoboots.py +++ b/exoboots.py @@ -23,7 +23,6 @@ ) from dephyEB51 import DephyEB51Actuator - class DephyExoboots(RobotBase[DephyEB51Actuator, SensorBase]): def start(self) -> None: @@ -62,24 +61,6 @@ def update(self) -> None: # print(f"Updating exoskeleton robot: {self.tag}") super().update() - def setup_control_modes(self) -> None: - """ - Call the setup_controller method for all actuators. - This method selects current control mode and sets PID gains for each actuator. - """ - - for actuator in self.actuators.values(): - actuator.set_control_mode(CONTROL_MODES.CURRENT) - CONSOLE_LOGGER.info("finished setting control mode") - - actuator.set_current_gains( - kp=DEFAULT_PID_GAINS.KP, - ki=DEFAULT_PID_GAINS.KI, - kd=DEFAULT_PID_GAINS.KD, - ff=DEFAULT_PID_GAINS.FF, - ) - CONSOLE_LOGGER.info("finished setting gains") - def spool_belts(self): """ Spool the belts of both actuators. @@ -96,7 +77,7 @@ def set_to_transparent_mode(self): self.update_current_setpoints(current_inputs=0, asymmetric=False) self.command_currents() - def detect_active_actuators(self) -> Union[start, list[str]]: + def detect_active_actuators(self) -> Union[str, list[str]]: """ Detect active actuators. Returns a string if only one actuator is active, otherwise a list of strings. @@ -327,11 +308,6 @@ def right(self) -> DephyEB51Actuator: CONSOLE_LOGGER.error("Ankle actuator not found. Please check for `right` key in the actuators dictionary.") exit(1) - try: - return self.actuators["right"] - except KeyError: - CONSOLE_LOGGER.error("Ankle actuator not found. Please check for `right` key in the actuators dictionary.") - exit(1) # DEMO: if __name__ == "__main__": diff --git a/threading_demo_updated.py b/threading_demo_updated.py index 3e866e6..8643645 100644 --- a/threading_demo_updated.py +++ b/threading_demo_updated.py @@ -122,7 +122,7 @@ class ActuatorThread(BaseWorkerThread): def __init__(self, name: str, actuator, - post_office, + msg_router, quit_event: Type[threading.Event], pause_event: Type[threading.Event], log_event: Type[threading.Event], @@ -130,13 +130,26 @@ def __init__(self, super().__init__(quit_event, pause_event, log_event, name, frequency=frequency) - self.post_office = post_office - self.mailbox = None - self.actuator = actuator + self.actuator.start() # start the actuator + + self.msg_router = msg_router + self.inbox = None def pre_iterate(self)->None: - pass + # Check mailbox for messages from GSE or other threads + mail_list = self.inbox.get_all_mail() + for mail in mail_list: + self.decode_message(mail) + + def decode_message(self, mail): + try: + for key, value in mail.contents.items(): + # TODO: ensure key matches the actuator's name (want to act according to the actuator's side) + if hasattr(self, key): + setattr(self, key, value) + except Exception as err: + self.data_logger.debug(f"Error decoding message: {err}") def iterate(self)->None: """ @@ -145,20 +158,10 @@ def iterate(self)->None: It handles the actuator's state updates. """ self.actuator.update() - self.data_logger.debug(f"motor position: {self.actuator.motor_position:.2f}") - - # create a queue that can send the actuator state to the DephyExoboots Robot class - self.enqueue_actuator_states() + CONSOLE_LOGGER.debug(f"motor position: {self.actuator.motor_position:.2f}") - def enqueue_actuator_states(self)-> None: - """ - Stores a dict of actuators state in the queue for the main thread. - This method is called to send the actuator's state to the main thread. - """ - self.actuator_queue.put({ - "motor_current": self.actuator.motor_current, - "temperature": self.actuator.case_temperature, - }) + # TODO: use exoboots context manager to potentially execute code + # TODO: command assistance def post_iterate(self)->None: # TODO add rest of stuff @@ -175,7 +178,7 @@ class GaitStateEstimatorThread(BaseWorkerThread): This class handles gait state estimation for BOTH exoskeletons/sides together. """ def __init__(self, - post_office, + msg_router, quit_event:Type[threading.Event], pause_event:Type[threading.Event], log_event: Type[threading.Event], @@ -184,21 +187,17 @@ def __init__(self, super().__init__(quit_event, pause_event, log_event, name=name, frequency=frequency) - self.post_office = post_office - self.mailbox = None + self.msg_router = msg_router + self.mailbox = None # will be instantiated # instantiate the walking simulator self.walker = WalkingSimulator(stride_period=1.20) - # TODO: initialize GSE Bertec here def pre_iterate(self)->None: pass - def pre_iterate(self)->None: - pass - def iterate(self): """ Main loop for the actuator thread. @@ -209,27 +208,27 @@ def iterate(self): self.time_in_stride = self.walker.update_time_in_stride() self.ank_angle = self.walker.update_ank_angle() - # TODO: do update_bertec_estimator.update() here - - # TODO: DON"T LOG HERE (make ligtweight) - self.data_logger.debug(f"Time in stride: {self.time_in_stride:.3f}s") - self.data_logger.debug(f"Ankle angle: {self.ank_angle:.2f} deg") - - # create a queue that can send the gait state to the DephyExoboots Robot class - self.enqueue_gait_states() - - def post_iterate(self)->None: - pass - - def enqueue_gait_states(self)-> None: - """ - Stores a dict of gait states in the queue for the main thread. - This method is called to send the gait state to the main thread. - """ - self.gse_queue.put({ - "time_in_stride": self.walker.time_in_stride, - "ank_angle": self.walker.ank_angle, - }) + # TODO: send corresponding time in stride (left and right) to the corresponding ACTIVE actuators (left and right) + try: + if hasattr(self.walker, 'left_stride_period'): + self.msg_router.send( + sender=self.name, + recipient="left", + contents={"stride_period": self.walker.left_stride_period} + ) + if hasattr(self.walker, 'right_stride_period'): + self.msg_router.send( + sender=self.name, + recipient="right", + contents={"stride_period": self.walker.right_stride_period} + ) + except Exception as e: + CONSOLE_LOGGER.debug(f"Error sending stride period: {e}") + + CONSOLE_LOGGER.debug(f"Time in stride: {self.time_in_stride:.3f}s") + CONSOLE_LOGGER.debug(f"Ankle angle: {self.ank_angle:.2f} deg") + + # TODO: send gait state to the actuators def post_iterate(self)->None: pass @@ -249,7 +248,7 @@ class GUICommunication(BaseWorkerThread): It then sends these setpoints to the actuators to handle. """ def __init__(self, - post_office, + msg_router, quit_event:Type[threading.Event], pause_event:Type[threading.Event], log_event: Type[threading.Event], @@ -259,13 +258,13 @@ def __init__(self, super().__init__(quit_event, pause_event, log_event, name=name, frequency=frequency) - self.post_office = post_office - self.mailbox = None + self.msg_router = msg_router + self.inbox = None self.torque_setpoint = 20.0 def pre_iterate(self)->None: # read mail regardless of paused state - mail_list = self.mailbox.getmail_all() # return list of mail + mail_list = self.inbox.get_all_mail() # return list of mail CONSOLE_LOGGER.debug(f"mail received in GUICommunication thread {len(mail_list)}") # unpackage the mail @@ -278,13 +277,12 @@ def decode_message(self, mail:list) -> Optional[Any]: Decode a message from the mailbox. """ try: - # example contents {"peak_torque": 30.1, "stride_period": 1.3} - for key, value in mail["contents"].items(): - #TODO value validation (not None or something) + for key, value in mail.contents.items(): + # TODO: value validation (not None or something) setattr(self, key, value) except Exception as err: - LOGGER.debug("Error decoding message:", err) + LOGGER.debug(f"Error decoding message: {err}") return None def iterate(self): @@ -314,6 +312,7 @@ def on_pause(self)->None: from typing import Union, Dict import time + class DephyExoboots(RobotBase[DephyEB51Actuator, SensorBase]): def start(self) -> None: @@ -352,24 +351,6 @@ def update(self) -> None: # print(f"Updating exoskeleton robot: {self.tag}") super().update() - def setup_control_modes(self) -> None: - """ - Call the setup_controller method for all actuators. - This method selects current control mode and sets PID gains for each actuator. - """ - - for actuator in self.actuators.values(): - actuator.set_control_mode(CONTROL_MODES.CURRENT) - CONSOLE_LOGGER.info("finished setting control mode") - - actuator.set_current_gains( - kp=DEFAULT_PID_GAINS.KP, - ki=DEFAULT_PID_GAINS.KI, - kd=DEFAULT_PID_GAINS.KD, - ff=DEFAULT_PID_GAINS.FF, - ) - CONSOLE_LOGGER.info("finished setting gains") - def spool_belts(self): """ Spool the belts of both actuators. @@ -386,7 +367,7 @@ def set_to_transparent_mode(self): self.update_current_setpoints(current_inputs=0, asymmetric=False) self.command_currents() - def detect_active_actuators(self) -> Union[start, list[str]]: + def detect_active_actuators(self) -> Union[str, list[str]]: """ Detect active actuators. Returns a string if only one actuator is active, otherwise a list of strings. @@ -618,15 +599,16 @@ def right(self) -> DephyEB51Actuator: exit(1) + class ThreadManager: """ This class manages thread creation, communication and termination for the exoskeleton system. """ - def __init__(self, msg_router, active_actuators_list) -> None: + def __init__(self, msg_router:MessageRouter, actuators:Dict) -> None: - self.active_actuators = active_actuators_list # List of active actuators - self.msg_router = msg_router # MessageRouter class instance + self.actuators = actuators # Dictionary of Actuators + self.msg_router = msg_router # MessageRouter class instance # create threading events common to all threads self._quit_event = threading.Event() # Event to signal threads to quit. @@ -641,22 +623,16 @@ def __init__(self, msg_router, active_actuators_list) -> None: # initialize dict of threads self._threads = {} - def stop(self) -> None: - """Signal threads to quit and join them""" - - # setting the quit event so all threads recieve the kill signal - self._quit_event.set() - def start(self) -> None: """ - Creates and starts the following threads: + Creates the following threads: - for each ACTIVE actuator - for the Gait State Estimator - for the GUI """ - for actuator in self.active_actuators: + + for actuator in self.actuators.values(): self.initialize_actuator_thread(actuator) - CONSOLE_LOGGER.debug(f"Started actuator thread for {actuator.side}") # creating 1 thread for the Gait State Estimator self.initialize_GSE_thread() @@ -664,6 +640,23 @@ def start(self) -> None: # creating 1 thread for GUI communication self.initialize_GUI_thread() + def start_all_threads(self)->None: + """ + Start all threads in the thread manager. + This method is called to start all threads after they have been initialized. + """ + + for thread in self._threads.values(): + thread.start() + LOGGER.debug(f"Thread {thread.name} started.") + + def stop(self) -> None: + """Signal threads to quit and join them""" + + # setting the quit event so all threads recieve the kill signal + self._quit_event.set() + LOGGER.debug("Setting quit event for all threads.") + def initialize_actuator_thread(self, actuator: DephyEB51Actuator) -> None: """ Create and start a thread for the specified actuator. @@ -676,10 +669,10 @@ def initialize_actuator_thread(self, actuator: DephyEB51Actuator) -> None: log_event=self._log_event, name=f"{actuator.side}", frequency=1, - post_office=self.post_office, + msg_router=self.msg_router, ) - LOGGER.debug(f"Started actuator thread for {actuator.side}") + LOGGER.debug(f"created {actuator.side} actuator thread") self._threads[actuator.side] = actuator_thread def initialize_GSE_thread(self) -> None: @@ -694,9 +687,9 @@ def initialize_GSE_thread(self) -> None: log_event=self._log_event, name=name, frequency=1, - post_office=self.post_office) + msg_router=self.msg_router) - LOGGER.debug(f"Started gse thread") + LOGGER.debug(f"created gse thread") self._threads[name] = gse_thread def initialize_GUI_thread(self) -> None: @@ -712,8 +705,8 @@ def initialize_GUI_thread(self) -> None: log_event=self._log_event, name=name, frequency=1, - post_office=self.post_office,) - LOGGER.debug(f"Started gui thread") + msg_router=self.msg_router) + LOGGER.debug(f"created gui thread") self._threads[name] = gui_thread def return_active_threads(self)->list: @@ -729,13 +722,13 @@ def __enter__(self) -> None: """ self.start() - def __exit__(self) -> None: + def __exit__(self, exc_type, exc_value, traceback) -> None: """ Context manager exit method. This method is called when the ThreadManager is used in a with statement. It stops all threads and cleans up. """ - self.start() + self.stop() @property def left_exo_queue(self) -> Dict[str, queue.Queue]: @@ -786,30 +779,38 @@ def gui_thread(self) -> Optional[ActuatorThread]: if __name__ == '__main__': - post_office = MessageRouter() + # create actuators + actuators = create_actuators(gear_ratio=1, + baud_rate=EXO_SETUP_CONST.BAUD_RATE, + freq=EXO_SETUP_CONST.FLEXSEA_FREQ, + debug_level=EXO_SETUP_CONST.LOG_LEVEL) - actuators = create_actuators(1, EXO_SETUP_CONST.BAUD_RATE, EXO_SETUP_CONST.FLEXSEA_FREQ, EXO_SETUP_CONST.LOG_LEVEL) + # create Exoboots Robot exoboots = DephyExoboots(tag="exoboots", actuators=actuators, - sensors={}, - msg_router=post_office) + sensors={}) + + # create a message router for inter-thread communication + msg_router = MessageRouter() + + # instantiate thread manager + system_manager = ThreadManager(msg_router=msg_router, actuators=actuators) + + # instantiate soft real-time clock clock = SoftRealtimeLoop(dt = 1 / 1) # Hz - with exoboots: - # set-up addressbook for the PostOffice - post_office.setup_addressbook(*exoboots.return_active_threads()) + with system_manager: + # set-up addressbook for the PostOffice & create inboxes for each thread + msg_router.setup_addressbook(*system_manager.return_active_threads()) - # exoboots.left_thread.start() - exoboots.right_thread.start() - exoboots.gse_thread.start() - exoboots.gui_thread.start() - CONSOLE_LOGGER.debug("All threads started.") + # start all threads + system_manager.start_all_threads() for t in clock: try: # send mail to the GUI thread - post_office.send(sender="main", recipient="gui", contents={"torque_setpoint": 30.0}) + msg_router.send(sender="main", recipient="gui", contents={"torque_setpoint": 30.0}) print("Main sent") # TODO: pass reported values into AssistanceGenerator class -> method is part of Exoboots Robot class From 4bba62238434e118dc273a8f38cacfbedd550057 Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Mon, 16 Jun 2025 19:42:10 +0100 Subject: [PATCH 08/27] added several todos...threading demo not sending msgs consistently. --- dephyEB51.py | 2 +- ...ssenger_Hub.py => exoboot_messenger_hub.py | 0 .../forceplate/ZMQ_PubSub.py | 33 +- tests/test_postal_service.py | 2 +- threading_demo.py | 683 +++++++++++---- threading_demo_updated.py | 829 ------------------ 6 files changed, 523 insertions(+), 1026 deletions(-) rename Exoboot_Messenger_Hub.py => exoboot_messenger_hub.py (100%) delete mode 100644 threading_demo_updated.py diff --git a/dephyEB51.py b/dephyEB51.py index 9f0c7f1..472d4e9 100644 --- a/dephyEB51.py +++ b/dephyEB51.py @@ -150,7 +150,7 @@ def update(self): self.filter_temp() # update the actuator state - super().update_allData() + super().update() # update the gear ratio self.update_gear_ratio() diff --git a/Exoboot_Messenger_Hub.py b/exoboot_messenger_hub.py similarity index 100% rename from Exoboot_Messenger_Hub.py rename to exoboot_messenger_hub.py diff --git a/src/exo/gait_state_estimator/forceplate/ZMQ_PubSub.py b/src/exo/gait_state_estimator/forceplate/ZMQ_PubSub.py index ac0dc6d..a3d53a6 100644 --- a/src/exo/gait_state_estimator/forceplate/ZMQ_PubSub.py +++ b/src/exo/gait_state_estimator/forceplate/ZMQ_PubSub.py @@ -1,36 +1,37 @@ """ -This module implements a publisher/subscriber networking protocol using ZMQ. -Its development was motivated by the need to share timestamps between two python programs on different PCs. -My current usage is to have the OSL as a publisher of its current log timestamp. -A script on a Vicon PC will subscribe to that timestamp to embed sync behavior in a vicon log. +This module implements a publisher/subscriber networking protocol using ZMQ. +Its development was motivated by the need to share timestamps between two python programs on different PCs. +My current usage is to have the OSL as a publisher of its current log timestamp. +A script on a Vicon PC will subscribe to that timestamp to embed sync behavior in a vicon log. -See the test_sub() and test_pub() methods for example usages. +See the test_sub() and test_pub() methods for example usages. Requires pyzmq Kevin Best, 8/17/2023 """ import zmq +from typing import Tuple class Subscriber(): - def __init__(self, publisher_ip = 'localhost', publisher_port = "5556", + def __init__(self, publisher_ip = 'localhost', publisher_port = "5556", timeout_ms = 100, topic_filter = '', get_latest_only = True) -> None: """ - Instantiate a subscriber object. Requires the IP address of the publisher, the port of the publisher, an optional timeout flag, optional topic filter, and a bool to ask for only the latest value if there are more than one in the buffer. + Instantiate a subscriber object. Requires the IP address of the publisher, the port of the publisher, an optional timeout flag, optional topic filter, and a bool to ask for only the latest value if there are more than one in the buffer. """ # Socket to talk to server context = zmq.Context() self.socket = context.socket(zmq.SUB) self.socket.setsockopt_string(zmq.SUBSCRIBE, topic_filter) if get_latest_only: - self.socket.setsockopt(zmq.CONFLATE, 1) + self.socket.setsockopt(zmq.CONFLATE, 1) self.socket.connect(("tcp://" + publisher_ip + ":%s") % publisher_port) self.timeout_ms = timeout_ms self.encoding = 'UTF-8' - - def get_message(self) -> (str, str, bool): + + def get_message(self) -> Tuple[str, str, bool]: """ - Checks for a message from the subscribed topics. If no message is available in the timeout, it'll return blank topic and message. The msg_received flag will also be false. + Checks for a message from the subscribed topics. If no message is available in the timeout, it'll return blank topic and message. The msg_received flag will also be false. Returns topic, message, msg_received flag """ msg_received = self.socket.poll(self.timeout_ms) @@ -46,8 +47,8 @@ def get_message(self) -> (str, str, bool): class Publisher(): - """ - Instantiates a publisher object. Only required input is a port. Any subscriber on the network can subscribe to this topic via the IP address of the publisher. + """ + Instantiates a publisher object. Only required input is a port. Any subscriber on the network can subscribe to this topic via the IP address of the publisher. """ def __init__(self, port = "5556") -> None: self.context = zmq.Context() @@ -57,7 +58,7 @@ def __init__(self, port = "5556") -> None: def publish(self, topic, message) -> None: """ - Publish a message on a specified topic. Note that topic names CANNOT have spaces in them. + Publish a message on a specified topic. Note that topic names CANNOT have spaces in them. """ assert " " not in topic, "topic name cannot have spaces!" self.socket.send_string(topic + " " + message) @@ -65,7 +66,7 @@ def publish(self, topic, message) -> None: def testSub(): """ - Import this method and run in a thread to test the subscriber behavior. Once you start a publisher, you should see the data parsed. + Import this method and run in a thread to test the subscriber behavior. Once you start a publisher, you should see the data parsed. """ sub = Subscriber() while(1): @@ -75,7 +76,7 @@ def testSub(): def testPub(): """ - Import this method and run in a thread to see publisher behavior. You should see whatever data you sent here appear in a subscriber. + Import this method and run in a thread to see publisher behavior. You should see whatever data you sent here appear in a subscriber. """ pub = Publisher() i = 0 diff --git a/tests/test_postal_service.py b/tests/test_postal_service.py index c0a5b3b..791e4e6 100644 --- a/tests/test_postal_service.py +++ b/tests/test_postal_service.py @@ -1,5 +1,5 @@ import pytest -from Exoboot_Messenger_Hub import Mail, MailBox, PostOffice +from exoboot_messenger_hub import Mail, MailBox, PostOffice class DummyThread: def __init__(self, name: str): diff --git a/threading_demo.py b/threading_demo.py index 6dc7722..b7d6e4b 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -10,14 +10,8 @@ # logging setup from src.utils.filing_utils import get_logging_info -from opensourceleg.logging import Logger, LogLevel -CONSOLE_LOGGER = Logger(enable_csv_logging=False, - log_path=get_logging_info(user_input_flag=False)[0], - stream_level = LogLevel.INFO, - log_format = "%(levelname)s: %(message)s" - ) +from opensourceleg.logging import Logger, LogLevel, LOGGER -from Exoboot_Messenger_Hub import PostOffice class BaseWorkerThread(threading.Thread, ABC): """ @@ -59,10 +53,10 @@ def run(self)->None: """ LOGGER.debug(f"[{self.name}] Thread running.") - while self.quit_event.is_set(): # while not quitting + while self.quit_event.is_set(): # while not quitting self.pre_iterate() # run a pre-iterate method - if not self.pause_event.is_set(): + if self.pause_event.is_set(): self.on_pause() # if paused, run once else: try: @@ -119,10 +113,11 @@ class ActuatorThread(BaseWorkerThread): Threading class for EACH actuator. This class handles the actuator's state updates and manages the actuator's control loop. """ + def __init__(self, name: str, actuator, - post_office, + msg_router, quit_event: Type[threading.Event], pause_event: Type[threading.Event], log_event: Type[threading.Event], @@ -130,13 +125,33 @@ def __init__(self, super().__init__(quit_event, pause_event, log_event, name, frequency=frequency) - self.post_office = post_office - self.mailbox = None - self.actuator = actuator + self.actuator.start() # start the actuator + + self.msg_router = msg_router + self.inbox = None def pre_iterate(self)->None: - pass + """ + Check inbox for messages from GSE & GUI threads + """ + + mail_list = self.inbox.get_all_mail() + for mail in mail_list: + self.decode_message(mail) + + def decode_message(self, mail): + """ + Decode a message from the inbox. + This method extracts the contents of the message and updates the actuator's state accordingly. + """ + try: + for key, value in mail.contents.items(): + # TODO: value validation (not None or something) + setattr(self, key, value) + + except Exception as err: + LOGGER.debug(f"Error decoding message: {err}") def iterate(self)->None: """ @@ -144,21 +159,17 @@ def iterate(self)->None: This method is called repeatedly in the thread's run loop. It handles the actuator's state updates. """ + self.actuator.update() - self.data_logger.debug(f"motor position: {self.actuator.motor_position:.2f}") + LOGGER.debug(f"motor position: {self.actuator.motor_position:.2f}") - # create a queue that can send the actuator state to the DephyExoboots Robot class - self.enqueue_actuator_states() + # TODO: use exoboots context manager to potentially execute code + # TODO: pass reported values into AssistanceGenerator class -> method is part of Exoboots Robot class + # TODO: determine appropriate torque setpoint given current gait state - def enqueue_actuator_states(self)-> None: - """ - Stores a dict of actuators state in the queue for the main thread. - This method is called to send the actuator's state to the main thread. - """ - self.actuator_queue.put({ - "motor_current": self.actuator.motor_current, - "temperature": self.actuator.case_temperature, - }) + # TODO: send torque setpoints to each corresponding actuator + # TODO: determine appropriate current setpoint that matches the torque setpoint -> handled by DephyEB51Actuator class (within each actuator thread) + # TODO: command appropriate current setpoint using DephyExoboots class def post_iterate(self)->None: # TODO add rest of stuff @@ -169,32 +180,44 @@ def on_pause(self)->None: pass + +from gse_bertec import Bertec_Estimator +from src.exo.gait_state_estimator.forceplate.ZMQ_PubSub import Subscriber +from src.settings.constants import IP_ADDRESSES + class GaitStateEstimatorThread(BaseWorkerThread): """ Threading class for the Gait State Estimator. This class handles gait state estimation for BOTH exoskeletons/sides together. """ + def __init__(self, - post_office, + msg_router, quit_event:Type[threading.Event], pause_event:Type[threading.Event], log_event: Type[threading.Event], + active_actuators:list[str], name:Optional[str] = None, frequency:int=100)->None: super().__init__(quit_event, pause_event, log_event, name=name, frequency=frequency) - self.post_office = post_office - self.mailbox = None + self.msg_router = msg_router + self.mailbox = None # will be instantiated - # instantiate the walking simulator - self.walker = WalkingSimulator(stride_period=1.20) + self.active_actuators = active_actuators + self.bertec_estimators = {} + # for each active actuator, initialize GSE Bertec + for actuator in self.active_actuators: + selected_topic = f"fz_{actuator}" # e.g., 'fz_left' or 'fz_right' - # TODO: initialize GSE Bertec here + # TODO: REMOVE -- FOR TESTING ONLY + self.bertec_estimators[actuator] = WalkingSimulator(stride_period=1.20) - def pre_iterate(self)->None: - pass + # TODO: UNCOMMENT + # bertec_subscriber = Subscriber(publisher_ip=IP_ADDRESSES.VICON_IP, topic_filter=selected_topic, timeout_ms=5) + # self.bertec_estimator[actuator] = Bertec_Estimator(zmq_subscriber=bertec_subscriber) def pre_iterate(self)->None: pass @@ -203,33 +226,22 @@ def iterate(self): """ Main loop for the actuator thread. This method is called repeatedly in the thread's run loop. - It handles the actuator's state updates. - """ - - self.time_in_stride = self.walker.update_time_in_stride() - self.ank_angle = self.walker.update_ank_angle() - # TODO: do update_bertec_estimator.update() here - - # TODO: DON"T LOG HERE (make ligtweight) - self.data_logger.debug(f"Time in stride: {self.time_in_stride:.3f}s") - self.data_logger.debug(f"Ankle angle: {self.ank_angle:.2f} deg") + It updates the gait state estimator and sends the current time in stride and stride period to the actuators. + """ - # create a queue that can send the gait state to the DephyExoboots Robot class - self.enqueue_gait_states() + # for each active actuator, + for actuator in self.active_actuators: - def post_iterate(self)->None: - pass + # TODO: update the gait state estimator for the actuator + # self.bertec_estimators[actuator].update() - def enqueue_gait_states(self)-> None: - """ - Stores a dict of gait states in the queue for the main thread. - This method is called to send the gait state to the main thread. - """ - self.gse_queue.put({ - "time_in_stride": self.walker.time_in_stride, - "ank_angle": self.walker.ank_angle, - }) + # send message to actuator inboxes + try: + msg_router.send(sender=self.name, recipient=actuator, contents={"time_in_stride": self.bertec_estimators[actuator].update_time_in_stride()}) + except: + LOGGER.debug(f"UNABLE TO SEND msg to '{actuator}' actuator from GaitStateEstimatorThread. Skipping.") + continue def post_iterate(self)->None: pass @@ -240,6 +252,8 @@ def on_pause(self)->None: import sys import select +import random + class GUICommunication(BaseWorkerThread): """ Threading class to simulate GUI communication via gRPC. @@ -248,8 +262,9 @@ class GUICommunication(BaseWorkerThread): It constantly monitors for new user input specifying a desired torque setpoint. It then sends these setpoints to the actuators to handle. """ + def __init__(self, - post_office, + msg_router, quit_event:Type[threading.Event], pause_event:Type[threading.Event], log_event: Type[threading.Event], @@ -259,33 +274,15 @@ def __init__(self, super().__init__(quit_event, pause_event, log_event, name=name, frequency=frequency) - self.post_office = post_office - self.mailbox = None + self.msg_router = msg_router + self.inbox = None self.torque_setpoint = 20.0 def pre_iterate(self)->None: - # read mail regardless of paused state - mail_list = self.mailbox.getmail_all() # return list of mail - CONSOLE_LOGGER.debug(f"mail received in GUICommunication thread {len(mail_list)}") - - # unpackage the mail - for mail in mail_list: - self.decode_message(mail) - print(f"torque setpoint is:{self.torque_setpoint}") - - def decode_message(self, mail:list) -> Optional[Any]: """ - Decode a message from the mailbox. + Pre-iterate method to check for new messages in the mailbox. """ - try: - # example contents {"peak_torque": 30.1, "stride_period": 1.3} - for key, value in mail["contents"].items(): - #TODO value validation (not None or something) - setattr(self, key, value) - - except Exception as err: - LOGGER.debug("Error decoding message:", err) - return None + pass def iterate(self): """ @@ -294,8 +291,22 @@ def iterate(self): If the user doesn't input a new value, the current setpoint is used. """ - # Put the torque setpoint into the queue for the main thread - pass + # set a random torque setpoint + self.torque_setpoint = random.randint(1,4)*10 + + # send torque setpoint to the actuator thread + # msg_router.send(sender=self.name, + # recipient="left", + # contents={"torque_setpoint": self.torque_setpoint}) + + try: + msg_router.send(sender=self.name, + recipient="right", + contents={"torque_setpoint": self.torque_setpoint}) + + LOGGER.debug(f"sent torque setpoint {self.torque_setpoint} to actuators") + except: + LOGGER.debug(f"UNABLE TO SEND msg to actuator from GUICommunication thread. Skipping.") def post_iterate(self)->None: pass @@ -311,70 +322,358 @@ def on_pause(self)->None: from opensourceleg.logging import LOGGER from opensourceleg.actuators.base import CONTROL_MODES from opensourceleg.actuators.dephy import DEFAULT_CURRENT_GAINS +from typing import Union, Dict +import time + class DephyExoboots(RobotBase[DephyEB51Actuator, SensorBase]): - def __init__(self, tag, actuators, sensors, post_office) -> None: + + def start(self) -> None: """ - Exoboot Robot class that extends RobotBase. - This class manages thread creation, data queue creation & management, as well as basic actuator control. - It is designed to handle multiple actuators and their respective threads, as well as a - Gait State Estimator (GSE) thread and a GUI communication thread. + Start the Exoskeleton. """ - super().__init__(tag=tag, actuators=actuators, sensors=sensors) - self._threads = {} + super().start() + + def stop(self) -> None: + """ + Stop the Exoskeleton. + """ + + super().stop() + + def set_actuator_mode_and_gains(self, actuator)-> None: + """ + Call the setup_controller method for all actuators. + This method selects current control mode and sets PID gains for each actuator. + """ + actuator.set_control_mode(CONTROL_MODES.CURRENT) + LOGGER.info("finished setting control mode") + + actuator.set_current_gains( + kp=DEFAULT_CURRENT_GAINS.kp, + ki=DEFAULT_CURRENT_GAINS.ki, + kd=DEFAULT_CURRENT_GAINS.kd, + ff=DEFAULT_CURRENT_GAINS.ff, + ) + LOGGER.info("finished setting gains") + + def update(self) -> None: + """ + Update the exoskeleton. + """ + # print(f"Updating exoskeleton robot: {self.tag}") + super().update() + + def spool_belts(self): + """ + Spool the belts of both actuators. + This method is called to prepare the actuators for operation. + """ + for actuator in self.actuators.values(): + actuator.spool_belt() + LOGGER.info(f"finished spooling belt of {actuator.side}") + + def set_to_transparent_mode(self): + """ + Set the exo currents to 0mA. + """ + self.update_current_setpoints(current_inputs=0, asymmetric=False) + self.command_currents() + + def detect_active_actuators(self) -> Union[str, list[str]]: + """ + Detect active actuators. + Returns a string if only one actuator is active, otherwise a list of strings. + """ + + active_sides = list(self.actuators.keys()) + + if len(active_sides) == 1: + return active_sides[0] + + return active_sides + + def create_current_setpts_dict(self) -> None: + """ + create dictionary of current setpoints (in mA) corresponding to actuator side + """ + self.current_setpoints = {} + for actuator in self.actuators.values(): + self.current_setpoints[actuator.side] = 0.0 + + # TODO: generate test to determine if current_setpoints dict has the same keys as the actuators dict + + def update_current_setpoints(self, current_inputs: Union[int, Dict[str, int]], asymmetric:bool=False) -> None: + """ + Directly assign currents to the 'current_setpoints' dictionary for current control. + + If symmetric, the same current value is applied to both sides (with motor sign). + If asymmetric, the user must pass a dictionary specifying currents for each side. + + Args: + - current: int or dict. If symmetric=False, this should be a dict with 'left' and 'right' keys. + - asymmetric: bool. If True, use side-specific currents from the dictionary. + """ + # TODO: ensure that current_inputs matches the number of active sides + # TODO: if more than the number of active sides provided, trim to active one only + # TODO: handle missing sides + + # TODO: clip current setpoints to below max limit + + if asymmetric: + for side, current in current_inputs.items(): # assign different currents for each actuator + actuator = getattr(self, side) + self.current_setpoints[side] = int(current) * actuator.motor_sign + else: + for side in self.actuators.keys(): # assign the same current for both actuators + actuator = getattr(self, side) + self.current_setpoints[side] = int(current_inputs) * actuator.motor_sign + + def convert_torque_to_current_setpoints(self, torque_setpoint: float) -> dict: + """ + Find the appropriate current setpoint for the actuators. + This method is called to determine the current setpoint based on the torque setpoint. + + arguments: + torque_setpoint: float, the desired torque setpoint in Nm. + + + returns: + current_setpoints: dict of currents for each active actuator. + key is the side of the actuator (left or right). + """ + for actuator in self.actuators.values(): + self.current_setpoints[actuator.side] = actuator.torque_to_current(torque_setpoint) + LOGGER.info(f"finished finding current setpoint for {actuator.side}") + + return self.current_setpoints + + def command_currents(self) -> None: + """ + Commands current setpoints to each actuator based on the current_setpoints dictionary. + The setpoints can be unique. + """ + # TODO: ensure current_setpoints values are integers, no greater than max current limit, and are not None + + for actuator in self.actuators.values(): + + current_setpoint = self.current_setpoints.get(actuator.side) + + if current_setpoint is not None: + actuator.set_motor_current(current_setpoint) + LOGGER.info(f"Finished setting current setpoint for {actuator.side}") + else: + LOGGER.warning(f"Unknown side '{actuator.side}' and unable to command current. Skipping.") + + def initialize_rt_plots(self) -> list: + """ + Initialize real-time plots for the exoskeleton robot. + Naming and plotting is flexible to each active actuator. + + The following time series are plotted: + - Current (A) + - Temperature (°C) + - Ankle Angle (°) + - Transmission Ratio + - Ankle Torque Setpoint (Nm) + + """ + # converting actuator dictionary keys to a list + active_sides_list = list(self.actuators.keys()) + + print("Active actuators:", active_sides_list) + + # pre-slice colors based on the number of active actuators + colors = ['r', 'b'][:len(active_sides_list)] + if len(active_sides_list) > len(colors): + raise ValueError("Not enough unique colors for the number of active actuators.") + + # repeat line styles and widths for each active actuator + line_styles = ['-' for _ in active_sides_list] + line_widths = [2 for _ in active_sides_list] + + current_plt_config = {'names' : active_sides_list, + 'colors' : colors, + 'line_style': line_styles, + 'title' : "Exo Current (A) vs. Sample", + 'ylabel': "Current (A)", + 'xlabel': "timestep", + 'line_width': line_widths, + 'yrange': [0,30] + } + + temp_plt_config = {'names' : active_sides_list, + 'colors' : colors, + 'line_style': line_styles, + 'title' : "Case Temperature (°C) vs. Sample", + 'ylabel': "Temperature (°C)", + 'xlabel': "timestep", + 'line_width': line_widths, + 'yrange': [20,60] + } + + in_swing_plt_config = {'names' : active_sides_list, + 'colors' : colors, + 'line_style': line_styles, + 'title' : "Bertec in-swing vs. Sample", + 'ylabel': "Bool", + 'xlabel': "timestep", + 'line_width': line_widths, + 'yrange': [0,150] + } + + TR_plt_config = {'names' : active_sides_list, + 'colors' : colors, + 'line_style': line_styles, + 'title' : "TR (°) vs. Sample", + 'ylabel': "N", + 'xlabel': "timestep", + 'line_width': line_widths, + 'yrange': [0,20] + } + + imu_plt_config = {'names' : active_sides_list, + 'colors' : colors, + 'line_style': line_styles, + 'title' : "Activations vs. Sample", + 'ylabel': "Bool", + 'xlabel': "timestep", + 'line_width': line_widths, + 'yrange': [0,50] + } + + plot_config = [current_plt_config, temp_plt_config, in_swing_plt_config, TR_plt_config, imu_plt_config] + + return plot_config + + def update_rt_plots(self, bertec_swing_flag, imu_activations) -> list: + """ + Updates the real-time plots with current values for: + - Current (A) + - Temperature (°C) + - Bertec In swing + - Transmission Ratio + - IMU estimator activations + + The data is collected from the exoboots object and returned as a list of arrays. + This is done for each active actuator only. + + Returns: + plot_data_array: A list of data arrays (for active actuators) for each plot. + """ + + data_to_plt = [] + + for actuator in self.actuators.values(): + data_to_plt.extend([ + abs(actuator.motor_current), # Motor current + actuator.case_temperature, # Case temperature + bertec_swing_flag, + actuator.gear_ratio, # Gear ratio + imu_activations + ]) + + return data_to_plt + + def track_variables_for_logging(self, logger: Logger) -> None: + """ + Track variables for each active actuator for logging to a single file + """ + + for actuator in self.actuators.values(): + dummy_grpc_value = 5.0 + dummy_ankle_torque_setpt = 20 + logger.track_variable(lambda: time.time(), "pitime") + logger.track_variable(lambda: dummy_grpc_value, "dollar_value") + logger.track_variable(lambda: dummy_ankle_torque_setpt, "torque_setpt_Nm") + + logger.track_variable(lambda: actuator.accelx, f"{actuator._tag}_accelx_mps2") + logger.track_variable(lambda: actuator.motor_current, f"{actuator._tag}_current_mA") + logger.track_variable(lambda: actuator.motor_position, f"{actuator._tag}_position_rad") + logger.track_variable(lambda: actuator.motor_encoder_counts, f"{actuator._tag}_encoder_counts") + logger.track_variable(lambda: actuator.case_temperature, f"{actuator._tag}_case_temp_C") + + tracked_vars = logger.get_tracked_variables() + print("Tracked variables:", tracked_vars) + + @property + def left(self) -> DephyEB51Actuator: + try: + return self.actuators["left"] + except KeyError: + LOGGER.error("Ankle actuator not found. Please check for `left` key in the actuators dictionary.") + exit(1) + + @property + def right(self) -> DephyEB51Actuator: + try: + return self.actuators["right"] + except KeyError: + LOGGER.error("Ankle actuator not found. Please check for `right` key in the actuators dictionary.") + exit(1) + + + +class ThreadManager: + """ + This class manages thread creation, communication and termination for the exoskeleton system. + """ + + def __init__(self, msg_router, actuators:Dict) -> None: + + self.actuators = actuators # Dictionary of Actuators + self.msg_router = msg_router # MessageRouter class instance + + # create threading events common to all threads self._quit_event = threading.Event() # Event to signal threads to quit. self._pause_event = threading.Event() # Event to signal threads to pause. self._log_event = threading.Event() # Event to signal threads to log - self.post_office = post_office # PostOffice class instance - # Thread event inits + # initialize thread events self._quit_event.set() # exo is running self._pause_event.clear() # exo starts paused self._log_event.clear() # exo starts not logging + # initialize dict of threads + self._threads = {} + def start(self) -> None: - """Start actuator threads""" + """ + Creates the following threads: + - for each ACTIVE actuator + - for the Gait State Estimator + - for the GUI + """ for actuator in self.actuators.values(): - # start the actuator - actuator.start() - - # set-up control modes and gains here before starting the threads - self.set_actuator_mode_and_gains(actuator) - - # creating and starting threads for each actuator self.initialize_actuator_thread(actuator) - CONSOLE_LOGGER.debug(f"Started actuator thread for {actuator.side}") # creating 1 thread for the Gait State Estimator - self.initialize_GSE_thread() + self.initialize_GSE_thread(active_actuators=self.actuators.keys()) # creating 1 thread for GUI communication self.initialize_GUI_thread() + def start_all_threads(self)->None: + """ + Start all threads in the thread manager. + This method is called to start all threads after they have been initialized. + """ + + for thread in self._threads.values(): + thread.start() + LOGGER.debug(f"Thread {thread.name} started.") + def stop(self) -> None: """Signal threads to quit and join them""" + # setting the quit event so all threads recieve the kill signal self._quit_event.set() - super().stop() - - def create_interthread_queue(self, name:str, max_size:int=0) -> queue.Queue: - """ - Create a FIFO queue for inter-thread communication. - Queues stored in dictionary with the name as the key. - - Args: - name (str): A unique name for the queue, typically the side of the actuator or thread. - Returns: - queue.Queue: A FIFO queue for inter-thread communication. - """ - - # create a FIFO queue with max size for inter-thread communication & store to dictionary - self.queues[name] = queue.Queue(maxsize=max_size) + LOGGER.debug("Setting quit event for all threads.") - return self.queues[name] + # TODO: ensure that both actuators are properly shut down - def initialize_actuator_thread(self, actuator: DephyEB51Actuator) -> None: + def initialize_actuator_thread(self, actuator:DephyEB51Actuator) -> None: """ Create and start a thread for the specified actuator. This method is called to set up the actuator communication thread. @@ -385,28 +684,33 @@ def initialize_actuator_thread(self, actuator: DephyEB51Actuator) -> None: pause_event=self._pause_event, log_event=self._log_event, name=f"{actuator.side}", - frequency=1, - post_office=self.post_office, + frequency=1000, + msg_router=self.msg_router, ) - LOGGER.debug(f"Started actuator thread for {actuator.side}") + LOGGER.debug(f"created {actuator.side} actuator thread") self._threads[actuator.side] = actuator_thread - def initialize_GSE_thread(self) -> None: + def initialize_GSE_thread(self, active_actuators:list[str]) -> None: """ Create and start the Gait State Estimator thread. This method is called to set up the GSE communication thread. + + Args: + active_actuators: List of active actuators to be monitored by the GSE. + This is used to initialize the Gait State Estimator. """ # create a FIFO queue with max size for inter-thread communication name = "gse" gse_thread = GaitStateEstimatorThread(quit_event=self._quit_event, pause_event=self._pause_event, log_event=self._log_event, + active_actuators=active_actuators, name=name, - frequency=1, - post_office=self.post_office) + frequency=250, + msg_router=self.msg_router) - LOGGER.debug(f"Started gse thread") + LOGGER.debug(f"created gse thread") self._threads[name] = gse_thread def initialize_GUI_thread(self) -> None: @@ -421,56 +725,75 @@ def initialize_GUI_thread(self) -> None: pause_event=self._pause_event, log_event=self._log_event, name=name, - frequency=1, - post_office=self.post_office,) - LOGGER.debug(f"Started gui thread") + frequency=100, + msg_router=self.msg_router) + LOGGER.debug(f"created gui thread") self._threads[name] = gui_thread - def set_actuator_mode_and_gains(self, actuator)-> None: + def return_active_threads(self)->list: """ - Call the setup_controller method for all actuators. - This method selects current control mode and sets PID gains for each actuator. + Return list of active thread addresses. """ - actuator.set_control_mode(CONTROL_MODES.CURRENT) - LOGGER.info("finished setting control mode") - - actuator.set_current_gains( - kp=DEFAULT_CURRENT_GAINS.kp, - ki=DEFAULT_CURRENT_GAINS.ki, - kd=DEFAULT_CURRENT_GAINS.kd, - ff=DEFAULT_CURRENT_GAINS.ff, - ) - LOGGER.info("finished setting gains") + return self._threads.values() - def update(self)->None: - """Required by RobotBase, but passing since ActuatorThread handles iterative exo state updates""" - pass + def __enter__(self) -> None: + """ + Context manager enter method. + This method is called when the ThreadManager is used in a with statement. + """ + self.start() - def return_active_threads(self)->list: + def __exit__(self, exc_type, exc_value, traceback) -> None: """ - Return list of active thread addresses. + Context manager exit method. + This method is called when the ThreadManager is used in a with statement. + It stops all threads and cleans up. """ - return self._threads.values() + self.stop() @property - def left_exo_queue(self) -> Dict[str, queue.Queue]: + def left_exo_queue(self) -> Union[None, queue.Queue]: """Get the queue for the left actuator""" - return self.queues.get("left") + thread = self._threads.get("left") + + if hasattr(thread, "inbox"): + return thread.inbox + else: + LOGGER.error("Left actuator thread has no attribute inbox") + return None @property - def right_exo_queue(self) -> Dict[str, queue.Queue]: + def right_exo_queue(self) -> Union[None, queue.Queue]: """Get the queue for the right actuator""" - return self.queues.get("right") + thread = self._threads.get("right") + + if hasattr(thread, "inbox"): + return thread.inbox + else: + LOGGER.error("Right actuator thread has no attribute inbox") + return None @property - def gse_queue(self) -> Dict[str, queue.Queue]: + def gse_queue(self) -> Union[None, queue.Queue]: """Get the queue for the gait state estimator""" - return self.queues.get("gse") + thread = self._threads.get("gse") + + if hasattr(thread, "inbox"): + return thread.inbox + else: + LOGGER.error("GSE thread has no attribute inbox") + return None @property - def gui_queue(self) -> Dict[str, queue.Queue]: + def gui_queue(self) -> Union[None, queue.Queue]: """Get the queue for the GUI communication""" - return self.queues.get("gui") + thread = self._threads.get("gui") + + if hasattr(thread, "inbox"): + return thread.inbox + else: + LOGGER.error("GUI thread has no attribute inbox") + return None @property def left_thread(self) -> Optional[ActuatorThread]: @@ -499,42 +822,44 @@ def gui_thread(self) -> Optional[ActuatorThread]: from src.utils.actuator_utils import create_actuators from src.settings.constants import EXO_SETUP_CONST from src.utils.walking_simulator import WalkingSimulator +from exoboot_messenger_hub import MessageRouter +from rtplot import client if __name__ == '__main__': - post_office = PostOffice() + # create actuators + actuators = create_actuators(gear_ratio=1, + baud_rate=EXO_SETUP_CONST.BAUD_RATE, + freq=EXO_SETUP_CONST.FLEXSEA_FREQ, + debug_level=EXO_SETUP_CONST.LOG_LEVEL) - actuators = create_actuators(1, EXO_SETUP_CONST.BAUD_RATE, EXO_SETUP_CONST.FLEXSEA_FREQ, EXO_SETUP_CONST.LOG_LEVEL) + # create Exoboots Robot exoboots = DephyExoboots(tag="exoboots", actuators=actuators, - sensors={}, - post_office=post_office) - clock = SoftRealtimeLoop(dt = 1 / 1) # Hz + sensors={}) - with exoboots: - # set-up addressbook for the PostOffice - post_office.setup_addressbook(*exoboots.return_active_threads()) + # create a message router for inter-thread communication + msg_router = MessageRouter() - # TODO add start_threads method to DephyExoboots class - # exoboots.left_thread.start() - exoboots.right_thread.start() - exoboots.gse_thread.start() - exoboots.gui_thread.start() - CONSOLE_LOGGER.debug("All threads started.") + # instantiate thread manager + system_manager = ThreadManager(msg_router=msg_router, actuators=actuators) - for t in clock: - try: + # instantiate soft real-time clock + clock = SoftRealtimeLoop(dt = 1 / 1) # Hz - # send mail to the GUI thread - post_office.send(sender="main", recipient="gui", contents={"torque_setpoint": 30.0}) - print("Main sent") + with system_manager: + # set-up addressbook for the PostOffice & create inboxes for each thread + msg_router.setup_addressbook(*system_manager.return_active_threads()) - # TODO: pass reported values into AssistanceGenerator class -> method is part of Exoboots Robot class - # TODO: determine appropriate torque setpoint given current gait state + # start all threads + system_manager.start_all_threads() - # TODO: send torque setpoints to each corresponding actuator - # TODO: determine appropriate current setpoint that matches the torque setpoint -> handled by DephyEB51Actuator class (within each actuator thread) - # TODO: command appropriate current setpoint using DephyExoboots class + # unpause exos (removed not from base thread for this to work) + system_manager._pause_event.set() + + for t in clock: + try: + pass except KeyboardInterrupt: print("KeyboardInterrupt received.") diff --git a/threading_demo_updated.py b/threading_demo_updated.py deleted file mode 100644 index 8643645..0000000 --- a/threading_demo_updated.py +++ /dev/null @@ -1,829 +0,0 @@ -# Abstract Base class for threading -import threading -import queue - -from typing import Type, Dict, Any, Optional -from abc import ABC, abstractmethod -from src.utils.flexible_sleeper import FlexibleSleeper -from non_singleton_logger import NonSingletonLogger -from dephyEB51 import DephyEB51Actuator - -# logging setup -from src.utils.filing_utils import get_logging_info -from opensourceleg.logging import Logger, LogLevel -CONSOLE_LOGGER = Logger(enable_csv_logging=False, - log_path=get_logging_info(user_input_flag=False)[0], - stream_level = LogLevel.INFO, - log_format = "%(levelname)s: %(message)s" - ) - -from Exoboot_Messenger_Hub import MessageRouter - -class BaseWorkerThread(threading.Thread, ABC): - """ - Abstract base class for worker threads in the exoskeleton system. - This class provides a template for creating threads that can be paused and quit. - It includes methods for thread lifecycle management and a run method that - handles the thread's main loop. - """ - - def __init__(self, - quit_event:Type[threading.Event], - pause_event:Type[threading.Event], - log_event:Type[threading.Event], - name:str, - frequency:int=100)->None: - - super().__init__(name=name) - self.quit_event = quit_event # event to signal quitting - self.pause_event = pause_event # event to signal pausing - self.log_event = log_event # event to signal logging - - self.daemon = True - self.frequency = int(frequency) - self.rt_loop = FlexibleSleeper(dt=1/frequency) # create a soft real-time loop with the specified frequency - - # set-up a logger for each thread/instance - # logger_name = f"{name}_logger" - # log_path = "./src/logs/" - # self.data_logger = NonSingletonLogger( - # log_path=log_path, - # file_name=logger_name, - # file_level=logging.DEBUG, - # stream_level=logging.INFO - # ) - - def run(self)->None: - """ - Main loop structure for each instantiated thread. - """ - LOGGER.debug(f"[{self.name}] Thread running.") - - while self.quit_event.is_set(): # while not quitting - self.pre_iterate() # run a pre-iterate method - - if not self.pause_event.is_set(): - self.on_pause() # if paused, run once - else: - try: - self.iterate() # call the iterate method to perform the thread's task - except Exception as e: - LOGGER.debug(f"Exception in thread {self.name}: {e}") - - self.post_iterate() # run a post-iterate method - self.rt_loop.pause() - - LOGGER.debug(f"[{self.name}] Thread exiting.") - - @abstractmethod - def pre_iterate(self): - """ - Override this abstract method in subclasses. - This method is called BEFORE the iterate method. - It can be used to perform any repetitive setup or initialization tasks. - """ - LOGGER.debug(f"[{self.name}] pre-iterate() called.") - pass - - @abstractmethod - def iterate(self): - """ - Override this abstract method in subclasses to do the thread's work. - This method contains the main logic for the thread's operation. - """ - LOGGER.debug(f"[{self.name}] iterate() called.") - pass - - @abstractmethod - def post_iterate(self): - """ - Override this abstract method in subclasses. - This method is called AFTER the iterate method. - It can be used to perform any repetitive cleanup or logging, or finalization tasks. - """ - LOGGER.debug(f"[{self.name}] post-iterate() called.") - pass - - @abstractmethod - def on_pause(self): - """ - Override this abstract method in subclasses. - Runs once before pausing threads. - """ - LOGGER.debug(f"[{self.name}] on_pre_pause() called.") - pass - - -class ActuatorThread(BaseWorkerThread): - """ - Threading class for EACH actuator. - This class handles the actuator's state updates and manages the actuator's control loop. - """ - def __init__(self, - name: str, - actuator, - msg_router, - quit_event: Type[threading.Event], - pause_event: Type[threading.Event], - log_event: Type[threading.Event], - frequency: int = 100) -> None: - - super().__init__(quit_event, pause_event, log_event, name, frequency=frequency) - - self.actuator = actuator - self.actuator.start() # start the actuator - - self.msg_router = msg_router - self.inbox = None - - def pre_iterate(self)->None: - # Check mailbox for messages from GSE or other threads - mail_list = self.inbox.get_all_mail() - for mail in mail_list: - self.decode_message(mail) - - def decode_message(self, mail): - try: - for key, value in mail.contents.items(): - # TODO: ensure key matches the actuator's name (want to act according to the actuator's side) - if hasattr(self, key): - setattr(self, key, value) - except Exception as err: - self.data_logger.debug(f"Error decoding message: {err}") - - def iterate(self)->None: - """ - Main loop for the actuator thread. - This method is called repeatedly in the thread's run loop. - It handles the actuator's state updates. - """ - self.actuator.update() - CONSOLE_LOGGER.debug(f"motor position: {self.actuator.motor_position:.2f}") - - # TODO: use exoboots context manager to potentially execute code - # TODO: command assistance - - def post_iterate(self)->None: - # TODO add rest of stuff - if self.log_event.is_set(): - LOGGER.debug(f"[{self.name}] log_event True") - - def on_pause(self)->None: - pass - - -class GaitStateEstimatorThread(BaseWorkerThread): - """ - Threading class for the Gait State Estimator. - This class handles gait state estimation for BOTH exoskeletons/sides together. - """ - def __init__(self, - msg_router, - quit_event:Type[threading.Event], - pause_event:Type[threading.Event], - log_event: Type[threading.Event], - name:Optional[str] = None, - frequency:int=100)->None: - - super().__init__(quit_event, pause_event, log_event, name=name, frequency=frequency) - - self.msg_router = msg_router - self.mailbox = None # will be instantiated - - # instantiate the walking simulator - self.walker = WalkingSimulator(stride_period=1.20) - - # TODO: initialize GSE Bertec here - - def pre_iterate(self)->None: - pass - - def iterate(self): - """ - Main loop for the actuator thread. - This method is called repeatedly in the thread's run loop. - It handles the actuator's state updates. - """ - - self.time_in_stride = self.walker.update_time_in_stride() - self.ank_angle = self.walker.update_ank_angle() - - # TODO: send corresponding time in stride (left and right) to the corresponding ACTIVE actuators (left and right) - try: - if hasattr(self.walker, 'left_stride_period'): - self.msg_router.send( - sender=self.name, - recipient="left", - contents={"stride_period": self.walker.left_stride_period} - ) - if hasattr(self.walker, 'right_stride_period'): - self.msg_router.send( - sender=self.name, - recipient="right", - contents={"stride_period": self.walker.right_stride_period} - ) - except Exception as e: - CONSOLE_LOGGER.debug(f"Error sending stride period: {e}") - - CONSOLE_LOGGER.debug(f"Time in stride: {self.time_in_stride:.3f}s") - CONSOLE_LOGGER.debug(f"Ankle angle: {self.ank_angle:.2f} deg") - - # TODO: send gait state to the actuators - - def post_iterate(self)->None: - pass - - def on_pause(self)->None: - pass - - -import sys -import select -class GUICommunication(BaseWorkerThread): - """ - Threading class to simulate GUI communication via gRPC. - This class handles GUI communication for BOTH exoskeletons/sides together. - - It constantly monitors for new user input specifying a desired torque setpoint. - It then sends these setpoints to the actuators to handle. - """ - def __init__(self, - msg_router, - quit_event:Type[threading.Event], - pause_event:Type[threading.Event], - log_event: Type[threading.Event], - name:Optional[str] = None, - frequency:int=100)->None: - - - super().__init__(quit_event, pause_event, log_event, name=name, frequency=frequency) - - self.msg_router = msg_router - self.inbox = None - self.torque_setpoint = 20.0 - - def pre_iterate(self)->None: - # read mail regardless of paused state - mail_list = self.inbox.get_all_mail() # return list of mail - CONSOLE_LOGGER.debug(f"mail received in GUICommunication thread {len(mail_list)}") - - # unpackage the mail - for mail in mail_list: - self.decode_message(mail) - print(f"torque setpoint is:{self.torque_setpoint}") - - def decode_message(self, mail:list) -> Optional[Any]: - """ - Decode a message from the mailbox. - """ - try: - for key, value in mail.contents.items(): - # TODO: value validation (not None or something) - setattr(self, key, value) - - except Exception as err: - LOGGER.debug(f"Error decoding message: {err}") - return None - - def iterate(self): - """ - Non-blocking: Only read user input if available. - Allow user to input a new desired torque setpoint. - If the user doesn't input a new value, the current setpoint is used. - """ - - # Put the torque setpoint into the queue for the main thread - pass - - def post_iterate(self)->None: - pass - - def on_pause(self)->None: - pass - - -# Mini Exoboots Robot Class for testing threading -from opensourceleg.robots.base import RobotBase -from opensourceleg.sensors.base import SensorBase -from dephyEB51 import DephyEB51Actuator -from opensourceleg.logging import LOGGER -from opensourceleg.actuators.base import CONTROL_MODES -from opensourceleg.actuators.dephy import DEFAULT_CURRENT_GAINS -from typing import Union, Dict -import time - - -class DephyExoboots(RobotBase[DephyEB51Actuator, SensorBase]): - - def start(self) -> None: - """ - Start the Exoskeleton. - """ - super().start() - - def stop(self) -> None: - """ - Stop the Exoskeleton. - """ - - super().stop() - - def set_actuator_mode_and_gains(self, actuator)-> None: - """ - Call the setup_controller method for all actuators. - This method selects current control mode and sets PID gains for each actuator. - """ - actuator.set_control_mode(CONTROL_MODES.CURRENT) - LOGGER.info("finished setting control mode") - - actuator.set_current_gains( - kp=DEFAULT_CURRENT_GAINS.kp, - ki=DEFAULT_CURRENT_GAINS.ki, - kd=DEFAULT_CURRENT_GAINS.kd, - ff=DEFAULT_CURRENT_GAINS.ff, - ) - LOGGER.info("finished setting gains") - - def update(self) -> None: - """ - Update the exoskeleton. - """ - # print(f"Updating exoskeleton robot: {self.tag}") - super().update() - - def spool_belts(self): - """ - Spool the belts of both actuators. - This method is called to prepare the actuators for operation. - """ - for actuator in self.actuators.values(): - actuator.spool_belt() - CONSOLE_LOGGER.info(f"finished spooling belt of {actuator.side}") - - def set_to_transparent_mode(self): - """ - Set the exo currents to 0mA. - """ - self.update_current_setpoints(current_inputs=0, asymmetric=False) - self.command_currents() - - def detect_active_actuators(self) -> Union[str, list[str]]: - """ - Detect active actuators. - Returns a string if only one actuator is active, otherwise a list of strings. - """ - active_sides = list(self.actuators.keys()) - - if len(active_sides) == 1: - return active_sides[0] - - return active_sides - - def create_current_setpts_dict(self) -> None: - """ - create dictionary of current setpoints (in mA) corresponding to actuator side - """ - self.current_setpoints = {} - for actuator in self.actuators.values(): - self.current_setpoints[actuator.side] = 0.0 - - # TODO: generate test to determine if current_setpoints dict has the same keys as the actuators dict - - def update_current_setpoints(self, current_inputs: Union[int, Dict[str, int]], asymmetric:bool=False) -> None: - """ - Directly assign currents to the 'current_setpoints' dictionary for current control. - - If symmetric, the same current value is applied to both sides (with motor sign). - If asymmetric, the user must pass a dictionary specifying currents for each side. - - Args: - - current: int or dict. If symmetric=False, this should be a dict with 'left' and 'right' keys. - - asymmetric: bool. If True, use side-specific currents from the dictionary. - """ - # TODO: ensure that current_inputs matches the number of active sides - # TODO: if more than the number of active sides provided, trim to active one only - # TODO: handle missing sides - - # TODO: clip current setpoints to below max limit - - if asymmetric: - for side, current in current_inputs.items(): # assign different currents for each actuator - actuator = getattr(self, side) - self.current_setpoints[side] = int(current) * actuator.motor_sign - else: - for side in self.actuators.keys(): # assign the same current for both actuators - actuator = getattr(self, side) - self.current_setpoints[side] = int(current_inputs) * actuator.motor_sign - - def convert_torque_to_current_setpoints(self, torque_setpoint: float) -> dict: - """ - Find the appropriate current setpoint for the actuators. - This method is called to determine the current setpoint based on the torque setpoint. - - arguments: - torque_setpoint: float, the desired torque setpoint in Nm. - - - returns: - current_setpoints: dict of currents for each active actuator. - key is the side of the actuator (left or right). - """ - for actuator in self.actuators.values(): - self.current_setpoints[actuator.side] = actuator.torque_to_current(torque_setpoint) - CONSOLE_LOGGER.info(f"finished finding current setpoint for {actuator.side}") - - return self.current_setpoints - - def command_currents(self) -> None: - """ - Commands current setpoints to each actuator based on the current_setpoints dictionary. - The setpoints can be unique. - """ - # TODO: ensure current_setpoints values are integers, no greater than max current limit, and are not None - - for actuator in self.actuators.values(): - - current_setpoint = self.current_setpoints.get(actuator.side) - - if current_setpoint is not None: - actuator.set_motor_current(current_setpoint) - CONSOLE_LOGGER.info(f"Finished setting current setpoint for {actuator.side}") - else: - CONSOLE_LOGGER.warning(f"Unknown side '{actuator.side}' and unable to command current. Skipping.") - - def initialize_rt_plots(self) -> list: - """ - Initialize real-time plots for the exoskeleton robot. - Naming and plotting is flexible to each active actuator. - - The following time series are plotted: - - Current (A) - - Temperature (°C) - - Ankle Angle (°) - - Transmission Ratio - - Ankle Torque Setpoint (Nm) - - """ - # converting actuator dictionary keys to a list - active_sides_list = list(self.actuators.keys()) - - print("Active actuators:", active_sides_list) - - # pre-slice colors based on the number of active actuators - colors = ['r', 'b'][:len(active_sides_list)] - if len(active_sides_list) > len(colors): - raise ValueError("Not enough unique colors for the number of active actuators.") - - # repeat line styles and widths for each active actuator - line_styles = ['-' for _ in active_sides_list] - line_widths = [2 for _ in active_sides_list] - - current_plt_config = {'names' : active_sides_list, - 'colors' : colors, - 'line_style': line_styles, - 'title' : "Exo Current (A) vs. Sample", - 'ylabel': "Current (A)", - 'xlabel': "timestep", - 'line_width': line_widths, - 'yrange': [0,30] - } - - temp_plt_config = {'names' : active_sides_list, - 'colors' : colors, - 'line_style': line_styles, - 'title' : "Case Temperature (°C) vs. Sample", - 'ylabel': "Temperature (°C)", - 'xlabel': "timestep", - 'line_width': line_widths, - 'yrange': [20,60] - } - - in_swing_plt_config = {'names' : active_sides_list, - 'colors' : colors, - 'line_style': line_styles, - 'title' : "Bertec in-swing vs. Sample", - 'ylabel': "Bool", - 'xlabel': "timestep", - 'line_width': line_widths, - 'yrange': [0,150] - } - - TR_plt_config = {'names' : active_sides_list, - 'colors' : colors, - 'line_style': line_styles, - 'title' : "TR (°) vs. Sample", - 'ylabel': "N", - 'xlabel': "timestep", - 'line_width': line_widths, - 'yrange': [0,20] - } - - imu_plt_config = {'names' : active_sides_list, - 'colors' : colors, - 'line_style': line_styles, - 'title' : "Activations vs. Sample", - 'ylabel': "Bool", - 'xlabel': "timestep", - 'line_width': line_widths, - 'yrange': [0,50] - } - - plot_config = [current_plt_config, temp_plt_config, in_swing_plt_config, TR_plt_config, imu_plt_config] - - return plot_config - - def update_rt_plots(self, bertec_swing_flag, imu_activations) -> list: - """ - Updates the real-time plots with current values for: - - Current (A) - - Temperature (°C) - - Bertec In swing - - Transmission Ratio - - IMU estimator activations - - The data is collected from the exoboots object and returned as a list of arrays. - This is done for each active actuator only. - - Returns: - plot_data_array: A list of data arrays (for active actuators) for each plot. - """ - - data_to_plt = [] - - for actuator in self.actuators.values(): - data_to_plt.extend([ - abs(actuator.motor_current), # Motor current - actuator.case_temperature, # Case temperature - bertec_swing_flag, - actuator.gear_ratio, # Gear ratio - imu_activations - ]) - - return data_to_plt - - def track_variables_for_logging(self, logger: Logger) -> None: - """ - Track variables for each active actuator for logging to a single file - """ - - for actuator in self.actuators.values(): - dummy_grpc_value = 5.0 - dummy_ankle_torque_setpt = 20 - logger.track_variable(lambda: time.time(), "pitime") - logger.track_variable(lambda: dummy_grpc_value, "dollar_value") - logger.track_variable(lambda: dummy_ankle_torque_setpt, "torque_setpt_Nm") - - logger.track_variable(lambda: actuator.accelx, f"{actuator._tag}_accelx_mps2") - logger.track_variable(lambda: actuator.motor_current, f"{actuator._tag}_current_mA") - logger.track_variable(lambda: actuator.motor_position, f"{actuator._tag}_position_rad") - logger.track_variable(lambda: actuator.motor_encoder_counts, f"{actuator._tag}_encoder_counts") - logger.track_variable(lambda: actuator.case_temperature, f"{actuator._tag}_case_temp_C") - - tracked_vars = logger.get_tracked_variables() - print("Tracked variables:", tracked_vars) - - @property - def left(self) -> DephyEB51Actuator: - try: - return self.actuators["left"] - except KeyError: - CONSOLE_LOGGER.error("Ankle actuator not found. Please check for `left` key in the actuators dictionary.") - exit(1) - - @property - def right(self) -> DephyEB51Actuator: - try: - return self.actuators["right"] - except KeyError: - CONSOLE_LOGGER.error("Ankle actuator not found. Please check for `right` key in the actuators dictionary.") - exit(1) - - - -class ThreadManager: - """ - This class manages thread creation, communication and termination for the exoskeleton system. - """ - - def __init__(self, msg_router:MessageRouter, actuators:Dict) -> None: - - self.actuators = actuators # Dictionary of Actuators - self.msg_router = msg_router # MessageRouter class instance - - # create threading events common to all threads - self._quit_event = threading.Event() # Event to signal threads to quit. - self._pause_event = threading.Event() # Event to signal threads to pause. - self._log_event = threading.Event() # Event to signal threads to log - - # initialize thread events - self._quit_event.set() # exo is running - self._pause_event.clear() # exo starts paused - self._log_event.clear() # exo starts not logging - - # initialize dict of threads - self._threads = {} - - def start(self) -> None: - """ - Creates the following threads: - - for each ACTIVE actuator - - for the Gait State Estimator - - for the GUI - """ - - for actuator in self.actuators.values(): - self.initialize_actuator_thread(actuator) - - # creating 1 thread for the Gait State Estimator - self.initialize_GSE_thread() - - # creating 1 thread for GUI communication - self.initialize_GUI_thread() - - def start_all_threads(self)->None: - """ - Start all threads in the thread manager. - This method is called to start all threads after they have been initialized. - """ - - for thread in self._threads.values(): - thread.start() - LOGGER.debug(f"Thread {thread.name} started.") - - def stop(self) -> None: - """Signal threads to quit and join them""" - - # setting the quit event so all threads recieve the kill signal - self._quit_event.set() - LOGGER.debug("Setting quit event for all threads.") - - def initialize_actuator_thread(self, actuator: DephyEB51Actuator) -> None: - """ - Create and start a thread for the specified actuator. - This method is called to set up the actuator communication thread. - """ - - actuator_thread = ActuatorThread(actuator=actuator, - quit_event=self._quit_event, - pause_event=self._pause_event, - log_event=self._log_event, - name=f"{actuator.side}", - frequency=1, - msg_router=self.msg_router, - ) - - LOGGER.debug(f"created {actuator.side} actuator thread") - self._threads[actuator.side] = actuator_thread - - def initialize_GSE_thread(self) -> None: - """ - Create and start the Gait State Estimator thread. - This method is called to set up the GSE communication thread. - """ - # create a FIFO queue with max size for inter-thread communication - name = "gse" - gse_thread = GaitStateEstimatorThread(quit_event=self._quit_event, - pause_event=self._pause_event, - log_event=self._log_event, - name=name, - frequency=1, - msg_router=self.msg_router) - - LOGGER.debug(f"created gse thread") - self._threads[name] = gse_thread - - def initialize_GUI_thread(self) -> None: - """ - Create and start the GUI thread for user input. - This method is called to set up the GUI communication thread. - """ - - # create a FIFO queue with max size for inter-thread communication - name = "gui" - gui_thread = GUICommunication(quit_event=self._quit_event, - pause_event=self._pause_event, - log_event=self._log_event, - name=name, - frequency=1, - msg_router=self.msg_router) - LOGGER.debug(f"created gui thread") - self._threads[name] = gui_thread - - def return_active_threads(self)->list: - """ - Return list of active thread addresses. - """ - return self._threads.values() - - def __enter__(self) -> None: - """ - Context manager enter method. - This method is called when the ThreadManager is used in a with statement. - """ - self.start() - - def __exit__(self, exc_type, exc_value, traceback) -> None: - """ - Context manager exit method. - This method is called when the ThreadManager is used in a with statement. - It stops all threads and cleans up. - """ - self.stop() - - @property - def left_exo_queue(self) -> Dict[str, queue.Queue]: - """Get the queue for the left actuator""" - return self.queues.get("left") - - @property - def right_exo_queue(self) -> Dict[str, queue.Queue]: - """Get the queue for the right actuator""" - return self.queues.get("right") - - @property - def gse_queue(self) -> Dict[str, queue.Queue]: - """Get the queue for the gait state estimator""" - return self.queues.get("gse") - - @property - def gui_queue(self) -> Dict[str, queue.Queue]: - """Get the queue for the GUI communication""" - return self.queues.get("gui") - - @property - def left_thread(self) -> Optional[ActuatorThread]: - """Get the left actuator thread""" - return self._threads.get("left") - - @property - def right_thread(self) -> Optional[ActuatorThread]: - """Get the right actuator thread""" - return self._threads.get("right") - - @property - def gse_thread(self) -> Optional[ActuatorThread]: - """Get the right actuator thread""" - return self._threads.get("gse") - - @property - def gui_thread(self) -> Optional[ActuatorThread]: - """Get the right actuator thread""" - return self._threads.get("gui") - - -# Example main loop -from opensourceleg.utilities import SoftRealtimeLoop -from src.utils.actuator_utils import create_actuators -from src.settings.constants import EXO_SETUP_CONST -from src.utils.walking_simulator import WalkingSimulator - -if __name__ == '__main__': - - # create actuators - actuators = create_actuators(gear_ratio=1, - baud_rate=EXO_SETUP_CONST.BAUD_RATE, - freq=EXO_SETUP_CONST.FLEXSEA_FREQ, - debug_level=EXO_SETUP_CONST.LOG_LEVEL) - - # create Exoboots Robot - exoboots = DephyExoboots(tag="exoboots", - actuators=actuators, - sensors={}) - - # create a message router for inter-thread communication - msg_router = MessageRouter() - - # instantiate thread manager - system_manager = ThreadManager(msg_router=msg_router, actuators=actuators) - - # instantiate soft real-time clock - clock = SoftRealtimeLoop(dt = 1 / 1) # Hz - - with system_manager: - # set-up addressbook for the PostOffice & create inboxes for each thread - msg_router.setup_addressbook(*system_manager.return_active_threads()) - - # start all threads - system_manager.start_all_threads() - - for t in clock: - try: - - # send mail to the GUI thread - msg_router.send(sender="main", recipient="gui", contents={"torque_setpoint": 30.0}) - print("Main sent") - - # TODO: pass reported values into AssistanceGenerator class -> method is part of Exoboots Robot class - # TODO: determine appropriate torque setpoint given current gait state - - # TODO: send torque setpoints to each corresponding actuator - # TODO: determine appropriate current setpoint that matches the torque setpoint -> handled by DephyEB51Actuator class (within each actuator thread) - # TODO: command appropriate current setpoint using DephyExoboots class - - except KeyboardInterrupt: - print("KeyboardInterrupt received.") - break - - except Exception as err: - print("Unexpected error in executing main controller:", err) - break From 083a9e1234bd832f58f4a86870e5d523ab000d81 Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Mon, 16 Jun 2025 22:59:01 +0100 Subject: [PATCH 09/27] added in exoboot stopping. modified walking simulator to directly send HS_time, etc. bugs need fixing in walking_simulator. --- gse_bertec.py | 2 +- src/exo/assistance_calculator.py | 3 +- src/utils/walking_simulator.py | 14 +++++++ threading_demo.py | 72 ++++++++++++++++++++++++++------ 4 files changed, 77 insertions(+), 14 deletions(-) diff --git a/gse_bertec.py b/gse_bertec.py index 6b517fe..3e1d155 100644 --- a/gse_bertec.py +++ b/gse_bertec.py @@ -30,7 +30,7 @@ def return_estimate(self): c) in swing """ - state_dict = {"HS": self.HS, + state_dict = {"HS_time": self.HS, "stride_period": self.stride_period_tracker.average(), "in_swing": not self.in_contact } diff --git a/src/exo/assistance_calculator.py b/src/exo/assistance_calculator.py index 57ffa0e..34a397b 100644 --- a/src/exo/assistance_calculator.py +++ b/src/exo/assistance_calculator.py @@ -241,7 +241,8 @@ def torque_generator(self, current_time:float, stride_period:float, peak_torque: # If torque_command is negative, raise ValueError and set to holding torque if torque_command < 0: - raise ValueError(f"Negative torque command generated: {torque_command}. Setting to holding torque.") + # TODO: comment back in: + # raise ValueError(f"Negative torque command generated: {torque_command}. Setting to holding torque.") torque_command = self.holding_torque return torque_command diff --git a/src/utils/walking_simulator.py b/src/utils/walking_simulator.py index f49c867..17a08f2 100644 --- a/src/utils/walking_simulator.py +++ b/src/utils/walking_simulator.py @@ -110,3 +110,17 @@ def in_swing_flag(self): else: self.in_swing_flag = False + def return_estimate(self): + """ + Return a dictionary of the most recent state of estimator {a, b, c} + a) most recent heel strike time + b) average stride period + c) in swing + + """ + state_dict = {"HS_time": self.start_time, + "stride_period": self.stride_period, + "in_swing": self.in_swing_flag + } + + return state_dict \ No newline at end of file diff --git a/threading_demo.py b/threading_demo.py index b7d6e4b..69e42ed 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -10,7 +10,7 @@ # logging setup from src.utils.filing_utils import get_logging_info -from opensourceleg.logging import Logger, LogLevel, LOGGER +from opensourceleg.logging import Logger, LogLevel class BaseWorkerThread(threading.Thread, ABC): @@ -56,9 +56,10 @@ def run(self)->None: while self.quit_event.is_set(): # while not quitting self.pre_iterate() # run a pre-iterate method - if self.pause_event.is_set(): + if not self.pause_event.is_set(): self.on_pause() # if paused, run once else: + try: self.iterate() # call the iterate method to perform the thread's task except Exception as e: @@ -108,6 +109,8 @@ def on_pause(self): pass +from src.exo.assistance_calculator import AssistanceCalculator + class ActuatorThread(BaseWorkerThread): """ Threading class for EACH actuator. @@ -126,11 +129,20 @@ def __init__(self, super().__init__(quit_event, pause_event, log_event, name, frequency=frequency) self.actuator = actuator - self.actuator.start() # start the actuator + self.actuator.start() # start actuator self.msg_router = msg_router self.inbox = None + # instantiate assistance generator + self.assistance_calculator = AssistanceCalculator() + + # set-up vars: + self.HS_time:float = 0.0 + self.stride_period:float = 1.2 + self.in_swing:bool = True + self.torque_setpoint:float = 0.0 + def pre_iterate(self)->None: """ Check inbox for messages from GSE & GUI threads @@ -161,15 +173,39 @@ def iterate(self)->None: """ self.actuator.update() - LOGGER.debug(f"motor position: {self.actuator.motor_position:.2f}") - # TODO: use exoboots context manager to potentially execute code - # TODO: pass reported values into AssistanceGenerator class -> method is part of Exoboots Robot class - # TODO: determine appropriate torque setpoint given current gait state + # obtain time in current stride + self.time_in_stride = time.perf_counter() - self.HS_time + + # acquire torque command based on gait estimate + LOGGER.debug("made it to A") + LOGGER.debug(f"all data: {self.time_in_stride},{self.stride_period},{float(self.torque_setpoint)},{self.in_swing}") + + torque_command = self.assistance_calculator.torque_generator(self.time_in_stride, + self.stride_period, + float(self.torque_setpoint), + self.in_swing) + + LOGGER.debug("made it to B") + + LOGGER.debug(f"torque command: {torque_command:.2f}") + LOGGER.debug(f" time in stride: {self.time_in_stride:.2f}") + LOGGER.debug(f" stride period: {self.stride_period:.2f}") + LOGGER.debug(f" peak torque setpoint: {self.torque_setpoint:.2f}") + LOGGER.debug(f" in swing flag: {self.in_swing:.2f}") - # TODO: send torque setpoints to each corresponding actuator - # TODO: determine appropriate current setpoint that matches the torque setpoint -> handled by DephyEB51Actuator class (within each actuator thread) - # TODO: command appropriate current setpoint using DephyExoboots class + # determine appropriate current setpoint that matches the torque setpoint + current_setpoint = self.actuator.torque_to_current(torque_command) + LOGGER.debug("made it to C") + + LOGGER.debug(f"current command: {current_setpoint:.2f}") + + # command appropriate current setpoint using DephyExoboots class + if current_setpoint is not None: + # self.actuator.set_motor_current(current_setpoint) + LOGGER.info(f"Finished setting current setpoint for {self.actuator.tag}") + else: + LOGGER.warning(f"Unable to command current for {self.actuator.tag}. Skipping.") def post_iterate(self)->None: # TODO add rest of stuff @@ -235,10 +271,16 @@ def iterate(self): # TODO: update the gait state estimator for the actuator # self.bertec_estimators[actuator].update() + self.bertec_estimators[actuator].update_time_in_stride() + # self.bertec_estimators[actuator].update_ank_angle() # send message to actuator inboxes + try: - msg_router.send(sender=self.name, recipient=actuator, contents={"time_in_stride": self.bertec_estimators[actuator].update_time_in_stride()}) + print(self.bertec_estimators[actuator].return_estimate()) + msg_router.send(sender=self.name, + recipient=actuator, + contents=self.bertec_estimators[actuator].return_estimate()) except: LOGGER.debug(f"UNABLE TO SEND msg to '{actuator}' actuator from GaitStateEstimatorThread. Skipping.") continue @@ -250,6 +292,7 @@ def on_pause(self)->None: pass + import sys import select import random @@ -671,7 +714,12 @@ def stop(self) -> None: self._quit_event.set() LOGGER.debug("Setting quit event for all threads.") - # TODO: ensure that both actuators are properly shut down + # ensure that both actuators are properly shut down + for actuator in self.actuators.values(): + LOGGER.debug(f"Calling stop method of {actuator.tag}") + actuator.stop() + + time.sleep(0.25) def initialize_actuator_thread(self, actuator:DephyEB51Actuator) -> None: """ From 56f351a00a368e95ae563d5031ef61442f10d177 Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Tue, 17 Jun 2025 18:31:56 +0100 Subject: [PATCH 10/27] modified non_singleton_logger to handle csv logging. tested csv logging with threading_demo. modified walking_sim to update according to internal frequency. needs verification in threading_demo --- non_singleton_logger.py | 117 ++++++++++++++++---- src/utils/non_singleton_logger.py | 0 src/utils/walking_simulator.py | 170 ++++++++++++++++++++++++------ threading_demo.py | 70 ++++++------ 4 files changed, 272 insertions(+), 85 deletions(-) delete mode 100644 src/utils/non_singleton_logger.py diff --git a/non_singleton_logger.py b/non_singleton_logger.py index 8ce8f9e..52b7e8c 100644 --- a/non_singleton_logger.py +++ b/non_singleton_logger.py @@ -2,11 +2,15 @@ import os from logging.handlers import RotatingFileHandler from datetime import datetime -from typing import Optional, Union +from typing import Optional, Any, Callable +import csv +import threading +from collections import deque +from datetime import datetime class NonSingletonLogger: """ - A logger class that does NOT use the singleton pattern. + A logger class that does NOT use the singleton pattern. Each instance is independent and can be used for per-thread logging. This logger can produce the following types of logs: - Debug @@ -14,9 +18,9 @@ class NonSingletonLogger: - Warning - Error - Critical - + The logs are saved to a file and can also be printed to the console. - + It takes in the following arguments: - log_path: Directory where the log file will be saved. - log_format: Format of the log messages. @@ -26,6 +30,7 @@ class NonSingletonLogger: - file_backup_count: Number of backup files to keep. - file_name: Name of the log file. If not provided, a timestamped name will be used. """ + def __init__( self, log_path: str = "./", @@ -35,10 +40,12 @@ def __init__( file_max_bytes: int = 0, file_backup_count: int = 5, file_name: Optional[str] = None, + buffer_size: int = 1000, + enable_csv_logging: bool = True ): # Ensure log directory exists os.makedirs(log_path, exist_ok=True) - + # if no filename is provided, create one using the current timestamp if file_name is None: now = datetime.now() @@ -46,23 +53,23 @@ def __init__( file_name = f"log_{timestamp}" elif "." in file_name: # otherwise, if the filename contains a dot (e.g. ".log"), then remove it file_name = file_name.split(".")[0] - + # assemble a filepath using the filename and log path self._file_path = os.path.join(log_path, f"{file_name}.log") - + # create a unique logger instance using the id of the current instance self._logger = logging.getLogger(f"NonSingletonLogger_{id(self)}") - + # set the logging level & prevent logs from being recorded by root logger self._logger.setLevel(file_level) - self._logger.propagate = False + self._logger.propagate = False # remove any existing handlers (to prevent duplicate logs) if self._logger.hasHandlers(): self._logger.handlers.clear() - + formatter = logging.Formatter(log_format) - + # set-up a file handler that writes to the log file # the handler rotates the file if it gets too big file_handler = RotatingFileHandler( @@ -75,13 +82,70 @@ def __init__( file_handler.setLevel(file_level) file_handler.setFormatter(formatter) self._logger.addHandler(file_handler) # add the file handler to the logger - + # stream handler prints logs to the console stream_handler = logging.StreamHandler() stream_handler.setLevel(stream_level) stream_handler.setFormatter(formatter) self._logger.addHandler(stream_handler) # add the stream handler to the logger + self._tracked_vars = {} + self._var_names = {} + self._buffer = deque(maxlen=buffer_size) + self._buffer_size = buffer_size + self._header_written = False + self._enable_csv_logging = enable_csv_logging + self._csv_path = os.path.join(log_path, f"{file_name}.csv") + self._file = None + self._writer = None + self._lock = threading.RLock() + + def track_variable(self, var_func: Callable[[], Any], name: str) -> None: + self._tracked_vars[id(var_func)] = var_func + self._var_names[id(var_func)] = name + self.debug(f"Started tracking variable: {name}") + + def update(self) -> None: + if not self._tracked_vars or not self._enable_csv_logging: + return + with self._lock: + data = [] + for var_id, get_value in self._tracked_vars.items(): + try: + value = get_value() + except Exception as e: + value = "ERROR" + data.append(str(value)) + if data: + self._buffer.append(data) + if len(self._buffer) >= self._buffer_size: + self.flush_buffer() + + def flush_buffer(self) -> None: + if not self._buffer or not self._enable_csv_logging: + return + with self._lock: + if self._file is None: + self._file = open(self._csv_path, "a", encoding="utf-8", newline="") + self._writer = csv.writer(self._file) + if not self._header_written: + self._write_header() + self._writer.writerows(self._buffer) + self._buffer.clear() + self._file.flush() + + def _write_header(self) -> None: + header = list(self._var_names.values()) + if header: + self._writer.writerow(header) + self._header_written = True + + def close(self): + self.flush_buffer() + if self._file: + self._file.close() + self._file = None + # logging methods at different levels: def debug(self, msg, *args, **kwargs): self._logger.debug(msg, *args, **kwargs) @@ -102,17 +166,34 @@ def critical(self, msg, *args, **kwargs): def file_path(self): return self._file_path +class test(): + def __init__(self): + self.x = 25 if __name__ == "__main__": - logger = NonSingletonLogger(log_path="./src/logs/", - file_name="test_logger", - file_level=logging.DEBUG, - stream_level=logging.INFO) - for i in range(3): + + logger = NonSingletonLogger(log_path="./src/logs/", + file_name="test_logger", + file_level=logging.DEBUG, + stream_level=logging.INFO, + buffer_size=100) + + # instantiate test class + obj = test() + logger.track_variable(lambda: obj.x, "test") + + for i in range(10): + logger.update() + logger.debug(f"Debug message {i}") logger.info(f"Info message {i}") logger.warning(f"Warning message {i}") logger.error(f"Error message {i}") logger.critical(f"Critical message {i}") - + print(f"Log file created at: {logger.file_path}") + logger.flush_buffer() + logger.close() + + + diff --git a/src/utils/non_singleton_logger.py b/src/utils/non_singleton_logger.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/utils/walking_simulator.py b/src/utils/walking_simulator.py index 17a08f2..0f8ec84 100644 --- a/src/utils/walking_simulator.py +++ b/src/utils/walking_simulator.py @@ -2,7 +2,9 @@ import sys import csv import numpy as np + from opensourceleg.utilities import SoftRealtimeLoop +from opensourceleg.logging import Logger, LogLevel """ How it works: @@ -16,23 +18,36 @@ class WalkingSimulator(): """ - Simulate a person walking with a fixed stride period. - Increments current_time_in_stride at each update, until it resets at the stride period. + Simulate a person walking with a variable stride period. + Increments current_time_in_stride at each update, until it resets at the set stride period. Output looks like a sawtooth wave. Ankle angle trajectory is from Reznick et al. 2021 (10° uphill walking) + + Walker data is updated every milisecond. + + Args: + stride_period (float): average stride time (in seconds) """ - def __init__(self, stride_period:float=1.20): + + def __init__(self, stride_period:float = 1.20): + + # simulation updates at fixed 1000 Hz frequency + # i.e. it will increment walker state every millisecond + self.sim_dt = 1.0 / 1000 + self._last_update_time = time.perf_counter() + + # start at heel strike (0% gait cycle and in stance) + self.seed_stride_period = stride_period self.current_time_in_stride:float = 0 self.stride_num:int = 0 - self.stride_period:float = stride_period - self.start_time:float = time.time() + self.stride_start_time:float = time.perf_counter() - self.percent_GC:float = 0 + self.current_percent_gait_cycle:float = 0.0 self.in_swing_flag:bool = False # ankle angle trajectory from Reznick et al. 2021 (10° uphill walking) - self.gc = [] - self.ank_ang = [] + self._gc_traj = [] + self._ank_ang_traj = [] try: with open('./src/utils/ankle_angle_trajectory.csv', 'r') as f: @@ -43,37 +58,75 @@ def __init__(self, stride_period:float=1.20): if row[0] == 1: continue else: - self.gc.append(float(row[0])) - self.ank_ang.append(float(row[1])) + self._gc_traj.append(float(row[0])) + self._ank_ang_traj.append(float(row[1])) except Exception as err: print(f"Error when loading & parsing 'ankle_angle_trajectory.csv': {err}") print(err) sys.exit(1) + def update(self) -> None: + """ + This method updates the simulator state only if enough time + has passed since the last update. If enough time hasn't passed, + then it won't update the estimate. + + Main loop can update as often as it likes, but simulator will + ONLY update at user-specified frequency specified at the init. + + """ + + now = time.perf_counter() + if now - self._last_update_time >= self.sim_dt: + self._last_update_time += self.sim_dt + + _ = self.update_time_in_stride() + _ = self.update_ank_angle() + self.toggle_in_swing_flag() + def update_time_in_stride(self)-> float: """ Updates the gait state based on the current time in stride. """ + + # before first stride, create stride_period attribute + if self.stride_num < 1: + self.stride_period = self.seed_stride_period + if self.current_time_in_stride >= self.stride_period: self.current_time_in_stride = 0 - self.start_time = time.time() + self.update_stride_period() # update stride period target + + self.stride_start_time = time.perf_counter() self.stride_num += 1 - # print(f'{self.stride_num} stride(s) completed') + print(f'{self.stride_num} stride(s) completed') + elif self.current_time_in_stride < self.stride_period: - self.current_time_in_stride = time.time() - self.start_time - # print(f"time in curr stride: {self.current_time_in_stride:.3f}") + self.current_time_in_stride = time.perf_counter() - self.stride_start_time + print(f"time in curr stride: {self.current_time_in_stride:.3f}") return self.current_time_in_stride + def update_stride_period(self): + """ + Updates stride period target to mimic human walking variability. + + Randomly samples a stride period from a normal distribution + centered at 1.2s, with a standard deviation of 0.4s. + """ + + self.stride_period = np.random.normal(loc=self.seed_stride_period, scale=0.2) + def update_ank_angle(self)-> float: """ Updates the ankle angle based on the current time in stride. """ - self.percent_gc = self.time_in_stride_to_percent_GC(self.current_time_in_stride) + + self.current_percent_gait_cycle = self.time_in_stride_to_percent_GC(self.current_time_in_stride) # interpolate angle at the current % gait cycle - interp_ang_at_query_pt = np.interp(self.percent_gc, self.gc, self.ank_ang) + interp_ang_at_query_pt = np.interp(self.current_percent_gait_cycle, self._gc_traj, self._ank_ang_traj) return interp_ang_at_query_pt @@ -81,46 +134,93 @@ def time_in_stride_to_percent_GC(self, time_in_stride:float)-> float: """ Converts the time in stride to a percentage of the gait cycle. """ + if time_in_stride < 0: raise ValueError("Time in stride cannot be negative.") elif time_in_stride > self.stride_period: - self.percent_GC = 100 + self.current_percent_gait_cycle = 100 else: - self.percent_GC = time_in_stride / self.stride_period * 100 + self.current_percent_gait_cycle = time_in_stride / self.stride_period * 100 - print(f"percent GC: {self.percent_GC:.2f}") + print(f"percent GC: {self.current_percent_gait_cycle:.2f}") - return self.percent_GC + return self.current_percent_gait_cycle def set_percent_toe_off(self, percent_toe_off:float=67)->None: """ Can use to set a percent toe-off-time if looking to get swing/stance flag output """ + self.percent_toe_off = percent_toe_off - def in_swing_flag(self): + def toggle_in_swing_flag(self): """ False if person is NOT in swing (i.e. stance), and True if person is in swing. Decides based on user-specified toe-off time. """ - if self.percent_GC >= self.percent_toe_off: + + if self.current_percent_gait_cycle >= self.percent_toe_off: self.in_swing_flag = True else: self.in_swing_flag = False def return_estimate(self): - """ - Return a dictionary of the most recent state of estimator {a, b, c} - a) most recent heel strike time - b) average stride period - c) in swing - - """ - state_dict = {"HS_time": self.start_time, - "stride_period": self.stride_period, - "in_swing": self.in_swing_flag - } - - return state_dict \ No newline at end of file + """ + Return a dictionary of the most recent state of estimator {a, b, c} + a) most recent heel strike time + b) average stride period + c) in swing flag + + """ + + state_dict = {"HS_time": self.stride_start_time, + "stride_period": self.stride_period, + "in_swing": self.in_swing_flag + } + + return state_dict + + +if __name__ == '__main__': + + walker = WalkingSimulator(stride_period=1.20) + walker.set_percent_toe_off(67) + + freq = 100 # Hz + clock = SoftRealtimeLoop(dt=1/freq) + + # create a logger + logger = Logger(log_path="walking_sim_test/", + file_name="test", + buffer_size=1000, + file_level=LogLevel.DEBUG, + stream_level=LogLevel.INFO + ) + + # track time, percent gait cycle and torque_command to a csv file + logger.track_variable(lambda: walker.current_time_in_stride, "time_in_stride_s") + logger.track_variable(lambda: walker.current_percent_gait_cycle, "percent_gait_cycle") + logger.track_variable(lambda: walker.in_swing_flag, "in_swing_flag_bool") + + for t in clock: + try: + # update walking sim + walker.update() + + estimate = walker.return_estimate() + print(estimate) + + # update logger + logger.update() + + except KeyboardInterrupt: + logger.flush_buffer() + logger.close() + break + + except Exception as err: + logger.flush_buffer() + logger.close() + break \ No newline at end of file diff --git a/threading_demo.py b/threading_demo.py index 69e42ed..e61eca8 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -11,6 +11,7 @@ # logging setup from src.utils.filing_utils import get_logging_info from opensourceleg.logging import Logger, LogLevel +import logging class BaseWorkerThread(threading.Thread, ABC): @@ -38,14 +39,15 @@ def __init__(self, self.rt_loop = FlexibleSleeper(dt=1/frequency) # create a soft real-time loop with the specified frequency # set-up a logger for each thread/instance - # logger_name = f"{name}_logger" - # log_path = "./src/logs/" - # self.data_logger = NonSingletonLogger( - # log_path=log_path, - # file_name=logger_name, - # file_level=logging.DEBUG, - # stream_level=logging.INFO - # ) + logger_name = f"{name}_logger" + log_path = "./src/logs/" + self.data_logger = NonSingletonLogger( + log_path=log_path, + file_name=logger_name, + file_level=logging.DEBUG, + stream_level=logging.INFO, + buffer_size=100 + ) def run(self)->None: """ @@ -143,6 +145,12 @@ def __init__(self, self.in_swing:bool = True self.torque_setpoint:float = 0.0 + self.data_logger.track_variable(lambda: self.HS_time, "HS_time") + self.data_logger.track_variable(lambda: self.stride_period, "stride_period") + self.data_logger.track_variable(lambda: self.in_swing, "in_swing_flag_bool") + self.data_logger.track_variable(lambda: self.torque_setpoint, "torque_setpt") + + def pre_iterate(self)->None: """ Check inbox for messages from GSE & GUI threads @@ -163,7 +171,7 @@ def decode_message(self, mail): setattr(self, key, value) except Exception as err: - LOGGER.debug(f"Error decoding message: {err}") + self.data_logger.debug(f"Error decoding message: {err}") def iterate(self)->None: """ @@ -173,44 +181,39 @@ def iterate(self)->None: """ self.actuator.update() + self.data_logger.update() # obtain time in current stride self.time_in_stride = time.perf_counter() - self.HS_time # acquire torque command based on gait estimate - LOGGER.debug("made it to A") - LOGGER.debug(f"all data: {self.time_in_stride},{self.stride_period},{float(self.torque_setpoint)},{self.in_swing}") - torque_command = self.assistance_calculator.torque_generator(self.time_in_stride, self.stride_period, float(self.torque_setpoint), self.in_swing) - LOGGER.debug("made it to B") - - LOGGER.debug(f"torque command: {torque_command:.2f}") - LOGGER.debug(f" time in stride: {self.time_in_stride:.2f}") - LOGGER.debug(f" stride period: {self.stride_period:.2f}") - LOGGER.debug(f" peak torque setpoint: {self.torque_setpoint:.2f}") - LOGGER.debug(f" in swing flag: {self.in_swing:.2f}") + self.data_logger.debug(f"torque command: {torque_command:.2f}") + self.data_logger.debug(f" time in stride: {self.time_in_stride:.2f}") + self.data_logger.debug(f" stride period: {self.stride_period:.2f}") + self.data_logger.debug(f" peak torque setpoint: {self.torque_setpoint:.2f}") + self.data_logger.debug(f" in swing flag: {self.in_swing:.2f}") # determine appropriate current setpoint that matches the torque setpoint current_setpoint = self.actuator.torque_to_current(torque_command) - LOGGER.debug("made it to C") - LOGGER.debug(f"current command: {current_setpoint:.2f}") + self.data_logger.debug(f"current command: {current_setpoint:.2f}") # command appropriate current setpoint using DephyExoboots class if current_setpoint is not None: # self.actuator.set_motor_current(current_setpoint) - LOGGER.info(f"Finished setting current setpoint for {self.actuator.tag}") + self.data_logger.info(f"Finished setting current setpoint for {self.actuator.tag}") else: - LOGGER.warning(f"Unable to command current for {self.actuator.tag}. Skipping.") + self.data_logger.warning(f"Unable to command current for {self.actuator.tag}. Skipping.") def post_iterate(self)->None: # TODO add rest of stuff if self.log_event.is_set(): - LOGGER.debug(f"[{self.name}] log_event True") + self.data_logger.debug(f"[{self.name}] log_event True") def on_pause(self)->None: pass @@ -249,7 +252,11 @@ def __init__(self, selected_topic = f"fz_{actuator}" # e.g., 'fz_left' or 'fz_right' # TODO: REMOVE -- FOR TESTING ONLY - self.bertec_estimators[actuator] = WalkingSimulator(stride_period=1.20) + walker = WalkingSimulator(stride_period=1.20) + walker.set_percent_toe_off(67) + + self.bertec_estimators[actuator] = walker + # TODO: UNCOMMENT # bertec_subscriber = Subscriber(publisher_ip=IP_ADDRESSES.VICON_IP, topic_filter=selected_topic, timeout_ms=5) @@ -271,18 +278,17 @@ def iterate(self): # TODO: update the gait state estimator for the actuator # self.bertec_estimators[actuator].update() - self.bertec_estimators[actuator].update_time_in_stride() - # self.bertec_estimators[actuator].update_ank_angle() + self.bertec_estimators[actuator].update() # send message to actuator inboxes - try: - print(self.bertec_estimators[actuator].return_estimate()) + self.data_logger.debug(self.bertec_estimators[actuator].return_estimate()) + msg_router.send(sender=self.name, recipient=actuator, contents=self.bertec_estimators[actuator].return_estimate()) except: - LOGGER.debug(f"UNABLE TO SEND msg to '{actuator}' actuator from GaitStateEstimatorThread. Skipping.") + self.data_logger.debug(f"UNABLE TO SEND msg to '{actuator}' actuator from GaitStateEstimatorThread. Skipping.") continue def post_iterate(self)->None: @@ -347,9 +353,9 @@ def iterate(self): recipient="right", contents={"torque_setpoint": self.torque_setpoint}) - LOGGER.debug(f"sent torque setpoint {self.torque_setpoint} to actuators") + self.data_logger.debug(f"sent torque setpoint {self.torque_setpoint} to actuators") except: - LOGGER.debug(f"UNABLE TO SEND msg to actuator from GUICommunication thread. Skipping.") + self.data_logger.debug(f"UNABLE TO SEND msg to actuator from GUICommunication thread. Skipping.") def post_iterate(self)->None: pass From fed38f1a6c2a2cf77a3614cba1c69c05d3fa6b18 Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Tue, 17 Jun 2025 20:21:43 +0100 Subject: [PATCH 11/27] fixed exoboots.py and JIM_testing script merge errors. --- exoboots.py | 70 +++++++++++-------- .../JIM_testing_current_cmd.py | 20 +++--- 2 files changed, 53 insertions(+), 37 deletions(-) diff --git a/exoboots.py b/exoboots.py index d2b5651..262c5c1 100644 --- a/exoboots.py +++ b/exoboots.py @@ -9,7 +9,7 @@ # TODO: fix these next 3 imports: from src.utils.filing_utils import get_logging_info -from opensourceleg.logging import Logger, LogLevel +from opensourceleg.logging import Logger, LogLevel, LOGGER CONSOLE_LOGGER = Logger(enable_csv_logging=False, log_path=get_logging_info(user_input_flag=False)[0], stream_level = LogLevel.INFO, @@ -23,7 +23,15 @@ ) from dephyEB51 import DephyEB51Actuator + class DephyExoboots(RobotBase[DephyEB51Actuator, SensorBase]): + """ + Bilateral Dephy EB51 Exoskeleton class derived from RobotBase. + + This class creates a DephyExoboots Robot, using the structure + provided by the RobotBase class. A robot is composed of a collection + of actuators and sensors. + """ def start(self) -> None: """ @@ -38,22 +46,6 @@ def stop(self) -> None: super().stop() - def set_actuator_mode_and_gains(self, actuator)-> None: - """ - Call the setup_controller method for all actuators. - This method selects current control mode and sets PID gains for each actuator. - """ - actuator.set_control_mode(CONTROL_MODES.CURRENT) - LOGGER.info("finished setting control mode") - - actuator.set_current_gains( - kp=DEFAULT_CURRENT_GAINS.kp, - ki=DEFAULT_CURRENT_GAINS.ki, - kd=DEFAULT_CURRENT_GAINS.kd, - ff=DEFAULT_CURRENT_GAINS.ff, - ) - LOGGER.info("finished setting gains") - def update(self) -> None: """ Update the exoskeleton. @@ -61,7 +53,25 @@ def update(self) -> None: # print(f"Updating exoskeleton robot: {self.tag}") super().update() - def spool_belts(self): + def setup_control_modes(self) -> None: + """ + Call the setup_controller method for all actuators. + This method selects current control mode and sets PID gains for each actuator. + """ + + for actuator in self.actuators.values(): + actuator.set_control_mode(CONTROL_MODES.CURRENT) + CONSOLE_LOGGER.info("finished setting control mode") + + actuator.set_current_gains( + kp=DEFAULT_PID_GAINS.KP, + ki=DEFAULT_PID_GAINS.KI, + kd=DEFAULT_PID_GAINS.KD, + ff=DEFAULT_PID_GAINS.FF, + ) + CONSOLE_LOGGER.info("finished setting gains") + + def spool_belts(self) -> None: """ Spool the belts of both actuators. This method is called to prepare the actuators for operation. @@ -77,7 +87,7 @@ def set_to_transparent_mode(self): self.update_current_setpoints(current_inputs=0, asymmetric=False) self.command_currents() - def detect_active_actuators(self) -> Union[str, list[str]]: + def detect_active_actuators(self) -> Union[start, list[str]]: """ Detect active actuators. Returns a string if only one actuator is active, otherwise a list of strings. @@ -125,7 +135,7 @@ def update_current_setpoints(self, current_inputs: Union[int, Dict[str, int]], a actuator = getattr(self, side) self.current_setpoints[side] = int(current_inputs) * actuator.motor_sign - def convert_torque_to_current_setpoints(self, torque_setpoint: float) -> dict: + def convert_torque_to_current_setpoints(self, torque_setpoint: float) -> None: """ Find the appropriate current setpoint for the actuators. This method is called to determine the current setpoint based on the torque setpoint. @@ -133,31 +143,32 @@ def convert_torque_to_current_setpoints(self, torque_setpoint: float) -> dict: arguments: torque_setpoint: float, the desired torque setpoint in Nm. - returns: - current_setpoints: dict of currents for each active actuator. + currents: dict of currents for each active actuator. key is the side of the actuator (left or right). """ for actuator in self.actuators.values(): self.current_setpoints[actuator.side] = actuator.torque_to_current(torque_setpoint) - CONSOLE_LOGGER.info(f"finished finding current setpoint for {actuator.side}") - - return self.current_setpoints + # CONSOLE_LOGGER.info(f"finished finding current setpoint for {actuator.side}") def command_currents(self) -> None: """ - Commands current setpoints to each actuator based on the current_setpoints dictionary. + Commands current setpoints to each actuator. The setpoints can be unique. + + arguments: + current_setpoints: dict of currents for each active actuator. + key is the side of the actuator (left or right). """ + # TODO: ensure current_setpoints values are integers, no greater than max current limit, and are not None for actuator in self.actuators.values(): - current_setpoint = self.current_setpoints.get(actuator.side) if current_setpoint is not None: actuator.set_motor_current(current_setpoint) - CONSOLE_LOGGER.info(f"Finished setting current setpoint for {actuator.side}") + # CONSOLE_LOGGER.info(f"Finished setting current setpoint for {actuator.side}") else: CONSOLE_LOGGER.warning(f"Unknown side '{actuator.side}' and unable to command current. Skipping.") @@ -279,7 +290,7 @@ def track_variables_for_logging(self, logger: Logger) -> None: for actuator in self.actuators.values(): dummy_grpc_value = 5.0 dummy_ankle_torque_setpt = 20 - logger.track_variable(lambda: time.time(), "pitime") + logger.track_variable(lambda: time.time(), f"pitime") logger.track_variable(lambda: dummy_grpc_value, "dollar_value") logger.track_variable(lambda: dummy_ankle_torque_setpt, "torque_setpt_Nm") @@ -311,6 +322,7 @@ def right(self) -> DephyEB51Actuator: # DEMO: if __name__ == "__main__": + # define dictionary of actuators & sensors actuators = create_actuators(1, EXO_SETUP_CONST.BAUD_RATE, EXO_SETUP_CONST.FLEXSEA_FREQ, EXO_SETUP_CONST.LOG_LEVEL) sensors = {} diff --git a/src/characterization/JIM_characterization/JIM_testing_current_cmd.py b/src/characterization/JIM_characterization/JIM_testing_current_cmd.py index 904b8bf..d00dba5 100644 --- a/src/characterization/JIM_characterization/JIM_testing_current_cmd.py +++ b/src/characterization/JIM_characterization/JIM_testing_current_cmd.py @@ -10,10 +10,8 @@ sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '../../..'))) from src.settings.constants import (TR_DATE_FORMATTER, - BAUD_RATE, - FLEXSEA_FREQ, - LOG_LEVEL, - RTPLOT_IP) + EXO_SETUP_CONST, + IP_ADDRESSES) from src.custom_logging.LoggingClass import FilingCabinet, LoggingNexus from exoboots import DephyExoboots from src.utils.actuator_utils import create_actuators @@ -28,13 +26,14 @@ def __init__(self): self.time = input("time in sec: ") self.rom = input("rom: ") self.dps = input("speed in °/s: " ) + self.alignment_event_time = input("alignment time in sec: " ) if __name__ == "__main__": # get user inputs args = userInputTest() # detect active serial ports & create actuator objects - actuators = create_actuators(1, BAUD_RATE, FLEXSEA_FREQ, LOG_LEVEL) + actuators = create_actuators(1, EXO_SETUP_CONST.BAUD_RATE, EXO_SETUP_CONST.FLEXSEA_FREQ, EXO_SETUP_CONST.LOG_LEVEL) # assign actuators to Exoboots Robot exoboots = DephyExoboots( @@ -57,7 +56,7 @@ def __init__(self): log_path = os.path.abspath(filingcabinet.getpfolderpath()) logger = Logger(log_path=log_path, file_name=fname, - buffer_size=10*FLEXSEA_FREQ, + buffer_size=10*EXO_SETUP_CONST.FLEXSEA_FREQ, file_level = LogLevel.DEBUG, stream_level = LogLevel.INFO, enable_csv_logging = True @@ -65,7 +64,7 @@ def __init__(self): # set-up real-time plots: JIM_data_plotter = JIM_data_plotter(exoboots.actuators) - client.configure_ip(RTPLOT_IP) + client.configure_ip(IP_ADDRESSES.RTPLOT_IP) plot_config = JIM_data_plotter.initialize_JIM_rt_plots() client.initialize_plots(plot_config) @@ -84,6 +83,10 @@ def __init__(self): # setup dict of current setpoints depending on active actuators exoboots.create_current_setpts_dict() + # pause exos for buffer time + time.sleep(float(args.alignment_event_time)) + print(f"{args.alignment_event_time} SEC SLEEP COMPLETED.") + for t in clock: try: # update robot sensor states @@ -96,7 +99,8 @@ def __init__(self): exoboots.command_currents() elif (t > ramp_period) and (t <= float(args.time)): - print("RAMP COMPLETED. Current setpoint is now {} mA".format(args.current_setpt_mA)) + + print(f"RAMP COMPLETED. Current setpoint is now {args.current_setpt_mA} mA") # Command the exo to peak set point current & hold for specified duration exoboots.update_current_setpoints(current_inputs=int(args.current_setpt_mA), asymmetric=False) exoboots.command_currents() From d114c88816f3738e44980d458209c6fc725d1a2e Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Tue, 17 Jun 2025 22:12:46 +0100 Subject: [PATCH 12/27] updated JIM testing script to include buffer time for JIM alignment movement --- .../JIM_testing_current_cmd.py | 11 ++--- .../JIM_characterization/JIM_utils.py | 42 ++++++++++++++++++- 2 files changed, 46 insertions(+), 7 deletions(-) diff --git a/src/characterization/JIM_characterization/JIM_testing_current_cmd.py b/src/characterization/JIM_characterization/JIM_testing_current_cmd.py index d00dba5..dec5b96 100644 --- a/src/characterization/JIM_characterization/JIM_testing_current_cmd.py +++ b/src/characterization/JIM_characterization/JIM_testing_current_cmd.py @@ -83,16 +83,17 @@ def __init__(self): # setup dict of current setpoints depending on active actuators exoboots.create_current_setpts_dict() - # pause exos for buffer time - time.sleep(float(args.alignment_event_time)) - print(f"{args.alignment_event_time} SEC SLEEP COMPLETED.") - for t in clock: try: # update robot sensor states exoboots.update() - if (t <= ramp_period): + if (t > 0.0) and (t <= float(args.alignment_event_time)): + exoboots.set_to_transparent_mode() + print(f"in transparent mode") + + elif (t > float(args.alignment_event_time)) and (t <= ramp_period): + print(f"{args.alignment_event_time} SEC SLEEP COMPLETED.") # ramp to torque linearly ramp_current = float(args.current_setpt_mA) * t/ramp_period exoboots.update_current_setpoints(current_inputs=ramp_current, asymmetric=False) diff --git a/src/characterization/JIM_characterization/JIM_utils.py b/src/characterization/JIM_characterization/JIM_utils.py index 2455b5d..9b44a2e 100644 --- a/src/characterization/JIM_characterization/JIM_utils.py +++ b/src/characterization/JIM_characterization/JIM_utils.py @@ -1,5 +1,5 @@ import time -from opensourceleg.logging import Logger +from opensourceleg.logging import Logger, LogLevel class JIM_data_plotter: def __init__(self, actuators: dict) -> None: @@ -119,4 +119,42 @@ def track_variables_for_JIM_logging(self, logger: Logger) -> None: logger.track_variable(lambda: actuator.case_temperature, f"{actuator._tag}_case_temp_C") tracked_vars = logger.get_tracked_variables() - print("Tracked variables:", tracked_vars) \ No newline at end of file + print("Tracked variables:", tracked_vars) + + +def JIM_time_position_vec_generator(start_velocity:float, final_velocity:float, target_position:float, total_time: float)->None: + """ + This method reports a time and position vector for use during JIM testing. + + Args: + - start_velocity: starting speed of JIM in dps + - final_velocity: final speed of the JIM to hit in dps + - target_position: final position of JIM (to accelerate to) in ° + - total_time: total time of JIM test in seconds + + Returns: + - time_vec: vector of times + - position_vec: vector of positions + """ + + + + return time_vec, position_vec + + +if __name__ == "__main__": + + logger = Logger(log_path='JIM_vec_test/', + file_name='JIM_vec_test', + buffer_size=1000, + file_level = LogLevel.DEBUG, + stream_level = LogLevel.INFO, + enable_csv_logging = True + ) + + start_dps = input("start speed in dps: ") + end_dps = input("end speed in dps: ") + pos = input("deg to accelerate to: ") + total_time = input("desired end time for JIM test: ") + + time_vec, position_vec = JIM_time_position_vec_generator(start_dps, end_dps, pos, total_time) From a26fdca74e97bf6d906846496f070cd6de724e24 Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Thu, 19 Jun 2025 20:55:15 +0100 Subject: [PATCH 13/27] fixed threads not quitting upon ctrl+c signal. was incorrectly setting the quit event instead of clearing. verified using threading demo logs that walking sim works as expected! --- .../JIM_characterization/JIM_utils.py | 3 +- threading_demo.py | 51 +++++++++++++------ 2 files changed, 37 insertions(+), 17 deletions(-) diff --git a/src/characterization/JIM_characterization/JIM_utils.py b/src/characterization/JIM_characterization/JIM_utils.py index 9b44a2e..a835767 100644 --- a/src/characterization/JIM_characterization/JIM_utils.py +++ b/src/characterization/JIM_characterization/JIM_utils.py @@ -122,6 +122,7 @@ def track_variables_for_JIM_logging(self, logger: Logger) -> None: print("Tracked variables:", tracked_vars) +# TODO: COMPLETE THIS FUNCTION def JIM_time_position_vec_generator(start_velocity:float, final_velocity:float, target_position:float, total_time: float)->None: """ This method reports a time and position vector for use during JIM testing. @@ -137,8 +138,6 @@ def JIM_time_position_vec_generator(start_velocity:float, final_velocity:float, - position_vec: vector of positions """ - - return time_vec, position_vec diff --git a/threading_demo.py b/threading_demo.py index e61eca8..cf06a32 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -70,6 +70,8 @@ def run(self)->None: self.post_iterate() # run a post-iterate method self.rt_loop.pause() + # close logger instances before exiting threads + self.data_logger.close() LOGGER.debug(f"[{self.name}] Thread exiting.") @abstractmethod @@ -145,12 +147,12 @@ def __init__(self, self.in_swing:bool = True self.torque_setpoint:float = 0.0 + # track vars for csv logging self.data_logger.track_variable(lambda: self.HS_time, "HS_time") self.data_logger.track_variable(lambda: self.stride_period, "stride_period") self.data_logger.track_variable(lambda: self.in_swing, "in_swing_flag_bool") self.data_logger.track_variable(lambda: self.torque_setpoint, "torque_setpt") - def pre_iterate(self)->None: """ Check inbox for messages from GSE & GUI threads @@ -181,7 +183,7 @@ def iterate(self)->None: """ self.actuator.update() - self.data_logger.update() + self.data_logger.update() # update logger # obtain time in current stride self.time_in_stride = time.perf_counter() - self.HS_time @@ -192,21 +194,21 @@ def iterate(self)->None: float(self.torque_setpoint), self.in_swing) - self.data_logger.debug(f"torque command: {torque_command:.2f}") - self.data_logger.debug(f" time in stride: {self.time_in_stride:.2f}") - self.data_logger.debug(f" stride period: {self.stride_period:.2f}") - self.data_logger.debug(f" peak torque setpoint: {self.torque_setpoint:.2f}") - self.data_logger.debug(f" in swing flag: {self.in_swing:.2f}") + # self.data_logger.debug(f"torque command: {torque_command:.2f}") + # self.data_logger.debug(f" time in stride: {self.time_in_stride:.2f}") + # self.data_logger.debug(f" stride period: {self.stride_period:.2f}") + # self.data_logger.debug(f" peak torque setpoint: {self.torque_setpoint:.2f}") + # self.data_logger.debug(f" in swing flag: {self.in_swing:.2f}") # determine appropriate current setpoint that matches the torque setpoint current_setpoint = self.actuator.torque_to_current(torque_command) - self.data_logger.debug(f"current command: {current_setpoint:.2f}") + # self.data_logger.debug(f"current command: {current_setpoint:.2f}") # command appropriate current setpoint using DephyExoboots class if current_setpoint is not None: # self.actuator.set_motor_current(current_setpoint) - self.data_logger.info(f"Finished setting current setpoint for {self.actuator.tag}") + pass else: self.data_logger.warning(f"Unable to command current for {self.actuator.tag}. Skipping.") @@ -247,6 +249,9 @@ def __init__(self, self.active_actuators = active_actuators self.bertec_estimators = {} + self.time_since_start = 0 + self.thread_start_time = time.perf_counter() + # for each active actuator, initialize GSE Bertec for actuator in self.active_actuators: selected_topic = f"fz_{actuator}" # e.g., 'fz_left' or 'fz_right' @@ -257,11 +262,18 @@ def __init__(self, self.bertec_estimators[actuator] = walker - # TODO: UNCOMMENT # bertec_subscriber = Subscriber(publisher_ip=IP_ADDRESSES.VICON_IP, topic_filter=selected_topic, timeout_ms=5) # self.bertec_estimator[actuator] = Bertec_Estimator(zmq_subscriber=bertec_subscriber) + # track vars for csv logging + self.data_logger.track_variable(lambda: self.time_since_start, f"{actuator}_time_since_start") + self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].stride_start_time, f"{actuator}_HS_time_") + self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].stride_period, f"{actuator}_stride_period") + self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].in_swing_flag, f"{actuator}_in_swing_flag_bool") + self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].current_time_in_stride, f"{actuator}_current_time_in_stride") + self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].current_percent_gait_cycle, f"{actuator}_current_percent_gait_cycle") + def pre_iterate(self)->None: pass @@ -272,6 +284,7 @@ def iterate(self): It updates the gait state estimator and sends the current time in stride and stride period to the actuators. """ + self.time_since_start = time.perf_counter() - self.thread_start_time # for each active actuator, for actuator in self.active_actuators: @@ -280,6 +293,9 @@ def iterate(self): # self.bertec_estimators[actuator].update() self.bertec_estimators[actuator].update() + # update csv logger + self.data_logger.update() + # send message to actuator inboxes try: self.data_logger.debug(self.bertec_estimators[actuator].return_estimate()) @@ -327,6 +343,9 @@ def __init__(self, self.inbox = None self.torque_setpoint = 20.0 + # track vars for csv logging + self.data_logger.track_variable(lambda: self.torque_setpoint, "torque_setpt") + def pre_iterate(self)->None: """ Pre-iterate method to check for new messages in the mailbox. @@ -340,6 +359,9 @@ def iterate(self): If the user doesn't input a new value, the current setpoint is used. """ + # update csv logging + self.data_logger.update() + # set a random torque setpoint self.torque_setpoint = random.randint(1,4)*10 @@ -352,8 +374,6 @@ def iterate(self): msg_router.send(sender=self.name, recipient="right", contents={"torque_setpoint": self.torque_setpoint}) - - self.data_logger.debug(f"sent torque setpoint {self.torque_setpoint} to actuators") except: self.data_logger.debug(f"UNABLE TO SEND msg to actuator from GUICommunication thread. Skipping.") @@ -716,8 +736,8 @@ def start_all_threads(self)->None: def stop(self) -> None: """Signal threads to quit and join them""" - # setting the quit event so all threads recieve the kill signal - self._quit_event.set() + # clearing the quit event so all threads recieve the kill signal + self._quit_event.clear() LOGGER.debug("Setting quit event for all threads.") # ensure that both actuators are properly shut down @@ -725,7 +745,8 @@ def stop(self) -> None: LOGGER.debug(f"Calling stop method of {actuator.tag}") actuator.stop() - time.sleep(0.25) + # ensure time to enact stop method before moving on + time.sleep(0.2) def initialize_actuator_thread(self, actuator:DephyEB51Actuator) -> None: """ From c24b94dbb7bd4e88283f4908ca938ba7c16b9172 Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Thu, 19 Jun 2025 21:50:44 +0100 Subject: [PATCH 14/27] added method to actuator thread to prevent peak torque setpoints from being updated mid-stride. also verified that assistance_calculator works with threading demo. commanded torque and current splines look good and hit setpoint --- threading_demo.py | 77 ++++++++++++++++++++++++++++++----------------- 1 file changed, 49 insertions(+), 28 deletions(-) diff --git a/threading_demo.py b/threading_demo.py index cf06a32..cbc5939 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -146,12 +146,20 @@ def __init__(self, self.stride_period:float = 1.2 self.in_swing:bool = True self.torque_setpoint:float = 0.0 + self.torque_command:float = 0.0 + self.current_setpoint:int = 0 + + self.thread_start_time:float = time.perf_counter() + self.time_since_start:float = 0.0 # track vars for csv logging + self.data_logger.track_variable(lambda: self.time_since_start, "time_since_start") self.data_logger.track_variable(lambda: self.HS_time, "HS_time") self.data_logger.track_variable(lambda: self.stride_period, "stride_period") self.data_logger.track_variable(lambda: self.in_swing, "in_swing_flag_bool") - self.data_logger.track_variable(lambda: self.torque_setpoint, "torque_setpt") + self.data_logger.track_variable(lambda: self.torque_setpoint, "peak_torque_setpt") + self.data_logger.track_variable(lambda: self.torque_command, "torque_cmd") + self.data_logger.track_variable(lambda: self.current_setpoint, "current_setpoint") def pre_iterate(self)->None: """ @@ -170,11 +178,27 @@ def decode_message(self, mail): try: for key, value in mail.contents.items(): # TODO: value validation (not None or something) - setattr(self, key, value) + + if key == "torque_setpoint": + self.peak_torque_update_monitor(key, value) + else: + setattr(self, key, value) except Exception as err: self.data_logger.debug(f"Error decoding message: {err}") + def peak_torque_update_monitor(self, key, value): + """ + This method monitors changes in the peak torque setpoint from the GUI thread + and ensures that a new setpoint is shared ONLY if the user is in swing-phase + + This is to prevent the current command from being altered mid-stride. + A new torque will only be felt upon the termination of the current stride. + """ + + if self.in_swing: + setattr(self, key, value) + def iterate(self)->None: """ Main loop for the actuator thread. @@ -182,6 +206,7 @@ def iterate(self)->None: It handles the actuator's state updates. """ + self.time_since_start = time.perf_counter() - self.thread_start_time self.actuator.update() self.data_logger.update() # update logger @@ -189,24 +214,16 @@ def iterate(self)->None: self.time_in_stride = time.perf_counter() - self.HS_time # acquire torque command based on gait estimate - torque_command = self.assistance_calculator.torque_generator(self.time_in_stride, + self.torque_command = self.assistance_calculator.torque_generator(self.time_in_stride, self.stride_period, float(self.torque_setpoint), self.in_swing) - # self.data_logger.debug(f"torque command: {torque_command:.2f}") - # self.data_logger.debug(f" time in stride: {self.time_in_stride:.2f}") - # self.data_logger.debug(f" stride period: {self.stride_period:.2f}") - # self.data_logger.debug(f" peak torque setpoint: {self.torque_setpoint:.2f}") - # self.data_logger.debug(f" in swing flag: {self.in_swing:.2f}") - # determine appropriate current setpoint that matches the torque setpoint - current_setpoint = self.actuator.torque_to_current(torque_command) - - # self.data_logger.debug(f"current command: {current_setpoint:.2f}") + self.current_setpoint = self.actuator.torque_to_current(self.torque_command) # command appropriate current setpoint using DephyExoboots class - if current_setpoint is not None: + if self.current_setpoint is not None: # self.actuator.set_motor_current(current_setpoint) pass else: @@ -244,7 +261,7 @@ def __init__(self, super().__init__(quit_event, pause_event, log_event, name=name, frequency=frequency) self.msg_router = msg_router - self.mailbox = None # will be instantiated + self.inbox = None # will be instantiated self.active_actuators = active_actuators self.bertec_estimators = {} @@ -333,6 +350,7 @@ def __init__(self, quit_event:Type[threading.Event], pause_event:Type[threading.Event], log_event: Type[threading.Event], + active_actuators:list[str], name:Optional[str] = None, frequency:int=100)->None: @@ -341,7 +359,11 @@ def __init__(self, self.msg_router = msg_router self.inbox = None - self.torque_setpoint = 20.0 + self.active_actuators = active_actuators + self.torque_setpoint:float = 20.0 + + self.thread_start_time:float = time.perf_counter() + self.time_since_start:float = 0.0 # track vars for csv logging self.data_logger.track_variable(lambda: self.torque_setpoint, "torque_setpt") @@ -359,23 +381,21 @@ def iterate(self): If the user doesn't input a new value, the current setpoint is used. """ + self.time_since_start = time.perf_counter() - self.thread_start_time + # update csv logging self.data_logger.update() # set a random torque setpoint self.torque_setpoint = random.randint(1,4)*10 - # send torque setpoint to the actuator thread - # msg_router.send(sender=self.name, - # recipient="left", - # contents={"torque_setpoint": self.torque_setpoint}) - - try: - msg_router.send(sender=self.name, - recipient="right", - contents={"torque_setpoint": self.torque_setpoint}) - except: - self.data_logger.debug(f"UNABLE TO SEND msg to actuator from GUICommunication thread. Skipping.") + for actuator in self.active_actuators: + try: + msg_router.send(sender=self.name, + recipient=actuator, + contents={"torque_setpoint": self.torque_setpoint}) + except: + self.data_logger.debug(f"UNABLE TO SEND msg to actuator from GUICommunication thread. Skipping.") def post_iterate(self)->None: pass @@ -721,7 +741,7 @@ def start(self) -> None: self.initialize_GSE_thread(active_actuators=self.actuators.keys()) # creating 1 thread for GUI communication - self.initialize_GUI_thread() + self.initialize_GUI_thread(active_actuators=self.actuators.keys()) def start_all_threads(self)->None: """ @@ -788,7 +808,7 @@ def initialize_GSE_thread(self, active_actuators:list[str]) -> None: LOGGER.debug(f"created gse thread") self._threads[name] = gse_thread - def initialize_GUI_thread(self) -> None: + def initialize_GUI_thread(self, active_actuators:list[str]) -> None: """ Create and start the GUI thread for user input. This method is called to set up the GUI communication thread. @@ -799,6 +819,7 @@ def initialize_GUI_thread(self) -> None: gui_thread = GUICommunication(quit_event=self._quit_event, pause_event=self._pause_event, log_event=self._log_event, + active_actuators=active_actuators, name=name, frequency=100, msg_router=self.msg_router) From 99b7e0470598a8aa0f82741e7784088b01093200 Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Fri, 20 Jun 2025 22:31:40 +0100 Subject: [PATCH 15/27] initial threading demo testing with actual exos complete! both exos spool and gse_bertec and gse_imu integrated for testing. other to-do's require addressing. --- dephyEB51.py | 24 +-- gse_imu.py | 7 +- src/exo/assistance_calculator.py | 13 +- src/settings/constants.py | 2 +- src/utils/actuator_utils.py | 4 +- threading_demo.py | 333 +++---------------------------- 6 files changed, 52 insertions(+), 331 deletions(-) diff --git a/dephyEB51.py b/dephyEB51.py index 472d4e9..40355b3 100644 --- a/dephyEB51.py +++ b/dephyEB51.py @@ -63,6 +63,7 @@ def __init__( dephy_log: bool = False, offline: bool = False, gear_ratio: float = 1.0, + imu_estimator: object = None ) -> None: CONSOLE_LOGGER.info("Initializing DephyEB51 actuator...") @@ -112,6 +113,9 @@ def __init__( self.tr_gen = VariableTransmissionRatio(self.side, TEST_TR_FILE) CONSOLE_LOGGER.info("instantiated variable transmission ratio") + # adding in imu estimator + self.imu_estimator = imu_estimator + def update_gear_ratio(self)-> None: """ @@ -146,15 +150,18 @@ def update(self): Updates the actuator state. """ - # filter the temperature before updating the thermal model - self.filter_temp() - # update the actuator state + # super().update_allData() super().update() + # filter the temperature before updating the thermal model + self.filter_temp() + # update the gear ratio self.update_gear_ratio() + # update imu gait state + # self.activation = self.imu_estimator.update(self.accelz) def assign_id_to_side(self)-> str: """ @@ -168,15 +175,11 @@ def spool_belt(self): LOGGER.info( f"Spooling {self.side} joint. " - "Please make sure the joint is free to move and press Enter to continue." + "Please make sure the joint is free to move." ) - input() self.set_motor_current(value=self.motor_sign * EXO_DEFAULT_CONFIG.BIAS_CURRENT) # in mA - time.sleep(0.3) - - def filter_temp(self): """ Filters the case temperature to remove any spikes. @@ -198,7 +201,6 @@ def filter_temp(self): CONSOLE_LOGGER.warning(f"HAD TO ANTI-SPIKE the TEMP: {self.case_temperature}") - def torque_to_current(self, torque: float) -> int: """ Converts torque setpoint (Nm) to a corresponding current (in mA) @@ -221,8 +223,6 @@ def torque_to_current(self, torque: float) -> int: return int(des_current) - - def current_to_torque(self)-> float: """ Converts current setpoint (in mA) to a corresponding torque (in Nm) @@ -249,4 +249,4 @@ def JIM_torque_to_current(self, inst_torque: float) -> int: """ pass - # TODO: Add method to home the exos at standing angle \ No newline at end of file + # TODO: Add method to home the exos at standing angle diff --git a/gse_imu.py b/gse_imu.py index 8796be2..a4dc598 100644 --- a/gse_imu.py +++ b/gse_imu.py @@ -17,7 +17,6 @@ def __init__(self, std_threshold = 2, run_len_threshold = 10, time_method = time self.std = 1 self.zscore = 0 - self.activation_state = False self.activations_pitime_local = 0 self.activations_zscore_local = 0 self.activations_pitime_start = [] @@ -38,7 +37,7 @@ def return_estimate(self): return state_dict - def update(self, accel): + def update(self, accel) -> dict: diff = abs(accel - self.prev_accel) # Mean/STD real-time @@ -77,7 +76,9 @@ def update(self, accel): else: pass - self.activations_status.append(self.activation_state) + # self.activations_status.append(self.activation_state) + + return self.activation_state diff --git a/src/exo/assistance_calculator.py b/src/exo/assistance_calculator.py index 34a397b..8c06122 100644 --- a/src/exo/assistance_calculator.py +++ b/src/exo/assistance_calculator.py @@ -19,14 +19,15 @@ from opensourceleg.logging import Logger, LogLevel from settings.constants import( - INCLINE_WALK_TIMINGS, + FLAT_WALK_TIMINGS, EXO_DEFAULT_CONFIG) +# TODO: add argument for spline params class AssistanceCalculator: def __init__(self, - t_rise:float=INCLINE_WALK_TIMINGS.P_RISE, - t_peak:float=INCLINE_WALK_TIMINGS.P_PEAK, - t_fall:float=INCLINE_WALK_TIMINGS.P_FALL, + t_rise:float=FLAT_WALK_TIMINGS.P_RISE, + t_peak:float=FLAT_WALK_TIMINGS.P_PEAK, + t_fall:float=FLAT_WALK_TIMINGS.P_FALL, holding_torque:float=EXO_DEFAULT_CONFIG.HOLDING_TORQUE, resolution:int=10000)->None: @@ -122,7 +123,7 @@ def calculate_onset_and_dropoff_times(self) -> None: self.t_dropoff = self.t_peak + self.t_fall # check that dropoff is less than toe-off (in percent stride units) - toe_off_percent = INCLINE_WALK_TIMINGS.P_TOE_OFF / self.end_of_stride_in_percent + toe_off_percent = FLAT_WALK_TIMINGS.P_TOE_OFF / self.end_of_stride_in_percent if self.t_dropoff >= toe_off_percent: raise ValueError("Drop-off time must be <= toe-off time; " "Please change the fall time to be within toe-off bounds.") @@ -281,7 +282,7 @@ def torque_generator(self, current_time:float, stride_period:float, peak_torque: for t in clock: try: # Update in_swing_flag based on percent_stride - if assistance_generator.percent_stride > INCLINE_WALK_TIMINGS.P_TOE_OFF: + if assistance_generator.percent_stride > FLAT_WALK_TIMINGS.P_TOE_OFF: in_swing_flag = True else: in_swing_flag = False diff --git a/src/settings/constants.py b/src/settings/constants.py index 85f27d4..a006703 100644 --- a/src/settings/constants.py +++ b/src/settings/constants.py @@ -117,7 +117,7 @@ """ Default Configuration """ EXO_DEFAULT_CONFIG = EXO_DEFAULT_CONSTANTS( HOLDING_TORQUE = 2, # in Nm - BIAS_CURRENT = 500 # mA (not the same as transparent mode) + BIAS_CURRENT = 750 # mA (not the same as transparent mode) ) """ IMU/GYRO Constants """ diff --git a/src/utils/actuator_utils.py b/src/utils/actuator_utils.py index cf8b7a4..1081972 100644 --- a/src/utils/actuator_utils.py +++ b/src/utils/actuator_utils.py @@ -10,6 +10,7 @@ stream_level = LogLevel.INFO, log_format = "%(levelname)s: %(message)s" ) +from gse_imu import IMU_Estimator class NoActuatorsFoundError(Exception): """Raised when no actuators are detected on available ports.""" @@ -76,7 +77,8 @@ def create_actuators(gear_ratio:float, baud_rate:int, freq:int, debug_level:int) port=port, baud_rate=baud_rate, frequency=freq, - debug_level=debug_level + debug_level=debug_level, + imu_estimator = IMU_Estimator() ) # log device ID of the actuator CONSOLE_LOGGER.info(f"Device ID: {actuator.dev_id}") diff --git a/threading_demo.py b/threading_demo.py index cbc5939..18d21c1 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -161,6 +161,7 @@ def __init__(self, self.data_logger.track_variable(lambda: self.torque_command, "torque_cmd") self.data_logger.track_variable(lambda: self.current_setpoint, "current_setpoint") + def pre_iterate(self)->None: """ Check inbox for messages from GSE & GUI threads @@ -179,10 +180,10 @@ def decode_message(self, mail): for key, value in mail.contents.items(): # TODO: value validation (not None or something) - if key == "torque_setpoint": - self.peak_torque_update_monitor(key, value) - else: - setattr(self, key, value) + # if key == "torque_setpoint": + # self.peak_torque_update_monitor(key, value) + # else: + setattr(self, key, value) except Exception as err: self.data_logger.debug(f"Error decoding message: {err}") @@ -195,7 +196,8 @@ def peak_torque_update_monitor(self, key, value): This is to prevent the current command from being altered mid-stride. A new torque will only be felt upon the termination of the current stride. """ - + # TODO: add in more logic to handle continous vs discrete modes (GUI doesn't send continous stream of data) + # TODO: potentially add in if self.in_swing: setattr(self, key, value) @@ -214,18 +216,17 @@ def iterate(self)->None: self.time_in_stride = time.perf_counter() - self.HS_time # acquire torque command based on gait estimate - self.torque_command = self.assistance_calculator.torque_generator(self.time_in_stride, - self.stride_period, - float(self.torque_setpoint), - self.in_swing) + self.torque_command = self.assistance_calculator.torque_generator(self.actuator.time_in_stride, + self.actuator.stride_period, + float(self.torque_setpoint), + self.actuator.in_swing) # determine appropriate current setpoint that matches the torque setpoint self.current_setpoint = self.actuator.torque_to_current(self.torque_command) # command appropriate current setpoint using DephyExoboots class if self.current_setpoint is not None: - # self.actuator.set_motor_current(current_setpoint) - pass + self.actuator.set_motor_current(self.current_setpoint) else: self.data_logger.warning(f"Unable to command current for {self.actuator.tag}. Skipping.") @@ -274,14 +275,13 @@ def __init__(self, selected_topic = f"fz_{actuator}" # e.g., 'fz_left' or 'fz_right' # TODO: REMOVE -- FOR TESTING ONLY - walker = WalkingSimulator(stride_period=1.20) - walker.set_percent_toe_off(67) - - self.bertec_estimators[actuator] = walker + # walker = WalkingSimulator(stride_period=1.20) + # walker.set_percent_toe_off(67) + # self.bertec_estimators[actuator] = walker # TODO: UNCOMMENT - # bertec_subscriber = Subscriber(publisher_ip=IP_ADDRESSES.VICON_IP, topic_filter=selected_topic, timeout_ms=5) - # self.bertec_estimator[actuator] = Bertec_Estimator(zmq_subscriber=bertec_subscriber) + bertec_subscriber = Subscriber(publisher_ip=IP_ADDRESSES.VICON_IP, topic_filter=selected_topic, timeout_ms=5) + self.bertec_estimators[actuator] = Bertec_Estimator(zmq_subscriber=bertec_subscriber) # track vars for csv logging self.data_logger.track_variable(lambda: self.time_since_start, f"{actuator}_time_since_start") @@ -307,7 +307,6 @@ def iterate(self): for actuator in self.active_actuators: # TODO: update the gait state estimator for the actuator - # self.bertec_estimators[actuator].update() self.bertec_estimators[actuator].update() # update csv logger @@ -387,7 +386,7 @@ def iterate(self): self.data_logger.update() # set a random torque setpoint - self.torque_setpoint = random.randint(1,4)*10 + self.torque_setpoint = 10 #random.randint(1,4)*1 for actuator in self.active_actuators: try: @@ -415,294 +414,6 @@ def on_pause(self)->None: import time -class DephyExoboots(RobotBase[DephyEB51Actuator, SensorBase]): - - def start(self) -> None: - """ - Start the Exoskeleton. - """ - super().start() - - def stop(self) -> None: - """ - Stop the Exoskeleton. - """ - - super().stop() - - def set_actuator_mode_and_gains(self, actuator)-> None: - """ - Call the setup_controller method for all actuators. - This method selects current control mode and sets PID gains for each actuator. - """ - actuator.set_control_mode(CONTROL_MODES.CURRENT) - LOGGER.info("finished setting control mode") - - actuator.set_current_gains( - kp=DEFAULT_CURRENT_GAINS.kp, - ki=DEFAULT_CURRENT_GAINS.ki, - kd=DEFAULT_CURRENT_GAINS.kd, - ff=DEFAULT_CURRENT_GAINS.ff, - ) - LOGGER.info("finished setting gains") - - def update(self) -> None: - """ - Update the exoskeleton. - """ - # print(f"Updating exoskeleton robot: {self.tag}") - super().update() - - def spool_belts(self): - """ - Spool the belts of both actuators. - This method is called to prepare the actuators for operation. - """ - for actuator in self.actuators.values(): - actuator.spool_belt() - LOGGER.info(f"finished spooling belt of {actuator.side}") - - def set_to_transparent_mode(self): - """ - Set the exo currents to 0mA. - """ - self.update_current_setpoints(current_inputs=0, asymmetric=False) - self.command_currents() - - def detect_active_actuators(self) -> Union[str, list[str]]: - """ - Detect active actuators. - Returns a string if only one actuator is active, otherwise a list of strings. - """ - - active_sides = list(self.actuators.keys()) - - if len(active_sides) == 1: - return active_sides[0] - - return active_sides - - def create_current_setpts_dict(self) -> None: - """ - create dictionary of current setpoints (in mA) corresponding to actuator side - """ - self.current_setpoints = {} - for actuator in self.actuators.values(): - self.current_setpoints[actuator.side] = 0.0 - - # TODO: generate test to determine if current_setpoints dict has the same keys as the actuators dict - - def update_current_setpoints(self, current_inputs: Union[int, Dict[str, int]], asymmetric:bool=False) -> None: - """ - Directly assign currents to the 'current_setpoints' dictionary for current control. - - If symmetric, the same current value is applied to both sides (with motor sign). - If asymmetric, the user must pass a dictionary specifying currents for each side. - - Args: - - current: int or dict. If symmetric=False, this should be a dict with 'left' and 'right' keys. - - asymmetric: bool. If True, use side-specific currents from the dictionary. - """ - # TODO: ensure that current_inputs matches the number of active sides - # TODO: if more than the number of active sides provided, trim to active one only - # TODO: handle missing sides - - # TODO: clip current setpoints to below max limit - - if asymmetric: - for side, current in current_inputs.items(): # assign different currents for each actuator - actuator = getattr(self, side) - self.current_setpoints[side] = int(current) * actuator.motor_sign - else: - for side in self.actuators.keys(): # assign the same current for both actuators - actuator = getattr(self, side) - self.current_setpoints[side] = int(current_inputs) * actuator.motor_sign - - def convert_torque_to_current_setpoints(self, torque_setpoint: float) -> dict: - """ - Find the appropriate current setpoint for the actuators. - This method is called to determine the current setpoint based on the torque setpoint. - - arguments: - torque_setpoint: float, the desired torque setpoint in Nm. - - - returns: - current_setpoints: dict of currents for each active actuator. - key is the side of the actuator (left or right). - """ - for actuator in self.actuators.values(): - self.current_setpoints[actuator.side] = actuator.torque_to_current(torque_setpoint) - LOGGER.info(f"finished finding current setpoint for {actuator.side}") - - return self.current_setpoints - - def command_currents(self) -> None: - """ - Commands current setpoints to each actuator based on the current_setpoints dictionary. - The setpoints can be unique. - """ - # TODO: ensure current_setpoints values are integers, no greater than max current limit, and are not None - - for actuator in self.actuators.values(): - - current_setpoint = self.current_setpoints.get(actuator.side) - - if current_setpoint is not None: - actuator.set_motor_current(current_setpoint) - LOGGER.info(f"Finished setting current setpoint for {actuator.side}") - else: - LOGGER.warning(f"Unknown side '{actuator.side}' and unable to command current. Skipping.") - - def initialize_rt_plots(self) -> list: - """ - Initialize real-time plots for the exoskeleton robot. - Naming and plotting is flexible to each active actuator. - - The following time series are plotted: - - Current (A) - - Temperature (°C) - - Ankle Angle (°) - - Transmission Ratio - - Ankle Torque Setpoint (Nm) - - """ - # converting actuator dictionary keys to a list - active_sides_list = list(self.actuators.keys()) - - print("Active actuators:", active_sides_list) - - # pre-slice colors based on the number of active actuators - colors = ['r', 'b'][:len(active_sides_list)] - if len(active_sides_list) > len(colors): - raise ValueError("Not enough unique colors for the number of active actuators.") - - # repeat line styles and widths for each active actuator - line_styles = ['-' for _ in active_sides_list] - line_widths = [2 for _ in active_sides_list] - - current_plt_config = {'names' : active_sides_list, - 'colors' : colors, - 'line_style': line_styles, - 'title' : "Exo Current (A) vs. Sample", - 'ylabel': "Current (A)", - 'xlabel': "timestep", - 'line_width': line_widths, - 'yrange': [0,30] - } - - temp_plt_config = {'names' : active_sides_list, - 'colors' : colors, - 'line_style': line_styles, - 'title' : "Case Temperature (°C) vs. Sample", - 'ylabel': "Temperature (°C)", - 'xlabel': "timestep", - 'line_width': line_widths, - 'yrange': [20,60] - } - - in_swing_plt_config = {'names' : active_sides_list, - 'colors' : colors, - 'line_style': line_styles, - 'title' : "Bertec in-swing vs. Sample", - 'ylabel': "Bool", - 'xlabel': "timestep", - 'line_width': line_widths, - 'yrange': [0,150] - } - - TR_plt_config = {'names' : active_sides_list, - 'colors' : colors, - 'line_style': line_styles, - 'title' : "TR (°) vs. Sample", - 'ylabel': "N", - 'xlabel': "timestep", - 'line_width': line_widths, - 'yrange': [0,20] - } - - imu_plt_config = {'names' : active_sides_list, - 'colors' : colors, - 'line_style': line_styles, - 'title' : "Activations vs. Sample", - 'ylabel': "Bool", - 'xlabel': "timestep", - 'line_width': line_widths, - 'yrange': [0,50] - } - - plot_config = [current_plt_config, temp_plt_config, in_swing_plt_config, TR_plt_config, imu_plt_config] - - return plot_config - - def update_rt_plots(self, bertec_swing_flag, imu_activations) -> list: - """ - Updates the real-time plots with current values for: - - Current (A) - - Temperature (°C) - - Bertec In swing - - Transmission Ratio - - IMU estimator activations - - The data is collected from the exoboots object and returned as a list of arrays. - This is done for each active actuator only. - - Returns: - plot_data_array: A list of data arrays (for active actuators) for each plot. - """ - - data_to_plt = [] - - for actuator in self.actuators.values(): - data_to_plt.extend([ - abs(actuator.motor_current), # Motor current - actuator.case_temperature, # Case temperature - bertec_swing_flag, - actuator.gear_ratio, # Gear ratio - imu_activations - ]) - - return data_to_plt - - def track_variables_for_logging(self, logger: Logger) -> None: - """ - Track variables for each active actuator for logging to a single file - """ - - for actuator in self.actuators.values(): - dummy_grpc_value = 5.0 - dummy_ankle_torque_setpt = 20 - logger.track_variable(lambda: time.time(), "pitime") - logger.track_variable(lambda: dummy_grpc_value, "dollar_value") - logger.track_variable(lambda: dummy_ankle_torque_setpt, "torque_setpt_Nm") - - logger.track_variable(lambda: actuator.accelx, f"{actuator._tag}_accelx_mps2") - logger.track_variable(lambda: actuator.motor_current, f"{actuator._tag}_current_mA") - logger.track_variable(lambda: actuator.motor_position, f"{actuator._tag}_position_rad") - logger.track_variable(lambda: actuator.motor_encoder_counts, f"{actuator._tag}_encoder_counts") - logger.track_variable(lambda: actuator.case_temperature, f"{actuator._tag}_case_temp_C") - - tracked_vars = logger.get_tracked_variables() - print("Tracked variables:", tracked_vars) - - @property - def left(self) -> DephyEB51Actuator: - try: - return self.actuators["left"] - except KeyError: - LOGGER.error("Ankle actuator not found. Please check for `left` key in the actuators dictionary.") - exit(1) - - @property - def right(self) -> DephyEB51Actuator: - try: - return self.actuators["right"] - except KeyError: - LOGGER.error("Ankle actuator not found. Please check for `right` key in the actuators dictionary.") - exit(1) - - - class ThreadManager: """ This class manages thread creation, communication and termination for the exoskeleton system. @@ -912,7 +623,6 @@ def gui_thread(self) -> Optional[ActuatorThread]: return self._threads.get("gui") - # Example main loop from opensourceleg.utilities import SoftRealtimeLoop from src.utils.actuator_utils import create_actuators @@ -920,6 +630,7 @@ def gui_thread(self) -> Optional[ActuatorThread]: from src.utils.walking_simulator import WalkingSimulator from exoboot_messenger_hub import MessageRouter from rtplot import client +from exoboots import DephyExoboots if __name__ == '__main__': @@ -934,6 +645,12 @@ def gui_thread(self) -> Optional[ActuatorThread]: actuators=actuators, sensors={}) + # set actuator modes + exoboots.setup_control_modes() + + # spool belts + exoboots.spool_belts() + # create a message router for inter-thread communication msg_router = MessageRouter() From c48bbe70ac0cca652a5b645e2eb8251af4078c80 Mon Sep 17 00:00:00 2001 From: Nundini Date: Sun, 22 Jun 2025 09:52:41 -0400 Subject: [PATCH 16/27] added continuous mode flag and comments to gse_imu --- gse_imu.py | 66 +++++-- src/settings/constants.py | 130 ++++++------- threading_demo.py | 391 +++++++++++++++++++++++--------------- 3 files changed, 348 insertions(+), 239 deletions(-) diff --git a/gse_imu.py b/gse_imu.py index a4dc598..42d4270 100644 --- a/gse_imu.py +++ b/gse_imu.py @@ -1,8 +1,27 @@ import time from math import sqrt + class IMU_Estimator: - def __init__(self, std_threshold = 2, run_len_threshold = 10, time_method = time.time): + """ + IMU_Estimator estimates activation events from onboard exoboot IMU acceleration data (z-axis) + using real-time mean/std tracking and z-score thresholding. + """ + + def __init__( + self, + std_threshold: float = 2, + run_len_threshold: int = 10, + time_method: float = time.time, + ): + """ + Initialize the IMU_Estimator. + + Args: + std_threshold (float): Z-score threshold for activation detection. + run_len_threshold (int): Number of consecutive samples below threshold to end activation. + time_method (callable): Function to get the current time (default: time.time). + """ self.std_threhold = std_threshold self.run_len_threshold = run_len_threshold @@ -28,26 +47,52 @@ def __init__(self, std_threshold = 2, run_len_threshold = 10, time_method = time self.time_method = time_method def __repr__(self): - rep_str = "{}, {}, {}, {}".format(self.activation_state, self.m, self.std, self.zscore) + """ + Return a string representation of the estimator's current state. + + Returns: + str: String summarizing activation state, mean, std, and z-score. + """ + + rep_str = "{}, {}, {}, {}".format( + self.activation_state, self.m, self.std, self.zscore + ) return rep_str def return_estimate(self): - state_dict = {"activation": self.activation_state - } + """ + Return the current activation state as a dictionary. + + Returns: + dict: Dictionary with the current activation state. + """ + + state_dict = {"activation": self.activation_state} return state_dict - def update(self, accel) -> dict: + def update(self, accel: float) -> dict: + """ + Update the estimator with a new acceleration value, compute statistics, + and manage activation state. + + Args: + accel (float): The new acceleration value to process. + + Returns: + bool: The current activation state after update. + """ + diff = abs(accel - self.prev_accel) # Mean/STD real-time x = diff self.n = self.n + 1 - m_new = (self.m + (x - self.m))/self.n - self.S = self.S + (x - m_new)*(x - self.m) + m_new = (self.m + (x - self.m)) / self.n + self.S = self.S + (x - m_new) * (x - self.m) - self.std = sqrt(self.S/self.n) - self.zscore = (x-m_new)/self.std + self.std = sqrt(self.S / self.n) + self.zscore = (x - m_new) / self.std # Track run length if self.zscore > self.std_threhold: @@ -81,7 +126,6 @@ def update(self, accel) -> dict: return self.activation_state - if __name__ == "__main__": asdf = IMU_Estimator() print("INIT") @@ -92,4 +136,4 @@ def update(self, accel) -> dict: print(asdf) asdf.update(100) - print(asdf) \ No newline at end of file + print(asdf) diff --git a/src/settings/constants.py b/src/settings/constants.py index a006703..d3ed08b 100644 --- a/src/settings/constants.py +++ b/src/settings/constants.py @@ -13,25 +13,23 @@ EXO_PID_GAINS, IMU_CONSTANTS, BERTEC_THRESHOLDS, - STATIC_IP_ADDRESSES + STATIC_IP_ADDRESSES, ) """ Static IP addresses """ -IP_ADDRESSES = STATIC_IP_ADDRESSES( - RTPLOT_IP = "35.3.69.66", - VICON_IP = '141.212.77.30' -) +IP_ADDRESSES = STATIC_IP_ADDRESSES(RTPLOT_IP="35.3.69.66", VICON_IP="141.212.77.30") """ File Paths on Pi """ -PORT_CFG_PATH = '/home/pi/VAS_exoboot_controller/ports.yaml' # DEPRECATED +PORT_CFG_PATH = "/home/pi/VAS_exoboot_controller/ports.yaml" # DEPRECATED SUBJECT_DATA_PATH = "subject_data" """ TRIAL TYPES AND CONDITIONS """ -TRIAL_CONDS_DICT = {"VICKREY": {"COND": ["WNE", "EPO", "NPO"], "DESC": []}, - "VAS": {"COND": [], "DESC": []}, - "JND": {"COND": ["SPLITLEG", "SAMELEG"], "DESC": ["UNIFORM", "STAIR"]}, - } +TRIAL_CONDS_DICT = { + "VICKREY": {"COND": ["WNE", "EPO", "NPO"], "DESC": []}, + "VAS": {"COND": [], "DESC": []}, + "JND": {"COND": ["SPLITLEG", "SAMELEG"], "DESC": ["UNIFORM", "STAIR"]}, +} """ Transmission Ratio Constants """ @@ -39,109 +37,101 @@ TR_COEFS_PREFIX = "{}_coefs".format(TR_FILE_PREFIX) TR_FULLDATA_PREFIX = "{}_fulldata".format(TR_FILE_PREFIX) TR_DATE_FORMATTER = "%Y_%m_%d_%H_%M" -TR_FOLDER_PATH = "./src/characterization/transmission_ratio_characterization/TR_coef_logs/" +TR_FOLDER_PATH = ( + "./src/characterization/transmission_ratio_characterization/TR_coef_logs/" +) TEST_TR_FILE = "default_TR_coefs_left_2025_02_04_16_13.csv" +""" Assistance Timing """ # tuned for VAS study -INCLINE_WALK_TIMINGS = SPLINE_PARAMS( - P_RISE=15, - P_PEAK=54, - P_FALL=12, - P_TOE_OFF=67 -) +INCLINE_WALK_TIMINGS = SPLINE_PARAMS(P_RISE=15, P_PEAK=54, P_FALL=12, P_TOE_OFF=67) # Varun's Pref Optimized Params for 1.20m/s: -FLAT_WALK_TIMINGS = SPLINE_PARAMS( - P_RISE=27.9, - P_PEAK=53.3, - P_FALL=7, - P_TOE_OFF=65 +FLAT_WALK_TIMINGS = SPLINE_PARAMS(P_RISE=27.9, P_PEAK=53.3, P_FALL=7, P_TOE_OFF=65) + +""" Assistance Type """ +CONTINUOUS_MODE_FLAG = ( + True # if true, peak torque commands will update mid-stride for instant feedback ) """ Device Identifiers """ KNOWN_PORTS = ["/dev/ttyACM0", "/dev/ttyACM1"] RIGHT_EXO_IDENTIFIERS = SIDE_SPECIFIC_EXO_IDENTIFIERS( - EXO_DEV_IDS = [77, 17584, 1013], - ANK_ENC_SIGN = -1, - MOTOR_SIGN = -1 + EXO_DEV_IDS=[77, 17584, 1013], ANK_ENC_SIGN=-1, MOTOR_SIGN=-1 ) LEFT_EXO_IDENTIFIERS = SIDE_SPECIFIC_EXO_IDENTIFIERS( - EXO_DEV_IDS = [888, 48390], - ANK_ENC_SIGN = 1, - MOTOR_SIGN = -1 + EXO_DEV_IDS=[888, 48390], ANK_ENC_SIGN=1, MOTOR_SIGN=-1 ) -DEV_ID_TO_SIDE_DICT = {id: 'right' for id in RIGHT_EXO_IDENTIFIERS.EXO_DEV_IDS} | {id: 'left' for id in LEFT_EXO_IDENTIFIERS.EXO_DEV_IDS} -DEV_ID_TO_ANK_ENC_SIGN_DICT = {id: RIGHT_EXO_IDENTIFIERS.ANK_ENC_SIGN for id in RIGHT_EXO_IDENTIFIERS.EXO_DEV_IDS} | {id: LEFT_EXO_IDENTIFIERS.ANK_ENC_SIGN for id in LEFT_EXO_IDENTIFIERS.EXO_DEV_IDS} -DEV_ID_TO_MOTOR_SIGN_DICT = {id: RIGHT_EXO_IDENTIFIERS.MOTOR_SIGN for id in RIGHT_EXO_IDENTIFIERS.EXO_DEV_IDS} | {id: LEFT_EXO_IDENTIFIERS.MOTOR_SIGN for id in LEFT_EXO_IDENTIFIERS.EXO_DEV_IDS} +DEV_ID_TO_SIDE_DICT = {id: "right" for id in RIGHT_EXO_IDENTIFIERS.EXO_DEV_IDS} | { + id: "left" for id in LEFT_EXO_IDENTIFIERS.EXO_DEV_IDS +} +DEV_ID_TO_ANK_ENC_SIGN_DICT = { + id: RIGHT_EXO_IDENTIFIERS.ANK_ENC_SIGN for id in RIGHT_EXO_IDENTIFIERS.EXO_DEV_IDS +} | {id: LEFT_EXO_IDENTIFIERS.ANK_ENC_SIGN for id in LEFT_EXO_IDENTIFIERS.EXO_DEV_IDS} +DEV_ID_TO_MOTOR_SIGN_DICT = { + id: RIGHT_EXO_IDENTIFIERS.MOTOR_SIGN for id in RIGHT_EXO_IDENTIFIERS.EXO_DEV_IDS +} | {id: LEFT_EXO_IDENTIFIERS.MOTOR_SIGN for id in LEFT_EXO_IDENTIFIERS.EXO_DEV_IDS} """ Device Attributes """ EB51_CONSTANTS = EXO_MOTOR_CONSTANTS( - MOT_ENC_CLICKS_TO_REV = 2**14, - MOT_ENC_CLICKS_TO_DEG = 360 / (2**14), - Kt = 0.146, # in Nm/A - EFFICIENCY = 0.9, - RES_PHASE = 0.279, - L_PHASE = 0.5 * 138 * 10e-6 + MOT_ENC_CLICKS_TO_REV=2**14, + MOT_ENC_CLICKS_TO_DEG=360 / (2**14), + Kt=0.146, # in Nm/A + EFFICIENCY=0.9, + RES_PHASE=0.279, + L_PHASE=0.5 * 138 * 10e-6, ) """ Device Basic Setup & Communication Constants """ EXO_SETUP_CONST = EXO_SETUP_CONSTANTS( - BAUD_RATE = 230400, - FLEXSEA_FREQ = 500, # in Hz (REMINDER: ONLY CERTAIN FREQUENCIES SUPPORTED ~ 1000, 500) - LOG_LEVEL = 3 + BAUD_RATE=230400, + FLEXSEA_FREQ=500, # in Hz (REMINDER: ONLY CERTAIN FREQUENCIES SUPPORTED ~ 1000, 500) + LOG_LEVEL=3, ) """ Controller Gains """ -DEFAULT_PID_GAINS = EXO_PID_GAINS( - KP=40, - KI=400, - KD=0, - FF=128 -) +DEFAULT_PID_GAINS = EXO_PID_GAINS(KP=40, KI=400, KD=0, FF=128) """ Thermal Parameters """ EXO_THERMAL_SAFETY_LIMITS = EXO_THERMAL_SAFETY_CONSTANTS( - MAX_CASE_TEMP = 75, # °C - MAX_WINDING_TEMP = 110 # °C + MAX_CASE_TEMP=75, MAX_WINDING_TEMP=110 # °C # °C ) """ Safety Limits """ EXO_CURRENT_SAFETY_LIMITS = EXO_CURRENT_SAFETY_CONSTANTS( - ZERO_CURRENT = 0, # mA - MAX_ALLOWABLE_CURRENT = 17000 # mA - ) + ZERO_CURRENT=0, MAX_ALLOWABLE_CURRENT=17000 # mA # mA +) """ Default Configuration """ EXO_DEFAULT_CONFIG = EXO_DEFAULT_CONSTANTS( - HOLDING_TORQUE = 2, # in Nm - BIAS_CURRENT = 750 # mA (not the same as transparent mode) - ) + HOLDING_TORQUE=2, BIAS_CURRENT=750 # in Nm # mA (not the same as transparent mode) +) """ IMU/GYRO Constants """ EXO_IMU_CONSTANTS = IMU_CONSTANTS( - ACCEL_GAIN = 1 / 8192, # note: osl DephyLegacyActuator uses m/s^2 instead of g's - GYRO_GAIN = 1 / 32.75, # note: osl DephyLegacyActuator uses rad/s instead of deg/s - ACCELX_SIGN = 1, # ALSO NOTE: ankle angles reported in deg*100 - ACCELY_SIGN = -1, - ACCELZ_SIGN = 1, - GYROX_SIGN = -1, - GYROY_SIGN = 1, - GYROZ_SIGN = 1 + ACCEL_GAIN=1 / 8192, # note: osl DephyLegacyActuator uses m/s^2 instead of g's + GYRO_GAIN=1 / 32.75, # note: osl DephyLegacyActuator uses rad/s instead of deg/s + ACCELX_SIGN=1, # ALSO NOTE: ankle angles reported in deg*100 + ACCELY_SIGN=-1, + ACCELZ_SIGN=1, + GYROX_SIGN=-1, + GYROY_SIGN=1, + GYROZ_SIGN=1, ) """ Bertec Thresholds """ BERTEC_THRESH = BERTEC_THRESHOLDS( - HS_THRESHOLD = 80, - TO_THRESHOLD = 30, - ACCEPT_STRIDE_THRESHOLD = 0.2, - ACCEPT_STANCE_THRESHOLD = 0.2, - BERTEC_ACC_LEFT = 0.25, - BERTEC_ACC_RIGHT = 0.25 + HS_THRESHOLD=80, + TO_THRESHOLD=30, + ACCEPT_STRIDE_THRESHOLD=0.2, + ACCEPT_STANCE_THRESHOLD=0.2, + BERTEC_ACC_LEFT=0.25, + BERTEC_ACC_RIGHT=0.25, ) """ Filtering Constants """ -GYROZ_W0: float = 1.0105 # Hz -TEMPANTISPIKE = 100 # °C \ No newline at end of file +GYROZ_W0: float = 1.0105 # Hz +TEMPANTISPIKE = 100 # °C diff --git a/threading_demo.py b/threading_demo.py index 18d21c1..622d501 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -22,21 +22,25 @@ class BaseWorkerThread(threading.Thread, ABC): handles the thread's main loop. """ - def __init__(self, - quit_event:Type[threading.Event], - pause_event:Type[threading.Event], - log_event:Type[threading.Event], - name:str, - frequency:int=100)->None: + def __init__( + self, + quit_event: Type[threading.Event], + pause_event: Type[threading.Event], + log_event: Type[threading.Event], + name: str, + frequency: int = 100, + ) -> None: super().__init__(name=name) - self.quit_event = quit_event # event to signal quitting + self.quit_event = quit_event # event to signal quitting self.pause_event = pause_event # event to signal pausing - self.log_event = log_event # event to signal logging + self.log_event = log_event # event to signal logging self.daemon = True self.frequency = int(frequency) - self.rt_loop = FlexibleSleeper(dt=1/frequency) # create a soft real-time loop with the specified frequency + self.rt_loop = FlexibleSleeper( + dt=1 / frequency + ) # create a soft real-time loop with the specified frequency # set-up a logger for each thread/instance logger_name = f"{name}_logger" @@ -46,28 +50,28 @@ def __init__(self, file_name=logger_name, file_level=logging.DEBUG, stream_level=logging.INFO, - buffer_size=100 + buffer_size=100, ) - def run(self)->None: + def run(self) -> None: """ Main loop structure for each instantiated thread. """ LOGGER.debug(f"[{self.name}] Thread running.") - while self.quit_event.is_set(): # while not quitting - self.pre_iterate() # run a pre-iterate method + while self.quit_event.is_set(): # while not quitting + self.pre_iterate() # run a pre-iterate method if not self.pause_event.is_set(): - self.on_pause() # if paused, run once + self.on_pause() # if paused, run once else: try: - self.iterate() # call the iterate method to perform the thread's task + self.iterate() # call the iterate method to perform the thread's task except Exception as e: LOGGER.debug(f"Exception in thread {self.name}: {e}") - self.post_iterate() # run a post-iterate method + self.post_iterate() # run a post-iterate method self.rt_loop.pause() # close logger instances before exiting threads @@ -114,6 +118,8 @@ def on_pause(self): from src.exo.assistance_calculator import AssistanceCalculator +from src.settings.constants import CONTINUOUS_MODE_FLAG + class ActuatorThread(BaseWorkerThread): """ @@ -121,19 +127,21 @@ class ActuatorThread(BaseWorkerThread): This class handles the actuator's state updates and manages the actuator's control loop. """ - def __init__(self, - name: str, - actuator, - msg_router, - quit_event: Type[threading.Event], - pause_event: Type[threading.Event], - log_event: Type[threading.Event], - frequency: int = 100) -> None: + def __init__( + self, + name: str, + actuator, + msg_router, + quit_event: Type[threading.Event], + pause_event: Type[threading.Event], + log_event: Type[threading.Event], + frequency: int = 100, + ) -> None: super().__init__(quit_event, pause_event, log_event, name, frequency=frequency) self.actuator = actuator - self.actuator.start() # start actuator + self.actuator.start() # start actuator self.msg_router = msg_router self.inbox = None @@ -142,27 +150,36 @@ def __init__(self, self.assistance_calculator = AssistanceCalculator() # set-up vars: - self.HS_time:float = 0.0 - self.stride_period:float = 1.2 - self.in_swing:bool = True - self.torque_setpoint:float = 0.0 - self.torque_command:float = 0.0 - self.current_setpoint:int = 0 - - self.thread_start_time:float = time.perf_counter() - self.time_since_start:float = 0.0 - - # track vars for csv logging - self.data_logger.track_variable(lambda: self.time_since_start, "time_since_start") + self.HS_time: float = 0.0 + self.stride_period: float = 1.2 + self.in_swing: bool = True + self.torque_setpoint: float = 0.0 + self.torque_command: float = 0.0 + self.current_setpoint: int = 0 + + self.thread_start_time: float = time.perf_counter() + self.time_since_start: float = 0.0 + + def log_vars(self): + """ + log desired variables to a csv file + """ + + self.data_logger.track_variable( + lambda: self.time_since_start, "time_since_start" + ) self.data_logger.track_variable(lambda: self.HS_time, "HS_time") self.data_logger.track_variable(lambda: self.stride_period, "stride_period") self.data_logger.track_variable(lambda: self.in_swing, "in_swing_flag_bool") - self.data_logger.track_variable(lambda: self.torque_setpoint, "peak_torque_setpt") + self.data_logger.track_variable( + lambda: self.torque_setpoint, "peak_torque_setpt" + ) self.data_logger.track_variable(lambda: self.torque_command, "torque_cmd") - self.data_logger.track_variable(lambda: self.current_setpoint, "current_setpoint") - + self.data_logger.track_variable( + lambda: self.current_setpoint, "current_setpoint" + ) - def pre_iterate(self)->None: + def pre_iterate(self) -> None: """ Check inbox for messages from GSE & GUI threads """ @@ -178,12 +195,10 @@ def decode_message(self, mail): """ try: for key, value in mail.contents.items(): - # TODO: value validation (not None or something) - - # if key == "torque_setpoint": - # self.peak_torque_update_monitor(key, value) - # else: - setattr(self, key, value) + if CONTINUOUS_MODE_FLAG and key == "torque_setpoint": + self.peak_torque_update_monitor(key, value) + else: + setattr(self, key, value) except Exception as err: self.data_logger.debug(f"Error decoding message: {err}") @@ -197,11 +212,11 @@ def peak_torque_update_monitor(self, key, value): A new torque will only be felt upon the termination of the current stride. """ # TODO: add in more logic to handle continous vs discrete modes (GUI doesn't send continous stream of data) - # TODO: potentially add in + if self.in_swing: setattr(self, key, value) - def iterate(self)->None: + def iterate(self) -> None: """ Main loop for the actuator thread. This method is called repeatedly in the thread's run loop. @@ -210,16 +225,18 @@ def iterate(self)->None: self.time_since_start = time.perf_counter() - self.thread_start_time self.actuator.update() - self.data_logger.update() # update logger + self.data_logger.update() # update logger # obtain time in current stride self.time_in_stride = time.perf_counter() - self.HS_time # acquire torque command based on gait estimate - self.torque_command = self.assistance_calculator.torque_generator(self.actuator.time_in_stride, - self.actuator.stride_period, - float(self.torque_setpoint), - self.actuator.in_swing) + self.torque_command = self.assistance_calculator.torque_generator( + self.actuator.time_in_stride, + self.actuator.stride_period, + float(self.torque_setpoint), + self.actuator.in_swing, + ) # determine appropriate current setpoint that matches the torque setpoint self.current_setpoint = self.actuator.torque_to_current(self.torque_command) @@ -228,41 +245,47 @@ def iterate(self)->None: if self.current_setpoint is not None: self.actuator.set_motor_current(self.current_setpoint) else: - self.data_logger.warning(f"Unable to command current for {self.actuator.tag}. Skipping.") + self.data_logger.warning( + f"Unable to command current for {self.actuator.tag}. Skipping." + ) - def post_iterate(self)->None: + def post_iterate(self) -> None: # TODO add rest of stuff if self.log_event.is_set(): self.data_logger.debug(f"[{self.name}] log_event True") - def on_pause(self)->None: + def on_pause(self) -> None: pass - from gse_bertec import Bertec_Estimator from src.exo.gait_state_estimator.forceplate.ZMQ_PubSub import Subscriber from src.settings.constants import IP_ADDRESSES + class GaitStateEstimatorThread(BaseWorkerThread): """ Threading class for the Gait State Estimator. This class handles gait state estimation for BOTH exoskeletons/sides together. """ - def __init__(self, - msg_router, - quit_event:Type[threading.Event], - pause_event:Type[threading.Event], - log_event: Type[threading.Event], - active_actuators:list[str], - name:Optional[str] = None, - frequency:int=100)->None: - - super().__init__(quit_event, pause_event, log_event, name=name, frequency=frequency) + def __init__( + self, + msg_router, + quit_event: Type[threading.Event], + pause_event: Type[threading.Event], + log_event: Type[threading.Event], + active_actuators: list[str], + name: Optional[str] = None, + frequency: int = 100, + ) -> None: + + super().__init__( + quit_event, pause_event, log_event, name=name, frequency=frequency + ) self.msg_router = msg_router - self.inbox = None # will be instantiated + self.inbox = None # will be instantiated self.active_actuators = active_actuators self.bertec_estimators = {} @@ -280,18 +303,46 @@ def __init__(self, # self.bertec_estimators[actuator] = walker # TODO: UNCOMMENT - bertec_subscriber = Subscriber(publisher_ip=IP_ADDRESSES.VICON_IP, topic_filter=selected_topic, timeout_ms=5) - self.bertec_estimators[actuator] = Bertec_Estimator(zmq_subscriber=bertec_subscriber) - - # track vars for csv logging - self.data_logger.track_variable(lambda: self.time_since_start, f"{actuator}_time_since_start") - self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].stride_start_time, f"{actuator}_HS_time_") - self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].stride_period, f"{actuator}_stride_period") - self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].in_swing_flag, f"{actuator}_in_swing_flag_bool") - self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].current_time_in_stride, f"{actuator}_current_time_in_stride") - self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].current_percent_gait_cycle, f"{actuator}_current_percent_gait_cycle") - - def pre_iterate(self)->None: + bertec_subscriber = Subscriber( + publisher_ip=IP_ADDRESSES.VICON_IP, + topic_filter=selected_topic, + timeout_ms=5, + ) + self.bertec_estimators[actuator] = Bertec_Estimator( + zmq_subscriber=bertec_subscriber + ) + + self.log_vars(actuator) + + def log_vars(self, actuator: str): + """ + log desired variables to a csv file + """ + self.data_logger.track_variable( + lambda: self.time_since_start, f"{actuator}_time_since_start" + ) + self.data_logger.track_variable( + lambda: self.bertec_estimators[actuator].stride_start_time, + f"{actuator}_HS_time_", + ) + self.data_logger.track_variable( + lambda: self.bertec_estimators[actuator].stride_period, + f"{actuator}_stride_period", + ) + self.data_logger.track_variable( + lambda: self.bertec_estimators[actuator].in_swing_flag, + f"{actuator}_in_swing_flag_bool", + ) + self.data_logger.track_variable( + lambda: self.bertec_estimators[actuator].current_time_in_stride, + f"{actuator}_current_time_in_stride", + ) + self.data_logger.track_variable( + lambda: self.bertec_estimators[actuator].current_percent_gait_cycle, + f"{actuator}_current_percent_gait_cycle", + ) + + def pre_iterate(self) -> None: pass def iterate(self): @@ -314,27 +365,33 @@ def iterate(self): # send message to actuator inboxes try: - self.data_logger.debug(self.bertec_estimators[actuator].return_estimate()) - - msg_router.send(sender=self.name, - recipient=actuator, - contents=self.bertec_estimators[actuator].return_estimate()) + self.data_logger.debug( + self.bertec_estimators[actuator].return_estimate() + ) + + msg_router.send( + sender=self.name, + recipient=actuator, + contents=self.bertec_estimators[actuator].return_estimate(), + ) except: - self.data_logger.debug(f"UNABLE TO SEND msg to '{actuator}' actuator from GaitStateEstimatorThread. Skipping.") + self.data_logger.debug( + f"UNABLE TO SEND msg to '{actuator}' actuator from GaitStateEstimatorThread. Skipping." + ) continue - def post_iterate(self)->None: + def post_iterate(self) -> None: pass - def on_pause(self)->None: + def on_pause(self) -> None: pass - import sys import select import random + class GUICommunication(BaseWorkerThread): """ Threading class to simulate GUI communication via gRPC. @@ -344,30 +401,39 @@ class GUICommunication(BaseWorkerThread): It then sends these setpoints to the actuators to handle. """ - def __init__(self, - msg_router, - quit_event:Type[threading.Event], - pause_event:Type[threading.Event], - log_event: Type[threading.Event], - active_actuators:list[str], - name:Optional[str] = None, - frequency:int=100)->None: - - - super().__init__(quit_event, pause_event, log_event, name=name, frequency=frequency) + def __init__( + self, + msg_router, + quit_event: Type[threading.Event], + pause_event: Type[threading.Event], + log_event: Type[threading.Event], + active_actuators: list[str], + name: Optional[str] = None, + frequency: int = 100, + ) -> None: + + super().__init__( + quit_event, pause_event, log_event, name=name, frequency=frequency + ) self.msg_router = msg_router self.inbox = None self.active_actuators = active_actuators - self.torque_setpoint:float = 20.0 + self.torque_setpoint: float = 20.0 + + self.thread_start_time: float = time.perf_counter() + self.time_since_start: float = 0.0 + + self.log_vars() - self.thread_start_time:float = time.perf_counter() - self.time_since_start:float = 0.0 + def log_vars(self): + """ + log desired variables to a csv file + """ - # track vars for csv logging self.data_logger.track_variable(lambda: self.torque_setpoint, "torque_setpt") - def pre_iterate(self)->None: + def pre_iterate(self) -> None: """ Pre-iterate method to check for new messages in the mailbox. """ @@ -386,20 +452,24 @@ def iterate(self): self.data_logger.update() # set a random torque setpoint - self.torque_setpoint = 10 #random.randint(1,4)*1 + self.torque_setpoint = 10 # random.randint(1,4)*1 for actuator in self.active_actuators: try: - msg_router.send(sender=self.name, - recipient=actuator, - contents={"torque_setpoint": self.torque_setpoint}) + msg_router.send( + sender=self.name, + recipient=actuator, + contents={"torque_setpoint": self.torque_setpoint}, + ) except: - self.data_logger.debug(f"UNABLE TO SEND msg to actuator from GUICommunication thread. Skipping.") + self.data_logger.debug( + f"UNABLE TO SEND msg to actuator from GUICommunication thread. Skipping." + ) - def post_iterate(self)->None: + def post_iterate(self) -> None: pass - def on_pause(self)->None: + def on_pause(self) -> None: pass @@ -419,20 +489,20 @@ class ThreadManager: This class manages thread creation, communication and termination for the exoskeleton system. """ - def __init__(self, msg_router, actuators:Dict) -> None: + def __init__(self, msg_router, actuators: Dict) -> None: - self.actuators = actuators # Dictionary of Actuators - self.msg_router = msg_router # MessageRouter class instance + self.actuators = actuators # Dictionary of Actuators + self.msg_router = msg_router # MessageRouter class instance # create threading events common to all threads - self._quit_event = threading.Event() # Event to signal threads to quit. - self._pause_event = threading.Event() # Event to signal threads to pause. - self._log_event = threading.Event() # Event to signal threads to log + self._quit_event = threading.Event() # Event to signal threads to quit. + self._pause_event = threading.Event() # Event to signal threads to pause. + self._log_event = threading.Event() # Event to signal threads to log # initialize thread events - self._quit_event.set() # exo is running - self._pause_event.clear() # exo starts paused - self._log_event.clear() # exo starts not logging + self._quit_event.set() # exo is running + self._pause_event.clear() # exo starts paused + self._log_event.clear() # exo starts not logging # initialize dict of threads self._threads = {} @@ -454,7 +524,7 @@ def start(self) -> None: # creating 1 thread for GUI communication self.initialize_GUI_thread(active_actuators=self.actuators.keys()) - def start_all_threads(self)->None: + def start_all_threads(self) -> None: """ Start all threads in the thread manager. This method is called to start all threads after they have been initialized. @@ -479,25 +549,26 @@ def stop(self) -> None: # ensure time to enact stop method before moving on time.sleep(0.2) - def initialize_actuator_thread(self, actuator:DephyEB51Actuator) -> None: + def initialize_actuator_thread(self, actuator: DephyEB51Actuator) -> None: """ Create and start a thread for the specified actuator. This method is called to set up the actuator communication thread. """ - actuator_thread = ActuatorThread(actuator=actuator, - quit_event=self._quit_event, - pause_event=self._pause_event, - log_event=self._log_event, - name=f"{actuator.side}", - frequency=1000, - msg_router=self.msg_router, - ) + actuator_thread = ActuatorThread( + actuator=actuator, + quit_event=self._quit_event, + pause_event=self._pause_event, + log_event=self._log_event, + name=f"{actuator.side}", + frequency=1000, + msg_router=self.msg_router, + ) LOGGER.debug(f"created {actuator.side} actuator thread") self._threads[actuator.side] = actuator_thread - def initialize_GSE_thread(self, active_actuators:list[str]) -> None: + def initialize_GSE_thread(self, active_actuators: list[str]) -> None: """ Create and start the Gait State Estimator thread. This method is called to set up the GSE communication thread. @@ -508,18 +579,20 @@ def initialize_GSE_thread(self, active_actuators:list[str]) -> None: """ # create a FIFO queue with max size for inter-thread communication name = "gse" - gse_thread = GaitStateEstimatorThread(quit_event=self._quit_event, - pause_event=self._pause_event, - log_event=self._log_event, - active_actuators=active_actuators, - name=name, - frequency=250, - msg_router=self.msg_router) + gse_thread = GaitStateEstimatorThread( + quit_event=self._quit_event, + pause_event=self._pause_event, + log_event=self._log_event, + active_actuators=active_actuators, + name=name, + frequency=250, + msg_router=self.msg_router, + ) LOGGER.debug(f"created gse thread") self._threads[name] = gse_thread - def initialize_GUI_thread(self, active_actuators:list[str]) -> None: + def initialize_GUI_thread(self, active_actuators: list[str]) -> None: """ Create and start the GUI thread for user input. This method is called to set up the GUI communication thread. @@ -527,17 +600,19 @@ def initialize_GUI_thread(self, active_actuators:list[str]) -> None: # create a FIFO queue with max size for inter-thread communication name = "gui" - gui_thread = GUICommunication(quit_event=self._quit_event, - pause_event=self._pause_event, - log_event=self._log_event, - active_actuators=active_actuators, - name=name, - frequency=100, - msg_router=self.msg_router) + gui_thread = GUICommunication( + quit_event=self._quit_event, + pause_event=self._pause_event, + log_event=self._log_event, + active_actuators=active_actuators, + name=name, + frequency=100, + msg_router=self.msg_router, + ) LOGGER.debug(f"created gui thread") self._threads[name] = gui_thread - def return_active_threads(self)->list: + def return_active_threads(self) -> list: """ Return list of active thread addresses. """ @@ -632,18 +707,18 @@ def gui_thread(self) -> Optional[ActuatorThread]: from rtplot import client from exoboots import DephyExoboots -if __name__ == '__main__': +if __name__ == "__main__": # create actuators - actuators = create_actuators(gear_ratio=1, - baud_rate=EXO_SETUP_CONST.BAUD_RATE, - freq=EXO_SETUP_CONST.FLEXSEA_FREQ, - debug_level=EXO_SETUP_CONST.LOG_LEVEL) + actuators = create_actuators( + gear_ratio=1, + baud_rate=EXO_SETUP_CONST.BAUD_RATE, + freq=EXO_SETUP_CONST.FLEXSEA_FREQ, + debug_level=EXO_SETUP_CONST.LOG_LEVEL, + ) # create Exoboots Robot - exoboots = DephyExoboots(tag="exoboots", - actuators=actuators, - sensors={}) + exoboots = DephyExoboots(tag="exoboots", actuators=actuators, sensors={}) # set actuator modes exoboots.setup_control_modes() @@ -658,7 +733,7 @@ def gui_thread(self) -> Optional[ActuatorThread]: system_manager = ThreadManager(msg_router=msg_router, actuators=actuators) # instantiate soft real-time clock - clock = SoftRealtimeLoop(dt = 1 / 1) # Hz + clock = SoftRealtimeLoop(dt=1 / 1) # Hz with system_manager: # set-up addressbook for the PostOffice & create inboxes for each thread From 4c30fc46a740b6f2351a26c1a4eb7ed6678b0aa8 Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Sun, 22 Jun 2025 15:42:18 +0100 Subject: [PATCH 17/27] added flag for toggling creation of GSE_thread, since we may not always want to have bertec streaming enabled. added flag to toggle walking simulator, just for easy switch into testing mode if bertec not available --- dephyEB51.py | 20 +- src/settings/constants.py | 4 + threading_demo.py | 413 +++++++++++++++----------------------- 3 files changed, 186 insertions(+), 251 deletions(-) diff --git a/dephyEB51.py b/dephyEB51.py index 40355b3..9c91da1 100644 --- a/dephyEB51.py +++ b/dephyEB51.py @@ -27,6 +27,7 @@ TEST_TR_FILE ) from src.exo.variable_transmission_ratio import VariableTransmissionRatio +from gse_imu import IMU_Estimator class DephyEB51Actuator(DephyLegacyActuator): @@ -113,9 +114,8 @@ def __init__( self.tr_gen = VariableTransmissionRatio(self.side, TEST_TR_FILE) CONSOLE_LOGGER.info("instantiated variable transmission ratio") - # adding in imu estimator - self.imu_estimator = imu_estimator - + # instantiate IMU-based gait-state estimator + self.imu_estimator = IMU_Estimator() def update_gear_ratio(self)-> None: """ @@ -125,6 +125,13 @@ def update_gear_ratio(self)-> None: return self._gear_ratio + def update_imu_gait_state(self, accelz:float): + """" + Update gait state using imu based thresholding + """ + + self.activation = self.imu_estimator.update(accelz) + # TODO: recharacterize transmission ratio without ENC_CLICKS_TO_DEG constant. That constant is redundant since Dephy already reports in degrees @property def ankle_angle(self) -> float: @@ -160,8 +167,11 @@ def update(self): # update the gear ratio self.update_gear_ratio() - # update imu gait state - # self.activation = self.imu_estimator.update(self.accelz) + # update gait state estimate + print("about to update IMU estimate") + self.update_imu_gait_state(self.accelz) + print("updated IMU estimate") + def assign_id_to_side(self)-> str: """ diff --git a/src/settings/constants.py b/src/settings/constants.py index d3ed08b..5b212a9 100644 --- a/src/settings/constants.py +++ b/src/settings/constants.py @@ -135,3 +135,7 @@ """ Filtering Constants """ GYROZ_W0: float = 1.0105 # Hz TEMPANTISPIKE = 100 # °C + +""" Gait State Estimation Toggle""" +RUN_BERTEC_GSE:bool = False # if true, an additional thread will be spun up that obtains bertec gait state estimates +USE_SIMULATED_WALKER:bool = True # if true, a simulated walker will output gait state estimates in the thread instead of the bertec \ No newline at end of file diff --git a/threading_demo.py b/threading_demo.py index 622d501..bd3553d 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -22,25 +22,21 @@ class BaseWorkerThread(threading.Thread, ABC): handles the thread's main loop. """ - def __init__( - self, - quit_event: Type[threading.Event], - pause_event: Type[threading.Event], - log_event: Type[threading.Event], - name: str, - frequency: int = 100, - ) -> None: + def __init__(self, + quit_event:Type[threading.Event], + pause_event:Type[threading.Event], + log_event:Type[threading.Event], + name:str, + frequency:int=100)->None: super().__init__(name=name) - self.quit_event = quit_event # event to signal quitting + self.quit_event = quit_event # event to signal quitting self.pause_event = pause_event # event to signal pausing - self.log_event = log_event # event to signal logging + self.log_event = log_event # event to signal logging self.daemon = True self.frequency = int(frequency) - self.rt_loop = FlexibleSleeper( - dt=1 / frequency - ) # create a soft real-time loop with the specified frequency + self.rt_loop = FlexibleSleeper(dt=1/frequency) # create a soft real-time loop with the specified frequency # set-up a logger for each thread/instance logger_name = f"{name}_logger" @@ -50,28 +46,28 @@ def __init__( file_name=logger_name, file_level=logging.DEBUG, stream_level=logging.INFO, - buffer_size=100, + buffer_size=100 ) - def run(self) -> None: + def run(self)->None: """ Main loop structure for each instantiated thread. """ LOGGER.debug(f"[{self.name}] Thread running.") - while self.quit_event.is_set(): # while not quitting - self.pre_iterate() # run a pre-iterate method + while self.quit_event.is_set(): # while not quitting + self.pre_iterate() # run a pre-iterate method if not self.pause_event.is_set(): - self.on_pause() # if paused, run once + self.on_pause() # if paused, run once else: try: - self.iterate() # call the iterate method to perform the thread's task + self.iterate() # call the iterate method to perform the thread's task except Exception as e: LOGGER.debug(f"Exception in thread {self.name}: {e}") - self.post_iterate() # run a post-iterate method + self.post_iterate() # run a post-iterate method self.rt_loop.pause() # close logger instances before exiting threads @@ -116,10 +112,7 @@ def on_pause(self): LOGGER.debug(f"[{self.name}] on_pre_pause() called.") pass - from src.exo.assistance_calculator import AssistanceCalculator -from src.settings.constants import CONTINUOUS_MODE_FLAG - class ActuatorThread(BaseWorkerThread): """ @@ -127,21 +120,19 @@ class ActuatorThread(BaseWorkerThread): This class handles the actuator's state updates and manages the actuator's control loop. """ - def __init__( - self, - name: str, - actuator, - msg_router, - quit_event: Type[threading.Event], - pause_event: Type[threading.Event], - log_event: Type[threading.Event], - frequency: int = 100, - ) -> None: + def __init__(self, + name: str, + actuator, + msg_router, + quit_event: Type[threading.Event], + pause_event: Type[threading.Event], + log_event: Type[threading.Event], + frequency: int = 100) -> None: super().__init__(quit_event, pause_event, log_event, name, frequency=frequency) self.actuator = actuator - self.actuator.start() # start actuator + self.actuator.start() # start actuator self.msg_router = msg_router self.inbox = None @@ -150,36 +141,27 @@ def __init__( self.assistance_calculator = AssistanceCalculator() # set-up vars: - self.HS_time: float = 0.0 - self.stride_period: float = 1.2 - self.in_swing: bool = True - self.torque_setpoint: float = 0.0 - self.torque_command: float = 0.0 - self.current_setpoint: int = 0 - - self.thread_start_time: float = time.perf_counter() - self.time_since_start: float = 0.0 - - def log_vars(self): - """ - log desired variables to a csv file - """ - - self.data_logger.track_variable( - lambda: self.time_since_start, "time_since_start" - ) + self.HS_time:float = 0.0 + self.stride_period:float = 1.2 + self.in_swing:bool = True + self.torque_setpoint:float = 0.0 + self.torque_command:float = 0.0 + self.current_setpoint:int = 0 + + self.thread_start_time:float = time.perf_counter() + self.time_since_start:float = 0.0 + + # track vars for csv logging + self.data_logger.track_variable(lambda: self.time_since_start, "time_since_start") self.data_logger.track_variable(lambda: self.HS_time, "HS_time") self.data_logger.track_variable(lambda: self.stride_period, "stride_period") self.data_logger.track_variable(lambda: self.in_swing, "in_swing_flag_bool") - self.data_logger.track_variable( - lambda: self.torque_setpoint, "peak_torque_setpt" - ) + self.data_logger.track_variable(lambda: self.torque_setpoint, "peak_torque_setpt") self.data_logger.track_variable(lambda: self.torque_command, "torque_cmd") - self.data_logger.track_variable( - lambda: self.current_setpoint, "current_setpoint" - ) + self.data_logger.track_variable(lambda: self.current_setpoint, "current_setpoint") + - def pre_iterate(self) -> None: + def pre_iterate(self)->None: """ Check inbox for messages from GSE & GUI threads """ @@ -195,10 +177,12 @@ def decode_message(self, mail): """ try: for key, value in mail.contents.items(): - if CONTINUOUS_MODE_FLAG and key == "torque_setpoint": - self.peak_torque_update_monitor(key, value) - else: - setattr(self, key, value) + # TODO: value validation (not None or something) + + # if key == "torque_setpoint": + # self.peak_torque_update_monitor(key, value) + # else: + setattr(self, key, value) except Exception as err: self.data_logger.debug(f"Error decoding message: {err}") @@ -212,11 +196,11 @@ def peak_torque_update_monitor(self, key, value): A new torque will only be felt upon the termination of the current stride. """ # TODO: add in more logic to handle continous vs discrete modes (GUI doesn't send continous stream of data) - + # TODO: potentially add in if self.in_swing: setattr(self, key, value) - def iterate(self) -> None: + def iterate(self)->None: """ Main loop for the actuator thread. This method is called repeatedly in the thread's run loop. @@ -225,18 +209,16 @@ def iterate(self) -> None: self.time_since_start = time.perf_counter() - self.thread_start_time self.actuator.update() - self.data_logger.update() # update logger + self.data_logger.update() # update logger # obtain time in current stride self.time_in_stride = time.perf_counter() - self.HS_time # acquire torque command based on gait estimate - self.torque_command = self.assistance_calculator.torque_generator( - self.actuator.time_in_stride, - self.actuator.stride_period, - float(self.torque_setpoint), - self.actuator.in_swing, - ) + self.torque_command = self.assistance_calculator.torque_generator(self.actuator.time_in_stride, + self.actuator.stride_period, + float(self.torque_setpoint), + self.actuator.in_swing) # determine appropriate current setpoint that matches the torque setpoint self.current_setpoint = self.actuator.torque_to_current(self.torque_command) @@ -245,23 +227,21 @@ def iterate(self) -> None: if self.current_setpoint is not None: self.actuator.set_motor_current(self.current_setpoint) else: - self.data_logger.warning( - f"Unable to command current for {self.actuator.tag}. Skipping." - ) + self.data_logger.warning(f"Unable to command current for {self.actuator.tag}. Skipping.") - def post_iterate(self) -> None: + def post_iterate(self)->None: # TODO add rest of stuff if self.log_event.is_set(): self.data_logger.debug(f"[{self.name}] log_event True") - def on_pause(self) -> None: + def on_pause(self)->None: pass + from gse_bertec import Bertec_Estimator from src.exo.gait_state_estimator.forceplate.ZMQ_PubSub import Subscriber -from src.settings.constants import IP_ADDRESSES - +from src.settings.constants import IP_ADDRESSES, USE_SIMULATED_WALKER class GaitStateEstimatorThread(BaseWorkerThread): """ @@ -269,23 +249,19 @@ class GaitStateEstimatorThread(BaseWorkerThread): This class handles gait state estimation for BOTH exoskeletons/sides together. """ - def __init__( - self, - msg_router, - quit_event: Type[threading.Event], - pause_event: Type[threading.Event], - log_event: Type[threading.Event], - active_actuators: list[str], - name: Optional[str] = None, - frequency: int = 100, - ) -> None: - - super().__init__( - quit_event, pause_event, log_event, name=name, frequency=frequency - ) + def __init__(self, + msg_router, + quit_event:Type[threading.Event], + pause_event:Type[threading.Event], + log_event: Type[threading.Event], + active_actuators:list[str], + name:Optional[str] = None, + frequency:int=100)->None: + + super().__init__(quit_event, pause_event, log_event, name=name, frequency=frequency) self.msg_router = msg_router - self.inbox = None # will be instantiated + self.inbox = None # will be instantiated self.active_actuators = active_actuators self.bertec_estimators = {} @@ -297,52 +273,24 @@ def __init__( for actuator in self.active_actuators: selected_topic = f"fz_{actuator}" # e.g., 'fz_left' or 'fz_right' - # TODO: REMOVE -- FOR TESTING ONLY - # walker = WalkingSimulator(stride_period=1.20) - # walker.set_percent_toe_off(67) - # self.bertec_estimators[actuator] = walker - - # TODO: UNCOMMENT - bertec_subscriber = Subscriber( - publisher_ip=IP_ADDRESSES.VICON_IP, - topic_filter=selected_topic, - timeout_ms=5, - ) - self.bertec_estimators[actuator] = Bertec_Estimator( - zmq_subscriber=bertec_subscriber - ) + if USE_SIMULATED_WALKER: + walker = WalkingSimulator(stride_period=1.20) + walker.set_percent_toe_off(67) + self.bertec_estimators[actuator] = walker - self.log_vars(actuator) + # track vars for csv logging + self.data_logger.track_variable(lambda: self.time_since_start, f"{actuator}_time_since_start") + self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].stride_start_time, f"{actuator}_HS_time_") + self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].stride_period, f"{actuator}_stride_period") + self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].in_swing_flag, f"{actuator}_in_swing_flag_bool") + self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].current_time_in_stride, f"{actuator}_current_time_in_stride") + self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].current_percent_gait_cycle, f"{actuator}_current_percent_gait_cycle") - def log_vars(self, actuator: str): - """ - log desired variables to a csv file - """ - self.data_logger.track_variable( - lambda: self.time_since_start, f"{actuator}_time_since_start" - ) - self.data_logger.track_variable( - lambda: self.bertec_estimators[actuator].stride_start_time, - f"{actuator}_HS_time_", - ) - self.data_logger.track_variable( - lambda: self.bertec_estimators[actuator].stride_period, - f"{actuator}_stride_period", - ) - self.data_logger.track_variable( - lambda: self.bertec_estimators[actuator].in_swing_flag, - f"{actuator}_in_swing_flag_bool", - ) - self.data_logger.track_variable( - lambda: self.bertec_estimators[actuator].current_time_in_stride, - f"{actuator}_current_time_in_stride", - ) - self.data_logger.track_variable( - lambda: self.bertec_estimators[actuator].current_percent_gait_cycle, - f"{actuator}_current_percent_gait_cycle", - ) + else: + bertec_subscriber = Subscriber(publisher_ip=IP_ADDRESSES.VICON_IP, topic_filter=selected_topic, timeout_ms=5) + self.bertec_estimators[actuator] = Bertec_Estimator(zmq_subscriber=bertec_subscriber) - def pre_iterate(self) -> None: + def pre_iterate(self)->None: pass def iterate(self): @@ -365,33 +313,27 @@ def iterate(self): # send message to actuator inboxes try: - self.data_logger.debug( - self.bertec_estimators[actuator].return_estimate() - ) - - msg_router.send( - sender=self.name, - recipient=actuator, - contents=self.bertec_estimators[actuator].return_estimate(), - ) + self.data_logger.debug(self.bertec_estimators[actuator].return_estimate()) + + msg_router.send(sender=self.name, + recipient=actuator, + contents=self.bertec_estimators[actuator].return_estimate()) except: - self.data_logger.debug( - f"UNABLE TO SEND msg to '{actuator}' actuator from GaitStateEstimatorThread. Skipping." - ) + self.data_logger.debug(f"UNABLE TO SEND msg to '{actuator}' actuator from GaitStateEstimatorThread. Skipping.") continue - def post_iterate(self) -> None: + def post_iterate(self)->None: pass - def on_pause(self) -> None: + def on_pause(self)->None: pass + import sys import select import random - class GUICommunication(BaseWorkerThread): """ Threading class to simulate GUI communication via gRPC. @@ -401,39 +343,30 @@ class GUICommunication(BaseWorkerThread): It then sends these setpoints to the actuators to handle. """ - def __init__( - self, - msg_router, - quit_event: Type[threading.Event], - pause_event: Type[threading.Event], - log_event: Type[threading.Event], - active_actuators: list[str], - name: Optional[str] = None, - frequency: int = 100, - ) -> None: - - super().__init__( - quit_event, pause_event, log_event, name=name, frequency=frequency - ) + def __init__(self, + msg_router, + quit_event:Type[threading.Event], + pause_event:Type[threading.Event], + log_event: Type[threading.Event], + active_actuators:list[str], + name:Optional[str] = None, + frequency:int=100)->None: + + + super().__init__(quit_event, pause_event, log_event, name=name, frequency=frequency) self.msg_router = msg_router self.inbox = None self.active_actuators = active_actuators - self.torque_setpoint: float = 20.0 - - self.thread_start_time: float = time.perf_counter() - self.time_since_start: float = 0.0 - - self.log_vars() + self.torque_setpoint:float = 20.0 - def log_vars(self): - """ - log desired variables to a csv file - """ + self.thread_start_time:float = time.perf_counter() + self.time_since_start:float = 0.0 + # track vars for csv logging self.data_logger.track_variable(lambda: self.torque_setpoint, "torque_setpt") - def pre_iterate(self) -> None: + def pre_iterate(self)->None: """ Pre-iterate method to check for new messages in the mailbox. """ @@ -452,34 +385,26 @@ def iterate(self): self.data_logger.update() # set a random torque setpoint - self.torque_setpoint = 10 # random.randint(1,4)*1 + self.torque_setpoint = 10 #random.randint(1,4)*1 for actuator in self.active_actuators: try: - msg_router.send( - sender=self.name, - recipient=actuator, - contents={"torque_setpoint": self.torque_setpoint}, - ) + msg_router.send(sender=self.name, + recipient=actuator, + contents={"torque_setpoint": self.torque_setpoint}) except: - self.data_logger.debug( - f"UNABLE TO SEND msg to actuator from GUICommunication thread. Skipping." - ) + self.data_logger.debug(f"UNABLE TO SEND msg to actuator from GUICommunication thread. Skipping.") - def post_iterate(self) -> None: + def post_iterate(self)->None: pass - def on_pause(self) -> None: + def on_pause(self)->None: pass -# Mini Exoboots Robot Class for testing threading -from opensourceleg.robots.base import RobotBase -from opensourceleg.sensors.base import SensorBase from dephyEB51 import DephyEB51Actuator from opensourceleg.logging import LOGGER -from opensourceleg.actuators.base import CONTROL_MODES -from opensourceleg.actuators.dephy import DEFAULT_CURRENT_GAINS +from src.settings.constants import RUN_BERTEC_GSE from typing import Union, Dict import time @@ -489,20 +414,20 @@ class ThreadManager: This class manages thread creation, communication and termination for the exoskeleton system. """ - def __init__(self, msg_router, actuators: Dict) -> None: + def __init__(self, msg_router, actuators:Dict) -> None: - self.actuators = actuators # Dictionary of Actuators - self.msg_router = msg_router # MessageRouter class instance + self.actuators = actuators # Dictionary of Actuators + self.msg_router = msg_router # MessageRouter class instance # create threading events common to all threads - self._quit_event = threading.Event() # Event to signal threads to quit. - self._pause_event = threading.Event() # Event to signal threads to pause. - self._log_event = threading.Event() # Event to signal threads to log + self._quit_event = threading.Event() # Event to signal threads to quit. + self._pause_event = threading.Event() # Event to signal threads to pause. + self._log_event = threading.Event() # Event to signal threads to log # initialize thread events - self._quit_event.set() # exo is running - self._pause_event.clear() # exo starts paused - self._log_event.clear() # exo starts not logging + self._quit_event.set() # exo is running + self._pause_event.clear() # exo starts paused + self._log_event.clear() # exo starts not logging # initialize dict of threads self._threads = {} @@ -511,7 +436,7 @@ def start(self) -> None: """ Creates the following threads: - for each ACTIVE actuator - - for the Gait State Estimator + - for the Gait State Estimator (ONLY IF THE RUN_BERTEC_GSE_FLAG = True) - for the GUI """ @@ -519,12 +444,13 @@ def start(self) -> None: self.initialize_actuator_thread(actuator) # creating 1 thread for the Gait State Estimator - self.initialize_GSE_thread(active_actuators=self.actuators.keys()) + if RUN_BERTEC_GSE == True: + self.initialize_GSE_thread(active_actuators=self.actuators.keys()) # creating 1 thread for GUI communication self.initialize_GUI_thread(active_actuators=self.actuators.keys()) - def start_all_threads(self) -> None: + def start_all_threads(self)->None: """ Start all threads in the thread manager. This method is called to start all threads after they have been initialized. @@ -549,26 +475,25 @@ def stop(self) -> None: # ensure time to enact stop method before moving on time.sleep(0.2) - def initialize_actuator_thread(self, actuator: DephyEB51Actuator) -> None: + def initialize_actuator_thread(self, actuator:DephyEB51Actuator) -> None: """ Create and start a thread for the specified actuator. This method is called to set up the actuator communication thread. """ - actuator_thread = ActuatorThread( - actuator=actuator, - quit_event=self._quit_event, - pause_event=self._pause_event, - log_event=self._log_event, - name=f"{actuator.side}", - frequency=1000, - msg_router=self.msg_router, - ) + actuator_thread = ActuatorThread(actuator=actuator, + quit_event=self._quit_event, + pause_event=self._pause_event, + log_event=self._log_event, + name=f"{actuator.side}", + frequency=1000, + msg_router=self.msg_router, + ) LOGGER.debug(f"created {actuator.side} actuator thread") self._threads[actuator.side] = actuator_thread - def initialize_GSE_thread(self, active_actuators: list[str]) -> None: + def initialize_GSE_thread(self, active_actuators:list[str]) -> None: """ Create and start the Gait State Estimator thread. This method is called to set up the GSE communication thread. @@ -579,20 +504,18 @@ def initialize_GSE_thread(self, active_actuators: list[str]) -> None: """ # create a FIFO queue with max size for inter-thread communication name = "gse" - gse_thread = GaitStateEstimatorThread( - quit_event=self._quit_event, - pause_event=self._pause_event, - log_event=self._log_event, - active_actuators=active_actuators, - name=name, - frequency=250, - msg_router=self.msg_router, - ) + gse_thread = GaitStateEstimatorThread(quit_event=self._quit_event, + pause_event=self._pause_event, + log_event=self._log_event, + active_actuators=active_actuators, + name=name, + frequency=250, + msg_router=self.msg_router) LOGGER.debug(f"created gse thread") self._threads[name] = gse_thread - def initialize_GUI_thread(self, active_actuators: list[str]) -> None: + def initialize_GUI_thread(self, active_actuators:list[str]) -> None: """ Create and start the GUI thread for user input. This method is called to set up the GUI communication thread. @@ -600,19 +523,17 @@ def initialize_GUI_thread(self, active_actuators: list[str]) -> None: # create a FIFO queue with max size for inter-thread communication name = "gui" - gui_thread = GUICommunication( - quit_event=self._quit_event, - pause_event=self._pause_event, - log_event=self._log_event, - active_actuators=active_actuators, - name=name, - frequency=100, - msg_router=self.msg_router, - ) + gui_thread = GUICommunication(quit_event=self._quit_event, + pause_event=self._pause_event, + log_event=self._log_event, + active_actuators=active_actuators, + name=name, + frequency=100, + msg_router=self.msg_router) LOGGER.debug(f"created gui thread") self._threads[name] = gui_thread - def return_active_threads(self) -> list: + def return_active_threads(self)->list: """ Return list of active thread addresses. """ @@ -707,18 +628,18 @@ def gui_thread(self) -> Optional[ActuatorThread]: from rtplot import client from exoboots import DephyExoboots -if __name__ == "__main__": +if __name__ == '__main__': # create actuators - actuators = create_actuators( - gear_ratio=1, - baud_rate=EXO_SETUP_CONST.BAUD_RATE, - freq=EXO_SETUP_CONST.FLEXSEA_FREQ, - debug_level=EXO_SETUP_CONST.LOG_LEVEL, - ) + actuators = create_actuators(gear_ratio=1, + baud_rate=EXO_SETUP_CONST.BAUD_RATE, + freq=EXO_SETUP_CONST.FLEXSEA_FREQ, + debug_level=EXO_SETUP_CONST.LOG_LEVEL) # create Exoboots Robot - exoboots = DephyExoboots(tag="exoboots", actuators=actuators, sensors={}) + exoboots = DephyExoboots(tag="exoboots", + actuators=actuators, + sensors={}) # set actuator modes exoboots.setup_control_modes() @@ -733,7 +654,7 @@ def gui_thread(self) -> Optional[ActuatorThread]: system_manager = ThreadManager(msg_router=msg_router, actuators=actuators) # instantiate soft real-time clock - clock = SoftRealtimeLoop(dt=1 / 1) # Hz + clock = SoftRealtimeLoop(dt = 1 / 1) # Hz with system_manager: # set-up addressbook for the PostOffice & create inboxes for each thread @@ -742,7 +663,7 @@ def gui_thread(self) -> Optional[ActuatorThread]: # start all threads system_manager.start_all_threads() - # unpause exos (removed not from base thread for this to work) + # unpause exos (removed "not" from base thread for this to work) system_manager._pause_event.set() for t in clock: From 6b6061d48b7e867848be507c6ea535abc7468605 Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Sun, 22 Jun 2025 15:51:46 +0100 Subject: [PATCH 18/27] added in continuous flag back in since for some reason it didn't get pulled from local -- will need to assess wth is going on --- src/settings/constants.py | 4 ++-- threading_demo.py | 14 ++++++-------- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/src/settings/constants.py b/src/settings/constants.py index 5b212a9..17a51f9 100644 --- a/src/settings/constants.py +++ b/src/settings/constants.py @@ -137,5 +137,5 @@ TEMPANTISPIKE = 100 # °C """ Gait State Estimation Toggle""" -RUN_BERTEC_GSE:bool = False # if true, an additional thread will be spun up that obtains bertec gait state estimates -USE_SIMULATED_WALKER:bool = True # if true, a simulated walker will output gait state estimates in the thread instead of the bertec \ No newline at end of file +RUN_BERTEC_GSE:bool = True # if true, an additional thread will be spun up that obtains bertec gait state estimates +USE_SIMULATED_WALKER:bool = True # if true, a simulated walker will output gait state estimates INSTEAD of the bertec in the thread \ No newline at end of file diff --git a/threading_demo.py b/threading_demo.py index bd3553d..a405918 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -113,6 +113,7 @@ def on_pause(self): pass from src.exo.assistance_calculator import AssistanceCalculator +from src.settings.constants import CONTINUOUS_MODE_FLAG class ActuatorThread(BaseWorkerThread): """ @@ -160,7 +161,6 @@ def __init__(self, self.data_logger.track_variable(lambda: self.torque_command, "torque_cmd") self.data_logger.track_variable(lambda: self.current_setpoint, "current_setpoint") - def pre_iterate(self)->None: """ Check inbox for messages from GSE & GUI threads @@ -177,12 +177,10 @@ def decode_message(self, mail): """ try: for key, value in mail.contents.items(): - # TODO: value validation (not None or something) - - # if key == "torque_setpoint": - # self.peak_torque_update_monitor(key, value) - # else: - setattr(self, key, value) + if CONTINUOUS_MODE_FLAG and key == "torque_setpoint": + self.peak_torque_update_monitor(key, value) + else: + setattr(self, key, value) except Exception as err: self.data_logger.debug(f"Error decoding message: {err}") @@ -196,7 +194,7 @@ def peak_torque_update_monitor(self, key, value): A new torque will only be felt upon the termination of the current stride. """ # TODO: add in more logic to handle continous vs discrete modes (GUI doesn't send continous stream of data) - # TODO: potentially add in + if self.in_swing: setattr(self, key, value) From edbcf11e5e300c27d945c360a78e5aa570dde884 Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Sun, 22 Jun 2025 16:04:54 +0100 Subject: [PATCH 19/27] ensured that the set_new_timing_params() in assistance_calculator properly regenerates a spline profile given new parameters. ensured that class is flexible to different timing params --- src/exo/assistance_calculator.py | 25 +++++++++++++++++++------ threading_demo.py | 14 ++++++++------ 2 files changed, 27 insertions(+), 12 deletions(-) diff --git a/src/exo/assistance_calculator.py b/src/exo/assistance_calculator.py index 8c06122..95dd4e3 100644 --- a/src/exo/assistance_calculator.py +++ b/src/exo/assistance_calculator.py @@ -19,15 +19,15 @@ from opensourceleg.logging import Logger, LogLevel from settings.constants import( - FLAT_WALK_TIMINGS, + INCLINE_WALK_TIMINGS, EXO_DEFAULT_CONFIG) # TODO: add argument for spline params class AssistanceCalculator: def __init__(self, - t_rise:float=FLAT_WALK_TIMINGS.P_RISE, - t_peak:float=FLAT_WALK_TIMINGS.P_PEAK, - t_fall:float=FLAT_WALK_TIMINGS.P_FALL, + t_rise:float=INCLINE_WALK_TIMINGS.P_RISE, + t_peak:float=INCLINE_WALK_TIMINGS.P_PEAK, + t_fall:float=INCLINE_WALK_TIMINGS.P_FALL, holding_torque:float=EXO_DEFAULT_CONFIG.HOLDING_TORQUE, resolution:int=10000)->None: @@ -76,6 +76,19 @@ def set_new_timing_params(self, t_rise:float, t_peak:float, t_fall:float) -> Non self.t_rise = t_rise self.t_peak = t_peak self.t_fall = t_fall + + # convert timing params to percent stride + self.convert_params_to_percent_stride() + + # determine torque onset & drop-off inflection pts + self.calculate_onset_and_dropoff_times() + + # determine rising and falling spline objects + rising_spline, falling_spline = self.create_spline_sections() + + # create normalized profile + self.create_normalized_profile(rising_spline, falling_spline) + except: raise Exception("set_new_timing_params failed in assistance generator") @@ -123,7 +136,7 @@ def calculate_onset_and_dropoff_times(self) -> None: self.t_dropoff = self.t_peak + self.t_fall # check that dropoff is less than toe-off (in percent stride units) - toe_off_percent = FLAT_WALK_TIMINGS.P_TOE_OFF / self.end_of_stride_in_percent + toe_off_percent = INCLINE_WALK_TIMINGS.P_TOE_OFF / self.end_of_stride_in_percent if self.t_dropoff >= toe_off_percent: raise ValueError("Drop-off time must be <= toe-off time; " "Please change the fall time to be within toe-off bounds.") @@ -282,7 +295,7 @@ def torque_generator(self, current_time:float, stride_period:float, peak_torque: for t in clock: try: # Update in_swing_flag based on percent_stride - if assistance_generator.percent_stride > FLAT_WALK_TIMINGS.P_TOE_OFF: + if assistance_generator.percent_stride > INCLINE_WALK_TIMINGS.P_TOE_OFF: in_swing_flag = True else: in_swing_flag = False diff --git a/threading_demo.py b/threading_demo.py index a405918..0022022 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -113,7 +113,7 @@ def on_pause(self): pass from src.exo.assistance_calculator import AssistanceCalculator -from src.settings.constants import CONTINUOUS_MODE_FLAG +from src.settings.constants import CONTINUOUS_MODE_FLAG, FLAT_WALK_TIMINGS, EXO_DEFAULT_CONFIG class ActuatorThread(BaseWorkerThread): """ @@ -139,7 +139,9 @@ def __init__(self, self.inbox = None # instantiate assistance generator - self.assistance_calculator = AssistanceCalculator() + self.assistance_calculator = AssistanceCalculator(t_rise=FLAT_WALK_TIMINGS.P_RISE, + t_peak=FLAT_WALK_TIMINGS.P_PEAK, + t_fall=FLAT_WALK_TIMINGS.P_FALL) # set-up vars: self.HS_time:float = 0.0 @@ -213,10 +215,10 @@ def iterate(self)->None: self.time_in_stride = time.perf_counter() - self.HS_time # acquire torque command based on gait estimate - self.torque_command = self.assistance_calculator.torque_generator(self.actuator.time_in_stride, - self.actuator.stride_period, - float(self.torque_setpoint), - self.actuator.in_swing) + self.torque_command = self.assistance_calculator.torque_generator(current_time=self.actuator.time_in_stride, + stride_period=self.actuator.stride_period, + peak_torque=float(self.torque_setpoint), + in_swing=self.actuator.in_swing) # determine appropriate current setpoint that matches the torque setpoint self.current_setpoint = self.actuator.torque_to_current(self.torque_command) From 30038e658a3ef7c05b642fa7c04f94e1b7575577 Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Sun, 22 Jun 2025 18:34:17 +0100 Subject: [PATCH 20/27] added in class that recieves data from actuators and plots to rtplot in main --- dephyEB51.py | 1 - exoboot_messenger_hub.py | 6 +- .../JIM_testing_current_cmd.py | 9 +- src/settings/constants.py | 1 + threading_demo.py | 173 ++++++++++++++++-- 5 files changed, 164 insertions(+), 26 deletions(-) diff --git a/dephyEB51.py b/dephyEB51.py index 9c91da1..6facb8a 100644 --- a/dephyEB51.py +++ b/dephyEB51.py @@ -172,7 +172,6 @@ def update(self): self.update_imu_gait_state(self.accelz) print("updated IMU estimate") - def assign_id_to_side(self)-> str: """ Determines side (left/right) of the actuator based on previously mapped device ID number. diff --git a/exoboot_messenger_hub.py b/exoboot_messenger_hub.py index a6cd5d8..edc4415 100644 --- a/exoboot_messenger_hub.py +++ b/exoboot_messenger_hub.py @@ -108,9 +108,9 @@ def setup_addressbook(self, *threads: Thread) -> None: CONSOLE_LOGGER.info(f"Inbox already exists for thread: {thread.name}") continue - elif not isinstance(thread, Thread): - # if thread is not a Thread instance, raise error - raise TypeError("Addressbook expected a Thread instance.") + # elif not isinstance(thread, Thread): + # # if thread is not a Thread instance, raise error + # raise TypeError("Addressbook expected a Thread instance.") elif thread.name in self._addressbook: # if thread name already exists in addressbook, skip diff --git a/src/characterization/JIM_characterization/JIM_testing_current_cmd.py b/src/characterization/JIM_characterization/JIM_testing_current_cmd.py index dec5b96..412648e 100644 --- a/src/characterization/JIM_characterization/JIM_testing_current_cmd.py +++ b/src/characterization/JIM_characterization/JIM_testing_current_cmd.py @@ -88,6 +88,10 @@ def __init__(self): # update robot sensor states exoboots.update() + # send data to server & update real-time plots + data_to_plt = JIM_data_plotter.update_JIM_rt_plots() + client.send_array(data_to_plt) + if (t > 0.0) and (t <= float(args.alignment_event_time)): exoboots.set_to_transparent_mode() print(f"in transparent mode") @@ -100,16 +104,11 @@ def __init__(self): exoboots.command_currents() elif (t > ramp_period) and (t <= float(args.time)): - print(f"RAMP COMPLETED. Current setpoint is now {args.current_setpt_mA} mA") # Command the exo to peak set point current & hold for specified duration exoboots.update_current_setpoints(current_inputs=int(args.current_setpt_mA), asymmetric=False) exoboots.command_currents() - # send data to server & update real-time plots - data_to_plt = JIM_data_plotter.update_JIM_rt_plots() - client.send_array(data_to_plt) - else: exoboots.set_to_transparent_mode() break diff --git a/src/settings/constants.py b/src/settings/constants.py index 17a51f9..7037cdb 100644 --- a/src/settings/constants.py +++ b/src/settings/constants.py @@ -17,6 +17,7 @@ ) """ Static IP addresses """ +# run rtplot with this command: python3 -m rtplot.server -p !!!INSERT CLIENT IP HERE!!! IP_ADDRESSES = STATIC_IP_ADDRESSES(RTPLOT_IP="35.3.69.66", VICON_IP="141.212.77.30") """ File Paths on Pi """ diff --git a/threading_demo.py b/threading_demo.py index 0022022..2a4cac9 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -113,7 +113,7 @@ def on_pause(self): pass from src.exo.assistance_calculator import AssistanceCalculator -from src.settings.constants import CONTINUOUS_MODE_FLAG, FLAT_WALK_TIMINGS, EXO_DEFAULT_CONFIG +from src.settings.constants import CONTINUOUS_MODE_FLAG, FLAT_WALK_TIMINGS class ActuatorThread(BaseWorkerThread): """ @@ -150,6 +150,7 @@ def __init__(self, self.torque_setpoint:float = 0.0 self.torque_command:float = 0.0 self.current_setpoint:int = 0 + self.time_in_stride:float = 0.0 self.thread_start_time:float = time.perf_counter() self.time_since_start:float = 0.0 @@ -208,17 +209,17 @@ def iterate(self)->None: """ self.time_since_start = time.perf_counter() - self.thread_start_time - self.actuator.update() + self.actuator.update() # update actuator states self.data_logger.update() # update logger # obtain time in current stride self.time_in_stride = time.perf_counter() - self.HS_time # acquire torque command based on gait estimate - self.torque_command = self.assistance_calculator.torque_generator(current_time=self.actuator.time_in_stride, - stride_period=self.actuator.stride_period, + self.torque_command = self.assistance_calculator.torque_generator(current_time=self.time_in_stride, + stride_period=self.stride_period, peak_torque=float(self.torque_setpoint), - in_swing=self.actuator.in_swing) + in_swing=self.in_swing) # determine appropriate current setpoint that matches the torque setpoint self.current_setpoint = self.actuator.torque_to_current(self.torque_command) @@ -230,6 +231,19 @@ def iterate(self)->None: self.data_logger.warning(f"Unable to command current for {self.actuator.tag}. Skipping.") def post_iterate(self)->None: + # send data to main thread for rtplotting + try: + msg_router.send(sender=self.name, + recipient="main", + contents={"time_since_start":self.time_since_start, + "peak_torque_setpoint": self.torque_setpoint, + "torque_cmd": self.torque_command, + "current_setpoint": self.current_setpoint, + }) + except: + self.data_logger.debug(f"UNABLE TO SEND msg to MAIN from {self.name} thread. Skipping.") + + # TODO add rest of stuff if self.log_event.is_set(): self.data_logger.debug(f"[{self.name}] log_event True") @@ -358,7 +372,7 @@ def __init__(self, self.msg_router = msg_router self.inbox = None self.active_actuators = active_actuators - self.torque_setpoint:float = 20.0 + self.torque_setpoint:float = 0.0 self.thread_start_time:float = time.perf_counter() self.time_since_start:float = 0.0 @@ -385,7 +399,7 @@ def iterate(self): self.data_logger.update() # set a random torque setpoint - self.torque_setpoint = 10 #random.randint(1,4)*1 + self.torque_setpoint = 7 #random.randint(1,4)*10 for actuator in self.active_actuators: try: @@ -619,6 +633,120 @@ def gui_thread(self) -> Optional[ActuatorThread]: return self._threads.get("gui") +class MainThreadMessageReception(): + + def __init__(self, actuators): + """ + Main thread named and set-up. + + Args: + - actuators(DephyEB51): dictonary of actuator objects + """ + + self.name = "main" + self.inbox = None + self.actuators = actuators + + # define initial vars to be recepted + self.current_setpoint:int = 0 + self.torque_setpoint:float = 0.0 + self.torque_command:float = 0.0 + + def check_msg_inbox(self): + """ + Main thread checks it's message inbox. + """ + mail_list = self.inbox.get_all_mail() + for mail in mail_list: + self.decode_message(mail) + + def decode_message(self, mail): + """ + Decode a message from the inbox. + This method extracts the contents of the message and updates the class states accordingly. + + Args: + Mail: mail object + """ + try: + for key, value in mail.contents.items(): + setattr(self, key, value) + + except Exception as err: + LOGGER.debug(f"Error decoding message: {err}") + + def initialize_rt_plots(self) -> list: + """ + Initialize real-time plots. + + """ + # converting actuator dictionary keys to a list + active_sides_list = list(self.actuators.keys()) + + print("Active actuators:", active_sides_list) + + # pre-slice colors based on the number of active actuators + colors = ['r', 'b'][:len(active_sides_list)] + if len(active_sides_list) > len(colors): + raise ValueError("Not enough unique colors for the number of active actuators.") + + # repeat line styles and widths for each active actuator + line_styles = ['-' for _ in active_sides_list] + line_widths = [2 for _ in active_sides_list] + + current_plt_config = {'names' : active_sides_list, + 'colors' : colors, + 'line_style': line_styles, + 'title' : "Exo Current (A) vs. Sample", + 'ylabel': "Current (A)", + 'xlabel': "timestep", + 'line_width': line_widths, + 'yrange': [0,30] + } + + torque_setpt_plt_config = {'names' : active_sides_list, + 'colors' : colors, + 'line_style': line_styles, + 'title' : "Exo Torque Setpt (Nm) vs. Sample", + 'ylabel': "Torque (Nm)", + 'xlabel': "timestep", + 'line_width': line_widths, + 'yrange': [0,30] + } + + torque_cmd_plt_config = {'names' : active_sides_list, + 'colors' : colors, + 'line_style': line_styles, + 'title' : "Exo Torque Cmds (A) vs. Sample", + 'ylabel': "Torque (Nm)", + 'xlabel': "timestep", + 'line_width': line_widths, + 'yrange': [0,30] + } + + plot_config = [current_plt_config, torque_setpt_plt_config, torque_cmd_plt_config] + + return plot_config + + def update_rt_plots(self) -> list: + """ + Updates the real-time plots with current values for: + + Returns: + plot_data_array: A list of data arrays (for active actuators) for each plot. + """ + + data_to_plt = [] + for actuator in self.actuators.values(): + data_to_plt.extend([ + abs(actuator.motor_current), + actuator.torque_setpoint, + actuator.torque_command + ]) + + return data_to_plt + + # Example main loop from opensourceleg.utilities import SoftRealtimeLoop from src.utils.actuator_utils import create_actuators @@ -630,23 +758,28 @@ def gui_thread(self) -> Optional[ActuatorThread]: if __name__ == '__main__': - # create actuators + # create actuators & Exoboots Robot actuators = create_actuators(gear_ratio=1, - baud_rate=EXO_SETUP_CONST.BAUD_RATE, - freq=EXO_SETUP_CONST.FLEXSEA_FREQ, - debug_level=EXO_SETUP_CONST.LOG_LEVEL) + baud_rate=EXO_SETUP_CONST.BAUD_RATE, + freq=EXO_SETUP_CONST.FLEXSEA_FREQ, + debug_level=EXO_SETUP_CONST.LOG_LEVEL) - # create Exoboots Robot exoboots = DephyExoboots(tag="exoboots", actuators=actuators, sensors={}) - # set actuator modes + # set actuator modes & spool belts exoboots.setup_control_modes() - - # spool belts exoboots.spool_belts() + # initialize class responsible for recieving data & rtplotting for the main thread + main_thread_receptor = MainThreadMessageReception(actuators) + + # initalize rtplotting + client.configure_ip(IP_ADDRESSES.RTPLOT_IP) + plot_config = main_thread_receptor.initialize_rt_plots() + client.initialize_plots(plot_config) + # create a message router for inter-thread communication msg_router = MessageRouter() @@ -658,7 +791,7 @@ def gui_thread(self) -> Optional[ActuatorThread]: with system_manager: # set-up addressbook for the PostOffice & create inboxes for each thread - msg_router.setup_addressbook(*system_manager.return_active_threads()) + msg_router.setup_addressbook(*system_manager.return_active_threads(), main_thread_receptor) # start all threads system_manager.start_all_threads() @@ -668,7 +801,13 @@ def gui_thread(self) -> Optional[ActuatorThread]: for t in clock: try: - pass + # decode messages from actuator threads + + + # send data to server & update real-time plots + data_to_plt = main_thread_receptor.update_rt_plots() + client.send_array(data_to_plt) + except KeyboardInterrupt: print("KeyboardInterrupt received.") From b4da2a4f001c87a355badade6ed8ee38f6202379 Mon Sep 17 00:00:00 2001 From: Nundini Date: Mon, 23 Jun 2025 09:46:18 -0400 Subject: [PATCH 21/27] added flexibile rtplotting vars for both actuators in MainThreadMessageReception class. needs testing --- threading_demo.py | 517 ++++++++++++++++++++++++++++------------------ 1 file changed, 311 insertions(+), 206 deletions(-) diff --git a/threading_demo.py b/threading_demo.py index 2a4cac9..d0307c2 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -22,21 +22,25 @@ class BaseWorkerThread(threading.Thread, ABC): handles the thread's main loop. """ - def __init__(self, - quit_event:Type[threading.Event], - pause_event:Type[threading.Event], - log_event:Type[threading.Event], - name:str, - frequency:int=100)->None: + def __init__( + self, + quit_event: Type[threading.Event], + pause_event: Type[threading.Event], + log_event: Type[threading.Event], + name: str, + frequency: int = 100, + ) -> None: super().__init__(name=name) - self.quit_event = quit_event # event to signal quitting + self.quit_event = quit_event # event to signal quitting self.pause_event = pause_event # event to signal pausing - self.log_event = log_event # event to signal logging + self.log_event = log_event # event to signal logging self.daemon = True self.frequency = int(frequency) - self.rt_loop = FlexibleSleeper(dt=1/frequency) # create a soft real-time loop with the specified frequency + self.rt_loop = FlexibleSleeper( + dt=1 / frequency + ) # create a soft real-time loop with the specified frequency # set-up a logger for each thread/instance logger_name = f"{name}_logger" @@ -46,28 +50,28 @@ def __init__(self, file_name=logger_name, file_level=logging.DEBUG, stream_level=logging.INFO, - buffer_size=100 + buffer_size=100, ) - def run(self)->None: + def run(self) -> None: """ Main loop structure for each instantiated thread. """ LOGGER.debug(f"[{self.name}] Thread running.") - while self.quit_event.is_set(): # while not quitting - self.pre_iterate() # run a pre-iterate method + while self.quit_event.is_set(): # while not quitting + self.pre_iterate() # run a pre-iterate method if not self.pause_event.is_set(): - self.on_pause() # if paused, run once + self.on_pause() # if paused, run once else: try: - self.iterate() # call the iterate method to perform the thread's task + self.iterate() # call the iterate method to perform the thread's task except Exception as e: LOGGER.debug(f"Exception in thread {self.name}: {e}") - self.post_iterate() # run a post-iterate method + self.post_iterate() # run a post-iterate method self.rt_loop.pause() # close logger instances before exiting threads @@ -112,59 +116,71 @@ def on_pause(self): LOGGER.debug(f"[{self.name}] on_pre_pause() called.") pass + from src.exo.assistance_calculator import AssistanceCalculator from src.settings.constants import CONTINUOUS_MODE_FLAG, FLAT_WALK_TIMINGS + class ActuatorThread(BaseWorkerThread): """ Threading class for EACH actuator. This class handles the actuator's state updates and manages the actuator's control loop. """ - def __init__(self, - name: str, - actuator, - msg_router, - quit_event: Type[threading.Event], - pause_event: Type[threading.Event], - log_event: Type[threading.Event], - frequency: int = 100) -> None: + def __init__( + self, + name: str, + actuator, + msg_router, + quit_event: Type[threading.Event], + pause_event: Type[threading.Event], + log_event: Type[threading.Event], + frequency: int = 100, + ) -> None: super().__init__(quit_event, pause_event, log_event, name, frequency=frequency) self.actuator = actuator - self.actuator.start() # start actuator + self.actuator.start() # start actuator self.msg_router = msg_router self.inbox = None # instantiate assistance generator - self.assistance_calculator = AssistanceCalculator(t_rise=FLAT_WALK_TIMINGS.P_RISE, - t_peak=FLAT_WALK_TIMINGS.P_PEAK, - t_fall=FLAT_WALK_TIMINGS.P_FALL) + self.assistance_calculator = AssistanceCalculator( + t_rise=FLAT_WALK_TIMINGS.P_RISE, + t_peak=FLAT_WALK_TIMINGS.P_PEAK, + t_fall=FLAT_WALK_TIMINGS.P_FALL, + ) # set-up vars: - self.HS_time:float = 0.0 - self.stride_period:float = 1.2 - self.in_swing:bool = True - self.torque_setpoint:float = 0.0 - self.torque_command:float = 0.0 - self.current_setpoint:int = 0 - self.time_in_stride:float = 0.0 + self.HS_time: float = 0.0 + self.stride_period: float = 1.2 + self.in_swing: bool = True + self.torque_setpoint: float = 0.0 + self.torque_command: float = 0.0 + self.current_setpoint: int = 0 + self.time_in_stride: float = 0.0 - self.thread_start_time:float = time.perf_counter() - self.time_since_start:float = 0.0 + self.thread_start_time: float = time.perf_counter() + self.time_since_start: float = 0.0 # track vars for csv logging - self.data_logger.track_variable(lambda: self.time_since_start, "time_since_start") + self.data_logger.track_variable( + lambda: self.time_since_start, "time_since_start" + ) self.data_logger.track_variable(lambda: self.HS_time, "HS_time") self.data_logger.track_variable(lambda: self.stride_period, "stride_period") self.data_logger.track_variable(lambda: self.in_swing, "in_swing_flag_bool") - self.data_logger.track_variable(lambda: self.torque_setpoint, "peak_torque_setpt") + self.data_logger.track_variable( + lambda: self.torque_setpoint, "peak_torque_setpt" + ) self.data_logger.track_variable(lambda: self.torque_command, "torque_cmd") - self.data_logger.track_variable(lambda: self.current_setpoint, "current_setpoint") + self.data_logger.track_variable( + lambda: self.current_setpoint, "current_setpoint" + ) - def pre_iterate(self)->None: + def pre_iterate(self) -> None: """ Check inbox for messages from GSE & GUI threads """ @@ -201,7 +217,7 @@ def peak_torque_update_monitor(self, key, value): if self.in_swing: setattr(self, key, value) - def iterate(self)->None: + def iterate(self) -> None: """ Main loop for the actuator thread. This method is called repeatedly in the thread's run loop. @@ -209,17 +225,19 @@ def iterate(self)->None: """ self.time_since_start = time.perf_counter() - self.thread_start_time - self.actuator.update() # update actuator states - self.data_logger.update() # update logger + self.actuator.update() # update actuator states + self.data_logger.update() # update logger # obtain time in current stride self.time_in_stride = time.perf_counter() - self.HS_time # acquire torque command based on gait estimate - self.torque_command = self.assistance_calculator.torque_generator(current_time=self.time_in_stride, - stride_period=self.stride_period, - peak_torque=float(self.torque_setpoint), - in_swing=self.in_swing) + self.torque_command = self.assistance_calculator.torque_generator( + current_time=self.time_in_stride, + stride_period=self.stride_period, + peak_torque=float(self.torque_setpoint), + in_swing=self.in_swing, + ) # determine appropriate current setpoint that matches the torque setpoint self.current_setpoint = self.actuator.torque_to_current(self.torque_command) @@ -228,54 +246,64 @@ def iterate(self)->None: if self.current_setpoint is not None: self.actuator.set_motor_current(self.current_setpoint) else: - self.data_logger.warning(f"Unable to command current for {self.actuator.tag}. Skipping.") + self.data_logger.warning( + f"Unable to command current for {self.actuator.tag}. Skipping." + ) - def post_iterate(self)->None: + def post_iterate(self) -> None: # send data to main thread for rtplotting try: - msg_router.send(sender=self.name, - recipient="main", - contents={"time_since_start":self.time_since_start, - "peak_torque_setpoint": self.torque_setpoint, - "torque_cmd": self.torque_command, - "current_setpoint": self.current_setpoint, - }) + msg_router.send( + sender=self.name, + recipient="main", + contents={ + # "time_since_start": self.time_since_start, + "peak_torque_setpoint": self.torque_setpoint, + "torque_cmd": self.torque_command, + "current_setpoint": self.current_setpoint, + }, + ) except: - self.data_logger.debug(f"UNABLE TO SEND msg to MAIN from {self.name} thread. Skipping.") - + self.data_logger.debug( + f"UNABLE TO SEND msg to MAIN from {self.name} thread. Skipping." + ) # TODO add rest of stuff if self.log_event.is_set(): self.data_logger.debug(f"[{self.name}] log_event True") - def on_pause(self)->None: + def on_pause(self) -> None: pass - from gse_bertec import Bertec_Estimator from src.exo.gait_state_estimator.forceplate.ZMQ_PubSub import Subscriber from src.settings.constants import IP_ADDRESSES, USE_SIMULATED_WALKER + class GaitStateEstimatorThread(BaseWorkerThread): """ Threading class for the Gait State Estimator. This class handles gait state estimation for BOTH exoskeletons/sides together. """ - def __init__(self, - msg_router, - quit_event:Type[threading.Event], - pause_event:Type[threading.Event], - log_event: Type[threading.Event], - active_actuators:list[str], - name:Optional[str] = None, - frequency:int=100)->None: - - super().__init__(quit_event, pause_event, log_event, name=name, frequency=frequency) + def __init__( + self, + msg_router, + quit_event: Type[threading.Event], + pause_event: Type[threading.Event], + log_event: Type[threading.Event], + active_actuators: list[str], + name: Optional[str] = None, + frequency: int = 100, + ) -> None: + + super().__init__( + quit_event, pause_event, log_event, name=name, frequency=frequency + ) self.msg_router = msg_router - self.inbox = None # will be instantiated + self.inbox = None # will be instantiated self.active_actuators = active_actuators self.bertec_estimators = {} @@ -293,18 +321,41 @@ def __init__(self, self.bertec_estimators[actuator] = walker # track vars for csv logging - self.data_logger.track_variable(lambda: self.time_since_start, f"{actuator}_time_since_start") - self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].stride_start_time, f"{actuator}_HS_time_") - self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].stride_period, f"{actuator}_stride_period") - self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].in_swing_flag, f"{actuator}_in_swing_flag_bool") - self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].current_time_in_stride, f"{actuator}_current_time_in_stride") - self.data_logger.track_variable(lambda: self.bertec_estimators[actuator].current_percent_gait_cycle, f"{actuator}_current_percent_gait_cycle") + self.data_logger.track_variable( + lambda: self.time_since_start, f"{actuator}_time_since_start" + ) + self.data_logger.track_variable( + lambda: self.bertec_estimators[actuator].stride_start_time, + f"{actuator}_HS_time_", + ) + self.data_logger.track_variable( + lambda: self.bertec_estimators[actuator].stride_period, + f"{actuator}_stride_period", + ) + self.data_logger.track_variable( + lambda: self.bertec_estimators[actuator].in_swing_flag, + f"{actuator}_in_swing_flag_bool", + ) + self.data_logger.track_variable( + lambda: self.bertec_estimators[actuator].current_time_in_stride, + f"{actuator}_current_time_in_stride", + ) + self.data_logger.track_variable( + lambda: self.bertec_estimators[actuator].current_percent_gait_cycle, + f"{actuator}_current_percent_gait_cycle", + ) else: - bertec_subscriber = Subscriber(publisher_ip=IP_ADDRESSES.VICON_IP, topic_filter=selected_topic, timeout_ms=5) - self.bertec_estimators[actuator] = Bertec_Estimator(zmq_subscriber=bertec_subscriber) - - def pre_iterate(self)->None: + bertec_subscriber = Subscriber( + publisher_ip=IP_ADDRESSES.VICON_IP, + topic_filter=selected_topic, + timeout_ms=5, + ) + self.bertec_estimators[actuator] = Bertec_Estimator( + zmq_subscriber=bertec_subscriber + ) + + def pre_iterate(self) -> None: pass def iterate(self): @@ -327,27 +378,33 @@ def iterate(self): # send message to actuator inboxes try: - self.data_logger.debug(self.bertec_estimators[actuator].return_estimate()) - - msg_router.send(sender=self.name, - recipient=actuator, - contents=self.bertec_estimators[actuator].return_estimate()) + self.data_logger.debug( + self.bertec_estimators[actuator].return_estimate() + ) + + msg_router.send( + sender=self.name, + recipient=actuator, + contents=self.bertec_estimators[actuator].return_estimate(), + ) except: - self.data_logger.debug(f"UNABLE TO SEND msg to '{actuator}' actuator from GaitStateEstimatorThread. Skipping.") + self.data_logger.debug( + f"UNABLE TO SEND msg to '{actuator}' actuator from GaitStateEstimatorThread. Skipping." + ) continue - def post_iterate(self)->None: + def post_iterate(self) -> None: pass - def on_pause(self)->None: + def on_pause(self) -> None: pass - import sys import select import random + class GUICommunication(BaseWorkerThread): """ Threading class to simulate GUI communication via gRPC. @@ -357,30 +414,33 @@ class GUICommunication(BaseWorkerThread): It then sends these setpoints to the actuators to handle. """ - def __init__(self, - msg_router, - quit_event:Type[threading.Event], - pause_event:Type[threading.Event], - log_event: Type[threading.Event], - active_actuators:list[str], - name:Optional[str] = None, - frequency:int=100)->None: - - - super().__init__(quit_event, pause_event, log_event, name=name, frequency=frequency) + def __init__( + self, + msg_router, + quit_event: Type[threading.Event], + pause_event: Type[threading.Event], + log_event: Type[threading.Event], + active_actuators: list[str], + name: Optional[str] = None, + frequency: int = 100, + ) -> None: + + super().__init__( + quit_event, pause_event, log_event, name=name, frequency=frequency + ) self.msg_router = msg_router self.inbox = None self.active_actuators = active_actuators - self.torque_setpoint:float = 0.0 + self.torque_setpoint: float = 0.0 - self.thread_start_time:float = time.perf_counter() - self.time_since_start:float = 0.0 + self.thread_start_time: float = time.perf_counter() + self.time_since_start: float = 0.0 # track vars for csv logging self.data_logger.track_variable(lambda: self.torque_setpoint, "torque_setpt") - def pre_iterate(self)->None: + def pre_iterate(self) -> None: """ Pre-iterate method to check for new messages in the mailbox. """ @@ -399,20 +459,24 @@ def iterate(self): self.data_logger.update() # set a random torque setpoint - self.torque_setpoint = 7 #random.randint(1,4)*10 + self.torque_setpoint = 7 # random.randint(1,4)*10 for actuator in self.active_actuators: try: - msg_router.send(sender=self.name, - recipient=actuator, - contents={"torque_setpoint": self.torque_setpoint}) + msg_router.send( + sender=self.name, + recipient=actuator, + contents={"torque_setpoint": self.torque_setpoint}, + ) except: - self.data_logger.debug(f"UNABLE TO SEND msg to actuator from GUICommunication thread. Skipping.") + self.data_logger.debug( + f"UNABLE TO SEND msg to actuator from GUICommunication thread. Skipping." + ) - def post_iterate(self)->None: + def post_iterate(self) -> None: pass - def on_pause(self)->None: + def on_pause(self) -> None: pass @@ -428,20 +492,20 @@ class ThreadManager: This class manages thread creation, communication and termination for the exoskeleton system. """ - def __init__(self, msg_router, actuators:Dict) -> None: + def __init__(self, msg_router, actuators: Dict) -> None: - self.actuators = actuators # Dictionary of Actuators - self.msg_router = msg_router # MessageRouter class instance + self.actuators = actuators # Dictionary of Actuators + self.msg_router = msg_router # MessageRouter class instance # create threading events common to all threads - self._quit_event = threading.Event() # Event to signal threads to quit. - self._pause_event = threading.Event() # Event to signal threads to pause. - self._log_event = threading.Event() # Event to signal threads to log + self._quit_event = threading.Event() # Event to signal threads to quit. + self._pause_event = threading.Event() # Event to signal threads to pause. + self._log_event = threading.Event() # Event to signal threads to log # initialize thread events - self._quit_event.set() # exo is running - self._pause_event.clear() # exo starts paused - self._log_event.clear() # exo starts not logging + self._quit_event.set() # exo is running + self._pause_event.clear() # exo starts paused + self._log_event.clear() # exo starts not logging # initialize dict of threads self._threads = {} @@ -464,7 +528,7 @@ def start(self) -> None: # creating 1 thread for GUI communication self.initialize_GUI_thread(active_actuators=self.actuators.keys()) - def start_all_threads(self)->None: + def start_all_threads(self) -> None: """ Start all threads in the thread manager. This method is called to start all threads after they have been initialized. @@ -489,25 +553,26 @@ def stop(self) -> None: # ensure time to enact stop method before moving on time.sleep(0.2) - def initialize_actuator_thread(self, actuator:DephyEB51Actuator) -> None: + def initialize_actuator_thread(self, actuator: DephyEB51Actuator) -> None: """ Create and start a thread for the specified actuator. This method is called to set up the actuator communication thread. """ - actuator_thread = ActuatorThread(actuator=actuator, - quit_event=self._quit_event, - pause_event=self._pause_event, - log_event=self._log_event, - name=f"{actuator.side}", - frequency=1000, - msg_router=self.msg_router, - ) + actuator_thread = ActuatorThread( + actuator=actuator, + quit_event=self._quit_event, + pause_event=self._pause_event, + log_event=self._log_event, + name=f"{actuator.side}", + frequency=1000, + msg_router=self.msg_router, + ) LOGGER.debug(f"created {actuator.side} actuator thread") self._threads[actuator.side] = actuator_thread - def initialize_GSE_thread(self, active_actuators:list[str]) -> None: + def initialize_GSE_thread(self, active_actuators: list[str]) -> None: """ Create and start the Gait State Estimator thread. This method is called to set up the GSE communication thread. @@ -518,18 +583,20 @@ def initialize_GSE_thread(self, active_actuators:list[str]) -> None: """ # create a FIFO queue with max size for inter-thread communication name = "gse" - gse_thread = GaitStateEstimatorThread(quit_event=self._quit_event, - pause_event=self._pause_event, - log_event=self._log_event, - active_actuators=active_actuators, - name=name, - frequency=250, - msg_router=self.msg_router) + gse_thread = GaitStateEstimatorThread( + quit_event=self._quit_event, + pause_event=self._pause_event, + log_event=self._log_event, + active_actuators=active_actuators, + name=name, + frequency=250, + msg_router=self.msg_router, + ) LOGGER.debug(f"created gse thread") self._threads[name] = gse_thread - def initialize_GUI_thread(self, active_actuators:list[str]) -> None: + def initialize_GUI_thread(self, active_actuators: list[str]) -> None: """ Create and start the GUI thread for user input. This method is called to set up the GUI communication thread. @@ -537,17 +604,19 @@ def initialize_GUI_thread(self, active_actuators:list[str]) -> None: # create a FIFO queue with max size for inter-thread communication name = "gui" - gui_thread = GUICommunication(quit_event=self._quit_event, - pause_event=self._pause_event, - log_event=self._log_event, - active_actuators=active_actuators, - name=name, - frequency=100, - msg_router=self.msg_router) + gui_thread = GUICommunication( + quit_event=self._quit_event, + pause_event=self._pause_event, + log_event=self._log_event, + active_actuators=active_actuators, + name=name, + frequency=100, + msg_router=self.msg_router, + ) LOGGER.debug(f"created gui thread") self._threads[name] = gui_thread - def return_active_threads(self)->list: + def return_active_threads(self) -> list: """ Return list of active thread addresses. """ @@ -633,7 +702,7 @@ def gui_thread(self) -> Optional[ActuatorThread]: return self._threads.get("gui") -class MainThreadMessageReception(): +class MainThreadMessageReception: def __init__(self, actuators): """ @@ -648,9 +717,23 @@ def __init__(self, actuators): self.actuators = actuators # define initial vars to be recepted - self.current_setpoint:int = 0 - self.torque_setpoint:float = 0.0 - self.torque_command:float = 0.0 + current_setpoint: int = 0 + torque_cmd: float = 0.0 + torque_setpoint: float = 0.0 + + recepted_vars = [current_setpoint, torque_cmd, torque_setpoint] + + for actuator in self.actuators.values(): + for var in recepted_vars: + setattr(self, f"{actuator}_{var}", 0) + + # self.left_current_setpoint: int = 0 + # self.left_torque_cmd: float = 0.0 + # self.left_peak_torque_setpoint: float = 0.0 + + # self.right_current_setpoint: int = 0 + # self.right_torque_cmd: float = 0.0 + # self.right_peak_torque_setpoint: float = 0.0 def check_msg_inbox(self): """ @@ -668,10 +751,12 @@ def decode_message(self, mail): Args: Mail: mail object """ + try: + sender = mail.sender # i.e., "left" or "right" for key, value in mail.contents.items(): - setattr(self, key, value) - + # e.g. store as left_torque_setpoint or right_torque_setpoint + setattr(self, f"{sender}_{key}", value) except Exception as err: LOGGER.debug(f"Error decoding message: {err}") @@ -686,45 +771,54 @@ def initialize_rt_plots(self) -> list: print("Active actuators:", active_sides_list) # pre-slice colors based on the number of active actuators - colors = ['r', 'b'][:len(active_sides_list)] + colors = ["r", "b"][: len(active_sides_list)] if len(active_sides_list) > len(colors): - raise ValueError("Not enough unique colors for the number of active actuators.") + raise ValueError( + "Not enough unique colors for the number of active actuators." + ) # repeat line styles and widths for each active actuator - line_styles = ['-' for _ in active_sides_list] + line_styles = ["-" for _ in active_sides_list] line_widths = [2 for _ in active_sides_list] - current_plt_config = {'names' : active_sides_list, - 'colors' : colors, - 'line_style': line_styles, - 'title' : "Exo Current (A) vs. Sample", - 'ylabel': "Current (A)", - 'xlabel': "timestep", - 'line_width': line_widths, - 'yrange': [0,30] - } - - torque_setpt_plt_config = {'names' : active_sides_list, - 'colors' : colors, - 'line_style': line_styles, - 'title' : "Exo Torque Setpt (Nm) vs. Sample", - 'ylabel': "Torque (Nm)", - 'xlabel': "timestep", - 'line_width': line_widths, - 'yrange': [0,30] - } - - torque_cmd_plt_config = {'names' : active_sides_list, - 'colors' : colors, - 'line_style': line_styles, - 'title' : "Exo Torque Cmds (A) vs. Sample", - 'ylabel': "Torque (Nm)", - 'xlabel': "timestep", - 'line_width': line_widths, - 'yrange': [0,30] - } - - plot_config = [current_plt_config, torque_setpt_plt_config, torque_cmd_plt_config] + current_plt_config = { + "names": active_sides_list, + "colors": colors, + "line_style": line_styles, + "title": "Exo Current (A) vs. Sample", + "ylabel": "Current (A)", + "xlabel": "timestep", + "line_width": line_widths, + "yrange": [0, 30], + } + + torque_setpt_plt_config = { + "names": active_sides_list, + "colors": colors, + "line_style": line_styles, + "title": "Exo Torque Setpt (Nm) vs. Sample", + "ylabel": "Torque (Nm)", + "xlabel": "timestep", + "line_width": line_widths, + "yrange": [0, 30], + } + + torque_cmd_plt_config = { + "names": active_sides_list, + "colors": colors, + "line_style": line_styles, + "title": "Exo Torque Cmds (A) vs. Sample", + "ylabel": "Torque (Nm)", + "xlabel": "timestep", + "line_width": line_widths, + "yrange": [0, 30], + } + + plot_config = [ + current_plt_config, + torque_setpt_plt_config, + torque_cmd_plt_config, + ] return plot_config @@ -738,11 +832,22 @@ def update_rt_plots(self) -> list: data_to_plt = [] for actuator in self.actuators.values(): - data_to_plt.extend([ - abs(actuator.motor_current), - actuator.torque_setpoint, - actuator.torque_command - ]) + if actuator == "left": + data_to_plt.extend( + [ + abs(self.left_current_setpoint), + self.left_torque_setpoint, + self.left_torque_cmd, + ] + ) + else: + data_to_plt.extend( + [ + abs(self.right_current_setpoint), + self.right_torque_setpoint, + self.right_torque_cmd, + ] + ) return data_to_plt @@ -756,17 +861,17 @@ def update_rt_plots(self) -> list: from rtplot import client from exoboots import DephyExoboots -if __name__ == '__main__': +if __name__ == "__main__": # create actuators & Exoboots Robot - actuators = create_actuators(gear_ratio=1, - baud_rate=EXO_SETUP_CONST.BAUD_RATE, - freq=EXO_SETUP_CONST.FLEXSEA_FREQ, - debug_level=EXO_SETUP_CONST.LOG_LEVEL) + actuators = create_actuators( + gear_ratio=1, + baud_rate=EXO_SETUP_CONST.BAUD_RATE, + freq=EXO_SETUP_CONST.FLEXSEA_FREQ, + debug_level=EXO_SETUP_CONST.LOG_LEVEL, + ) - exoboots = DephyExoboots(tag="exoboots", - actuators=actuators, - sensors={}) + exoboots = DephyExoboots(tag="exoboots", actuators=actuators, sensors={}) # set actuator modes & spool belts exoboots.setup_control_modes() @@ -787,11 +892,13 @@ def update_rt_plots(self) -> list: system_manager = ThreadManager(msg_router=msg_router, actuators=actuators) # instantiate soft real-time clock - clock = SoftRealtimeLoop(dt = 1 / 1) # Hz + clock = SoftRealtimeLoop(dt=1 / 1) # Hz with system_manager: # set-up addressbook for the PostOffice & create inboxes for each thread - msg_router.setup_addressbook(*system_manager.return_active_threads(), main_thread_receptor) + msg_router.setup_addressbook( + *system_manager.return_active_threads(), main_thread_receptor + ) # start all threads system_manager.start_all_threads() @@ -803,12 +910,10 @@ def update_rt_plots(self) -> list: try: # decode messages from actuator threads - # send data to server & update real-time plots data_to_plt = main_thread_receptor.update_rt_plots() client.send_array(data_to_plt) - except KeyboardInterrupt: print("KeyboardInterrupt received.") break From efbc46d1aa18425687d1f5bb1a1923c8a8da0714 Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Mon, 23 Jun 2025 16:50:37 +0100 Subject: [PATCH 22/27] tested and verified that main message receptor now recieves messages from Actuator thread and appropriately plots them to the rtplot. tbd: centralized logging for logging nexus. may have to be careful about delays in queue communication between left vs right. --- dephyEB51.py | 2 - src/utils/walking_simulator.py | 4 +- threading_demo.py | 118 +++++++++++++++------------------ 3 files changed, 55 insertions(+), 69 deletions(-) diff --git a/dephyEB51.py b/dephyEB51.py index 6facb8a..70e47e4 100644 --- a/dephyEB51.py +++ b/dephyEB51.py @@ -168,9 +168,7 @@ def update(self): self.update_gear_ratio() # update gait state estimate - print("about to update IMU estimate") self.update_imu_gait_state(self.accelz) - print("updated IMU estimate") def assign_id_to_side(self)-> str: """ diff --git a/src/utils/walking_simulator.py b/src/utils/walking_simulator.py index 0f8ec84..f271c72 100644 --- a/src/utils/walking_simulator.py +++ b/src/utils/walking_simulator.py @@ -100,11 +100,11 @@ def update_time_in_stride(self)-> float: self.stride_start_time = time.perf_counter() self.stride_num += 1 - print(f'{self.stride_num} stride(s) completed') + # print(f'{self.stride_num} stride(s) completed') elif self.current_time_in_stride < self.stride_period: self.current_time_in_stride = time.perf_counter() - self.stride_start_time - print(f"time in curr stride: {self.current_time_in_stride:.3f}") + # print(f"time in curr stride: {self.current_time_in_stride:.3f}") return self.current_time_in_stride diff --git a/threading_demo.py b/threading_demo.py index d0307c2..3d8c19c 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -157,7 +157,7 @@ def __init__( self.HS_time: float = 0.0 self.stride_period: float = 1.2 self.in_swing: bool = True - self.torque_setpoint: float = 0.0 + self.peak_torque_setpoint: float = 0.0 self.torque_command: float = 0.0 self.current_setpoint: int = 0 self.time_in_stride: float = 0.0 @@ -173,9 +173,9 @@ def __init__( self.data_logger.track_variable(lambda: self.stride_period, "stride_period") self.data_logger.track_variable(lambda: self.in_swing, "in_swing_flag_bool") self.data_logger.track_variable( - lambda: self.torque_setpoint, "peak_torque_setpt" + lambda: self.peak_torque_setpoint, "peak_torque_setpt" ) - self.data_logger.track_variable(lambda: self.torque_command, "torque_cmd") + self.data_logger.track_variable(lambda: self.torque_command, "torque_command") self.data_logger.track_variable( lambda: self.current_setpoint, "current_setpoint" ) @@ -196,7 +196,7 @@ def decode_message(self, mail): """ try: for key, value in mail.contents.items(): - if CONTINUOUS_MODE_FLAG and key == "torque_setpoint": + if CONTINUOUS_MODE_FLAG and key == "peak_torque_setpoint": self.peak_torque_update_monitor(key, value) else: setattr(self, key, value) @@ -235,7 +235,7 @@ def iterate(self) -> None: self.torque_command = self.assistance_calculator.torque_generator( current_time=self.time_in_stride, stride_period=self.stride_period, - peak_torque=float(self.torque_setpoint), + peak_torque=float(self.peak_torque_setpoint), in_swing=self.in_swing, ) @@ -244,7 +244,8 @@ def iterate(self) -> None: # command appropriate current setpoint using DephyExoboots class if self.current_setpoint is not None: - self.actuator.set_motor_current(self.current_setpoint) + # self.actuator.set_motor_current(self.current_setpoint) + pass else: self.data_logger.warning( f"Unable to command current for {self.actuator.tag}. Skipping." @@ -258,11 +259,12 @@ def post_iterate(self) -> None: recipient="main", contents={ # "time_since_start": self.time_since_start, - "peak_torque_setpoint": self.torque_setpoint, - "torque_cmd": self.torque_command, + "peak_torque_setpoint": self.peak_torque_setpoint, + "torque_command": self.torque_command, "current_setpoint": self.current_setpoint, }, ) + except: self.data_logger.debug( f"UNABLE TO SEND msg to MAIN from {self.name} thread. Skipping." @@ -432,13 +434,13 @@ def __init__( self.msg_router = msg_router self.inbox = None self.active_actuators = active_actuators - self.torque_setpoint: float = 0.0 + self.peak_torque_setpoint: float = 0.0 self.thread_start_time: float = time.perf_counter() self.time_since_start: float = 0.0 # track vars for csv logging - self.data_logger.track_variable(lambda: self.torque_setpoint, "torque_setpt") + self.data_logger.track_variable(lambda: self.peak_torque_setpoint, "torque_setpt") def pre_iterate(self) -> None: """ @@ -459,14 +461,14 @@ def iterate(self): self.data_logger.update() # set a random torque setpoint - self.torque_setpoint = 7 # random.randint(1,4)*10 + self.peak_torque_setpoint = 7 # random.randint(1,4)*10 for actuator in self.active_actuators: try: msg_router.send( sender=self.name, recipient=actuator, - contents={"torque_setpoint": self.torque_setpoint}, + contents={"peak_torque_setpoint": self.peak_torque_setpoint}, ) except: self.data_logger.debug( @@ -565,7 +567,7 @@ def initialize_actuator_thread(self, actuator: DephyEB51Actuator) -> None: pause_event=self._pause_event, log_event=self._log_event, name=f"{actuator.side}", - frequency=1000, + frequency=500, msg_router=self.msg_router, ) @@ -610,7 +612,7 @@ def initialize_GUI_thread(self, active_actuators: list[str]) -> None: log_event=self._log_event, active_actuators=active_actuators, name=name, - frequency=100, + frequency=1, msg_router=self.msg_router, ) LOGGER.debug(f"created gui thread") @@ -716,25 +718,17 @@ def __init__(self, actuators): self.inbox = None self.actuators = actuators - # define initial vars to be recepted - current_setpoint: int = 0 - torque_cmd: float = 0.0 - torque_setpoint: float = 0.0 - - recepted_vars = [current_setpoint, torque_cmd, torque_setpoint] + # define list of vars to be recepted + recepted_vars = ["peak_torque_setpoint", + "torque_command", + "current_setpoint" + ] - for actuator in self.actuators.values(): + # loop through actuator names & assign vars as attributes of class + for actuator in self.actuators.keys(): for var in recepted_vars: setattr(self, f"{actuator}_{var}", 0) - # self.left_current_setpoint: int = 0 - # self.left_torque_cmd: float = 0.0 - # self.left_peak_torque_setpoint: float = 0.0 - - # self.right_current_setpoint: int = 0 - # self.right_torque_cmd: float = 0.0 - # self.right_peak_torque_setpoint: float = 0.0 - def check_msg_inbox(self): """ Main thread checks it's message inbox. @@ -755,8 +749,9 @@ def decode_message(self, mail): try: sender = mail.sender # i.e., "left" or "right" for key, value in mail.contents.items(): - # e.g. store as left_torque_setpoint or right_torque_setpoint - setattr(self, f"{sender}_{key}", value) + # e.g. store as left_peak_torque_setpoint or right_peak_torque_setpoint + setattr(self, f"{sender}_{key}", value/1000 if key == "current_setpoint" else value) + except Exception as err: LOGGER.debug(f"Error decoding message: {err}") @@ -781,44 +776,42 @@ def initialize_rt_plots(self) -> list: line_styles = ["-" for _ in active_sides_list] line_widths = [2 for _ in active_sides_list] - current_plt_config = { + torque_setpt_plt_config = { "names": active_sides_list, "colors": colors, "line_style": line_styles, - "title": "Exo Current (A) vs. Sample", - "ylabel": "Current (A)", + "title": "Exo Torque Setpt (Nm) vs. Sample", + "ylabel": "Torque (Nm)", "xlabel": "timestep", "line_width": line_widths, - "yrange": [0, 30], + "yrange": [0, 45], } - torque_setpt_plt_config = { + torque_cmd_plt_config = { "names": active_sides_list, "colors": colors, "line_style": line_styles, - "title": "Exo Torque Setpt (Nm) vs. Sample", + "title": "Exo Torque Cmds (A) vs. Sample", "ylabel": "Torque (Nm)", "xlabel": "timestep", "line_width": line_widths, - "yrange": [0, 30], + "yrange": [0, 45], } - torque_cmd_plt_config = { + current_plt_config = { "names": active_sides_list, "colors": colors, "line_style": line_styles, - "title": "Exo Torque Cmds (A) vs. Sample", - "ylabel": "Torque (Nm)", + "title": "Exo Current (A) vs. Sample", + "ylabel": "Current (A)", "xlabel": "timestep", "line_width": line_widths, "yrange": [0, 30], } - plot_config = [ - current_plt_config, - torque_setpt_plt_config, - torque_cmd_plt_config, - ] + plot_config = [torque_setpt_plt_config, + torque_cmd_plt_config, + current_plt_config] return plot_config @@ -831,23 +824,17 @@ def update_rt_plots(self) -> list: """ data_to_plt = [] - for actuator in self.actuators.values(): - if actuator == "left": - data_to_plt.extend( - [ - abs(self.left_current_setpoint), - self.left_torque_setpoint, - self.left_torque_cmd, - ] - ) - else: - data_to_plt.extend( - [ - abs(self.right_current_setpoint), - self.right_torque_setpoint, - self.right_torque_cmd, - ] - ) + + # looping through actuator names, plot data that is sent from actuator thread + # does not contain actual motor currents in response to commands (just what's commanded) + for actuator in self.actuators.keys(): + data_to_plt.append(getattr(self, f"{actuator}_peak_torque_setpoint")) + + for actuator in self.actuators.keys(): + data_to_plt.append(getattr(self, f"{actuator}_torque_command")) + + for actuator in self.actuators.keys(): + data_to_plt.append(abs(getattr(self, f"{actuator}_current_setpoint"))) return data_to_plt @@ -875,7 +862,7 @@ def update_rt_plots(self) -> list: # set actuator modes & spool belts exoboots.setup_control_modes() - exoboots.spool_belts() + # exoboots.spool_belts() # initialize class responsible for recieving data & rtplotting for the main thread main_thread_receptor = MainThreadMessageReception(actuators) @@ -892,7 +879,7 @@ def update_rt_plots(self) -> list: system_manager = ThreadManager(msg_router=msg_router, actuators=actuators) # instantiate soft real-time clock - clock = SoftRealtimeLoop(dt=1 / 1) # Hz + clock = SoftRealtimeLoop(dt=1 / 200) # Hz with system_manager: # set-up addressbook for the PostOffice & create inboxes for each thread @@ -909,6 +896,7 @@ def update_rt_plots(self) -> list: for t in clock: try: # decode messages from actuator threads + main_thread_receptor.check_msg_inbox() # send data to server & update real-time plots data_to_plt = main_thread_receptor.update_rt_plots() From ea7402003cac2afbc1fbbb205126a75714c9b269 Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Mon, 23 Jun 2025 22:26:10 +0100 Subject: [PATCH 23/27] Bertec_Estimator fixes and changes. Added TIME_METHOD for consistency across all classes (GSE_IMU, GSE_BERTEC). Re-added acceptance tresholding for new stride periods (more lenient 0.2 -> 1.0). Tested full on bertec with streaming with Assistance_Calculator. --- dephyEB51.py | 20 ++-- gse_bertec.py | 23 +++- gse_imu.py | 63 ++++++++-- src/settings/constants.py | 10 +- src/settings/constants_dataclasses.py | 2 - threading_demo.py | 161 ++++++++++++++++++-------- 6 files changed, 205 insertions(+), 74 deletions(-) diff --git a/dephyEB51.py b/dephyEB51.py index 70e47e4..456e1d8 100644 --- a/dephyEB51.py +++ b/dephyEB51.py @@ -125,13 +125,6 @@ def update_gear_ratio(self)-> None: return self._gear_ratio - def update_imu_gait_state(self, accelz:float): - """" - Update gait state using imu based thresholding - """ - - self.activation = self.imu_estimator.update(accelz) - # TODO: recharacterize transmission ratio without ENC_CLICKS_TO_DEG constant. That constant is redundant since Dephy already reports in degrees @property def ankle_angle(self) -> float: @@ -168,7 +161,18 @@ def update(self): self.update_gear_ratio() # update gait state estimate - self.update_imu_gait_state(self.accelz) + self.imu_estimator.update(self.accelz, self.ankle_angle) + + def imu_gait_state_estimate(self, var:str)->dict: + """ + Returns the IMU's gait state estimate + + Args: + - desired variable from imu estimate to return + """ + state_dict = self.imu_estimator.return_estimate() + + return state_dict[var] def assign_id_to_side(self)-> str: """ diff --git a/gse_bertec.py b/gse_bertec.py index 3e1d155..c61afba 100644 --- a/gse_bertec.py +++ b/gse_bertec.py @@ -1,12 +1,13 @@ import time - from src.utils.filter_utils import MovingAverageFilter +from src.settings.constants import TIME_METHOD, BERTEC_THRESH + class Bertec_Estimator: """ Stride phase estimation using forceplate thresholding """ - def __init__(self, zmq_subscriber, stride_period_init=1.2, filter_size=10, hs_threshold = 80, to_threshold = 30): + def __init__(self, zmq_subscriber, stride_period_init=1.2, filter_size=10, hs_threshold = 80, to_threshold = 30, time_method=TIME_METHOD): # ZMQ subscriber self.subscriber = zmq_subscriber @@ -14,6 +15,9 @@ def __init__(self, zmq_subscriber, stride_period_init=1.2, filter_size=10, hs_th self.hs_threshold = hs_threshold self.to_threshold = to_threshold + # Time Method + self.time_method = time_method + # State variables self.HS = 0 self.TO = 0 @@ -30,6 +34,7 @@ def return_estimate(self): c) in swing """ + state_dict = {"HS_time": self.HS, "stride_period": self.stride_period_tracker.average(), "in_swing": not self.in_contact @@ -44,6 +49,11 @@ def update(self): a) if new stride has been observed b) force from Bertec """ + # TODO add pause event + # if not pause_event: + # updatefilters = True + # else: + # updatefilters = pause_event.is_set() # ZMQ streaming get message topic, force, timestep_valid = self.subscriber.get_message() @@ -59,7 +69,7 @@ def update(self): if force < self.to_threshold: # New Toe off self.in_contact = False - self.TO = time.time() + self.TO = self.time_method() else: # In stance @@ -72,9 +82,12 @@ def update(self): new_stride = True # Record new stride period and update estimate - HS_new = time.time() + HS_new = self.time_method() stride_period_new = HS_new - self.HS - self.stride_period_tracker.update(stride_period_new) + stride_period_avg = self.stride_period_tracker.average() + + if abs((stride_period_new - stride_period_avg) / stride_period_avg) < BERTEC_THRESH.ACCEPT_STRIDE_THRESHOLD: #TODO do when pause_event and updatefilters: + self.stride_period_tracker.update(stride_period_new) self.HS = HS_new diff --git a/gse_imu.py b/gse_imu.py index 42d4270..fe71242 100644 --- a/gse_imu.py +++ b/gse_imu.py @@ -1,18 +1,28 @@ import time from math import sqrt +from src.utils.filter_utils import MovingAverageFilter +from src.settings.constants import TIME_METHOD class IMU_Estimator: """ IMU_Estimator estimates activation events from onboard exoboot IMU acceleration data (z-axis) using real-time mean/std tracking and z-score thresholding. + + Outputs: + - stride_period: time between two heel-strike activations + - in_swing: flag that indicates that foot is in swing + - HS_time: timestamp of most recent activation (heel strike) + """ def __init__( self, std_threshold: float = 2, run_len_threshold: int = 10, - time_method: float = time.time, + time_method: float = TIME_METHOD, + stride_period_init: float = 1.20, + filter_size:int = 10 ): """ Initialize the IMU_Estimator. @@ -20,7 +30,7 @@ def __init__( Args: std_threshold (float): Z-score threshold for activation detection. run_len_threshold (int): Number of consecutive samples below threshold to end activation. - time_method (callable): Function to get the current time (default: time.time). + time_method (callable): Function to get the current time (default: TIME_METHOD). """ self.std_threhold = std_threshold @@ -46,6 +56,10 @@ def __init__( self.time_method = time_method + self.HS_time:float = self.time_method() + self.in_swing:bool = True + self.stride_period_tracker = MovingAverageFilter(initial_value=stride_period_init, size=filter_size) + def __repr__(self): """ Return a string representation of the estimator's current state. @@ -67,11 +81,17 @@ def return_estimate(self): dict: Dictionary with the current activation state. """ - state_dict = {"activation": self.activation_state} + # state_dict = {"activation": self.activation_state} + + state_dict = {"HS_time": self.HS_time, + "stride_period": self.stride_period_tracker.average(), + "in_swing": self.in_swing, + "activation": self.activation_state + } return state_dict - def update(self, accel: float) -> dict: + def update(self, accel:float, ank_ang:float): """ Update the estimator with a new acceleration value, compute statistics, and manage activation state. @@ -121,9 +141,33 @@ def update(self, accel: float) -> dict: else: pass - # self.activations_status.append(self.activation_state) + self.detect_which_gait_event(ank_ang) - return self.activation_state + def detect_which_gait_event(self, ank_ang:float): + """ + Detects gait event depending on activation state. + + Updates the last heel strike time, stride period and in_swing flag + """ + + # TODO: ensure that only the first activation is detected as heel strike + # if some gait event registered + if self.activation_state: + # check if heel strike event + if self.in_swing and (ank_ang > 20) and (ank_ang < 40): + self.in_swing = False # now in stance, i.e. heel strike just occured + self.latest_HS = self.time_method() # record the latest heel strike time + latest_stride_period = self.latest_HS - self.HS_time # compute the latest stride period + self.stride_period_tracker.update(latest_stride_period) # update the stride period estimate + + self.HS_time = self.latest_HS + + # check if toe-off event + elif (self.in_swing==False) and (ank_ang > 60): + self.in_swing = True # now in swing, i.e. toe-off just occured + + else: + pass if __name__ == "__main__": @@ -132,8 +176,9 @@ def update(self, accel: float) -> dict: print(asdf) for i in range(20): - asdf.update(i) - print(asdf) + asdf.update(i, i+20) + asdf.return_estimate() + print(asdf.return_estimate()) - asdf.update(100) + asdf.update(100, 60) print(asdf) diff --git a/src/settings/constants.py b/src/settings/constants.py index 7037cdb..67d6e81 100644 --- a/src/settings/constants.py +++ b/src/settings/constants.py @@ -1,6 +1,7 @@ """ Collection of all the constants used throughout exoboot controller """ +import time from .constants_dataclasses import ( SPLINE_PARAMS, @@ -127,8 +128,7 @@ BERTEC_THRESH = BERTEC_THRESHOLDS( HS_THRESHOLD=80, TO_THRESHOLD=30, - ACCEPT_STRIDE_THRESHOLD=0.2, - ACCEPT_STANCE_THRESHOLD=0.2, + ACCEPT_STRIDE_THRESHOLD=1, BERTEC_ACC_LEFT=0.25, BERTEC_ACC_RIGHT=0.25, ) @@ -139,4 +139,8 @@ """ Gait State Estimation Toggle""" RUN_BERTEC_GSE:bool = True # if true, an additional thread will be spun up that obtains bertec gait state estimates -USE_SIMULATED_WALKER:bool = True # if true, a simulated walker will output gait state estimates INSTEAD of the bertec in the thread \ No newline at end of file +USE_SIMULATED_WALKER:bool = False # if true, a simulated walker will output gait state estimates INSTEAD of the bertec in the thread + + +"""" TIME METHOD """ +TIME_METHOD = time.perf_counter diff --git a/src/settings/constants_dataclasses.py b/src/settings/constants_dataclasses.py index 275b7aa..2d8fb70 100644 --- a/src/settings/constants_dataclasses.py +++ b/src/settings/constants_dataclasses.py @@ -175,7 +175,6 @@ class BERTEC_THRESHOLDS: ... HS_THRESHOLD = 15, % threshold to detect heel strike (N) ... TO_THRESHOLD = 50, % threshold to detect toe off (N) ... ACCEPT_STRIDE_THRESHOLD = 0.2, % acceptable stride threshold (sec) - ... ACCEPT_STANCE_THRESHOLD = 0.2, % acceptable stance threshold (sec) ... BERTEC_ACC_LEFT = 0.2 % left tread acceleration ... BERTEC_ACC_RIGHT = 0.2 % right tread acceleration ... ) @@ -187,7 +186,6 @@ class BERTEC_THRESHOLDS: TO_THRESHOLD:int ACCEPT_STRIDE_THRESHOLD:float - ACCEPT_STANCE_THRESHOLD:float BERTEC_ACC_LEFT:float BERTEC_ACC_RIGHT:float diff --git a/threading_demo.py b/threading_demo.py index 3d8c19c..34afa60 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -179,6 +179,10 @@ def __init__( self.data_logger.track_variable( lambda: self.current_setpoint, "current_setpoint" ) + # track gse_imu vars + self.data_logger.track_variable(lambda: self.actuator.imu_gait_state_estimate("HS_time"), "HS_time_imu") + self.data_logger.track_variable(lambda: self.actuator.imu_gait_state_estimate("stride_period"), "stride_period_imu") + self.data_logger.track_variable(lambda: self.actuator.imu_gait_state_estimate("in_swing"), "in_swing_imu") def pre_iterate(self) -> None: """ @@ -225,8 +229,8 @@ def iterate(self) -> None: """ self.time_since_start = time.perf_counter() - self.thread_start_time - self.actuator.update() # update actuator states - self.data_logger.update() # update logger + self.actuator.update() # update actuator states + self.data_logger.update() # update logger # obtain time in current stride self.time_in_stride = time.perf_counter() - self.HS_time @@ -244,8 +248,8 @@ def iterate(self) -> None: # command appropriate current setpoint using DephyExoboots class if self.current_setpoint is not None: - # self.actuator.set_motor_current(self.current_setpoint) - pass + self.actuator.set_motor_current(self.current_setpoint) + # pass else: self.data_logger.warning( f"Unable to command current for {self.actuator.tag}. Skipping." @@ -262,6 +266,8 @@ def post_iterate(self) -> None: "peak_torque_setpoint": self.peak_torque_setpoint, "torque_command": self.torque_command, "current_setpoint": self.current_setpoint, + "motor_current": self.actuator.motor_current, + "activation_status": self.actuator.imu_gait_state_estimate("activation") }, ) @@ -317,49 +323,67 @@ def __init__( for actuator in self.active_actuators: selected_topic = f"fz_{actuator}" # e.g., 'fz_left' or 'fz_right' - if USE_SIMULATED_WALKER: - walker = WalkingSimulator(stride_period=1.20) - walker.set_percent_toe_off(67) - self.bertec_estimators[actuator] = walker - - # track vars for csv logging - self.data_logger.track_variable( - lambda: self.time_since_start, f"{actuator}_time_since_start" - ) - self.data_logger.track_variable( - lambda: self.bertec_estimators[actuator].stride_start_time, - f"{actuator}_HS_time_", - ) - self.data_logger.track_variable( - lambda: self.bertec_estimators[actuator].stride_period, - f"{actuator}_stride_period", - ) - self.data_logger.track_variable( - lambda: self.bertec_estimators[actuator].in_swing_flag, - f"{actuator}_in_swing_flag_bool", - ) - self.data_logger.track_variable( - lambda: self.bertec_estimators[actuator].current_time_in_stride, - f"{actuator}_current_time_in_stride", - ) - self.data_logger.track_variable( - lambda: self.bertec_estimators[actuator].current_percent_gait_cycle, - f"{actuator}_current_percent_gait_cycle", - ) + # if USE_SIMULATED_WALKER: + # walker = WalkingSimulator(stride_period=1.20) + # walker.set_percent_toe_off(67) + # self.bertec_estimators[actuator] = walker + + # # track vars for csv logging + # self.data_logger.track_variable( + # lambda: self.time_since_start, f"{actuator}_time_since_start" + # ) + # self.data_logger.track_variable( + # lambda: self.bertec_estimators[actuator].stride_start_time, + # f"{actuator}_HS_time_", + # ) + # self.data_logger.track_variable( + # lambda: self.bertec_estimators[actuator].stride_period, + # f"{actuator}_stride_period", + # ) + # self.data_logger.track_variable( + # lambda: self.bertec_estimators[actuator].in_swing_flag, + # f"{actuator}_in_swing_flag_bool", + # ) + # self.data_logger.track_variable( + # lambda: self.bertec_estimators[actuator].current_time_in_stride, + # f"{actuator}_current_time_in_stride", + # ) + # self.data_logger.track_variable( + # lambda: self.bertec_estimators[actuator].current_percent_gait_cycle, + # f"{actuator}_current_percent_gait_cycle", + # ) + + # else: + bertec_subscriber = Subscriber( + publisher_ip=IP_ADDRESSES.VICON_IP, + topic_filter=selected_topic, + timeout_ms=5, + ) + self.bertec_estimators[actuator] = Bertec_Estimator( + zmq_subscriber=bertec_subscriber + ) - else: - bertec_subscriber = Subscriber( - publisher_ip=IP_ADDRESSES.VICON_IP, - topic_filter=selected_topic, - timeout_ms=5, - ) - self.bertec_estimators[actuator] = Bertec_Estimator( - zmq_subscriber=bertec_subscriber - ) + # # track vars for csv logging + # self.data_logger.track_variable( + # lambda: self.time_since_start, f"{actuator}_time_since_start" + # ) + # self.data_logger.track_variable( + # lambda: self.bertec_estimators[actuator].HS, + # f"{actuator}_HS_time_", + # ) + # self.data_logger.track_variable( + # lambda: self.bertec_estimators[actuator].stride_period_tracker.average(), + # f"{actuator}_stride_period", + # ) + # self.data_logger.track_variable( + # lambda: not self.bertec_estimators[actuator].in_contact, + # f"{actuator}_in_swing_flag_bool", + # ) def pre_iterate(self) -> None: pass + def iterate(self): """ Main loop for the actuator thread. @@ -372,8 +396,9 @@ def iterate(self): # for each active actuator, for actuator in self.active_actuators: - # TODO: update the gait state estimator for the actuator + # Update the gait state estimator for the actuator self.bertec_estimators[actuator].update() + print(f"asdf {actuator}: {self.bertec_estimators[actuator].return_estimate()}") # update csv logger self.data_logger.update() @@ -461,7 +486,7 @@ def iterate(self): self.data_logger.update() # set a random torque setpoint - self.peak_torque_setpoint = 7 # random.randint(1,4)*10 + self.peak_torque_setpoint = 15.0 #random.randint(2,4)*10 for actuator in self.active_actuators: try: @@ -721,7 +746,9 @@ def __init__(self, actuators): # define list of vars to be recepted recepted_vars = ["peak_torque_setpoint", "torque_command", - "current_setpoint" + "current_setpoint", + "motor_current", + "activation_status" ] # loop through actuator names & assign vars as attributes of class @@ -750,7 +777,10 @@ def decode_message(self, mail): sender = mail.sender # i.e., "left" or "right" for key, value in mail.contents.items(): # e.g. store as left_peak_torque_setpoint or right_peak_torque_setpoint - setattr(self, f"{sender}_{key}", value/1000 if key == "current_setpoint" else value) + if key == "current_setpoint": + setattr(self, f"{sender}_{key}", value/1000) + else: + setattr(self, f"{sender}_{key}", value) except Exception as err: LOGGER.debug(f"Error decoding message: {err}") @@ -809,9 +839,34 @@ def initialize_rt_plots(self) -> list: "yrange": [0, 30], } + mot_current_plt_config = { + "names": active_sides_list, + "colors": colors, + "line_style": line_styles, + "title": "Exo Motor Current (A) vs. Sample", + "ylabel": "Motor Current (A)", + "xlabel": "timestep", + "line_width": line_widths, + "yrange": [0, 30], + } + + activation_plt_config = { + "names": active_sides_list, + "colors": colors, + "line_style": line_styles, + "title": "Activation vs. Sample", + "ylabel": "Gait Event Activation", + "xlabel": "timestep", + "line_width": line_widths, + "yrange": [0, 1], + } + + plot_config = [torque_setpt_plt_config, torque_cmd_plt_config, - current_plt_config] + current_plt_config, + mot_current_plt_config, + activation_plt_config] return plot_config @@ -836,6 +891,12 @@ def update_rt_plots(self) -> list: for actuator in self.actuators.keys(): data_to_plt.append(abs(getattr(self, f"{actuator}_current_setpoint"))) + for actuator in self.actuators.keys(): + data_to_plt.append(abs(getattr(self, f"{actuator}_motor_current"))) + + for actuator in self.actuators.keys(): + data_to_plt.append(abs(getattr(self, f"{actuator}_activation_status"))) + return data_to_plt @@ -849,6 +910,12 @@ def update_rt_plots(self) -> list: from exoboots import DephyExoboots if __name__ == "__main__": + # import time + # time1 = time.perf_counter() + # time2 = time.time() + + # print("blah: ", time1 - time2) + # time.sleep(10) # create actuators & Exoboots Robot actuators = create_actuators( From ebb012ad116d930312da4e43b884418bbb3a4226 Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Mon, 23 Jun 2025 22:46:06 +0100 Subject: [PATCH 24/27] random changes --- gse_bertec.py | 3 ++- threading_demo.py | 3 +++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/gse_bertec.py b/gse_bertec.py index c61afba..3d2a02a 100644 --- a/gse_bertec.py +++ b/gse_bertec.py @@ -86,7 +86,8 @@ def update(self): stride_period_new = HS_new - self.HS stride_period_avg = self.stride_period_tracker.average() - if abs((stride_period_new - stride_period_avg) / stride_period_avg) < BERTEC_THRESH.ACCEPT_STRIDE_THRESHOLD: #TODO do when pause_event and updatefilters: + # Make sure new stride is "reasonable" + if abs((stride_period_new - stride_period_avg) / stride_period_avg) < BERTEC_THRESH.ACCEPT_STRIDE_THRESHOLD: # TODO do when pause_event and updatefilters: self.stride_period_tracker.update(stride_period_new) self.HS = HS_new diff --git a/threading_demo.py b/threading_demo.py index 34afa60..4bb6e6c 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -235,6 +235,9 @@ def iterate(self) -> None: # obtain time in current stride self.time_in_stride = time.perf_counter() - self.HS_time + # TODO add delay compensation + # + # acquire torque command based on gait estimate self.torque_command = self.assistance_calculator.torque_generator( current_time=self.time_in_stride, From e2206211f020cbb0c3563b671cdae9f1ae7b7f57 Mon Sep 17 00:00:00 2001 From: Nundini Date: Tue, 24 Jun 2025 11:36:23 -0400 Subject: [PATCH 25/27] added .mat file for gse_imu tuning and also added to gitignore --- .gitignore | 3 ++- gse_imu.py | 48 ++++++++++++++++++++++++++++++------------------ 2 files changed, 32 insertions(+), 19 deletions(-) diff --git a/.gitignore b/.gitignore index b5d8b8e..25c8d81 100644 --- a/.gitignore +++ b/.gitignore @@ -2,4 +2,5 @@ *.pyc *.log py39-venv/ -py311-venv/ \ No newline at end of file +py311-venv/ +*.mat \ No newline at end of file diff --git a/gse_imu.py b/gse_imu.py index fe71242..c9fa8e1 100644 --- a/gse_imu.py +++ b/gse_imu.py @@ -4,6 +4,7 @@ from src.settings.constants import TIME_METHOD + class IMU_Estimator: """ IMU_Estimator estimates activation events from onboard exoboot IMU acceleration data (z-axis) @@ -22,7 +23,7 @@ def __init__( run_len_threshold: int = 10, time_method: float = TIME_METHOD, stride_period_init: float = 1.20, - filter_size:int = 10 + filter_size: int = 10, ): """ Initialize the IMU_Estimator. @@ -56,9 +57,11 @@ def __init__( self.time_method = time_method - self.HS_time:float = self.time_method() - self.in_swing:bool = True - self.stride_period_tracker = MovingAverageFilter(initial_value=stride_period_init, size=filter_size) + self.HS_time: float = self.time_method() + self.in_swing: bool = True + self.stride_period_tracker = MovingAverageFilter( + initial_value=stride_period_init, size=filter_size + ) def __repr__(self): """ @@ -83,15 +86,16 @@ def return_estimate(self): # state_dict = {"activation": self.activation_state} - state_dict = {"HS_time": self.HS_time, - "stride_period": self.stride_period_tracker.average(), - "in_swing": self.in_swing, - "activation": self.activation_state - } + state_dict = { + "HS_time": self.HS_time, + "stride_period": self.stride_period_tracker.average(), + "in_swing": self.in_swing, + "activation": self.activation_state, + } return state_dict - def update(self, accel:float, ank_ang:float): + def update(self, accel: float, ank_ang: float): """ Update the estimator with a new acceleration value, compute statistics, and manage activation state. @@ -143,7 +147,7 @@ def update(self, accel:float, ank_ang:float): self.detect_which_gait_event(ank_ang) - def detect_which_gait_event(self, ank_ang:float): + def detect_which_gait_event(self, ank_ang: float): """ Detects gait event depending on activation state. @@ -155,16 +159,22 @@ def detect_which_gait_event(self, ank_ang:float): if self.activation_state: # check if heel strike event if self.in_swing and (ank_ang > 20) and (ank_ang < 40): - self.in_swing = False # now in stance, i.e. heel strike just occured - self.latest_HS = self.time_method() # record the latest heel strike time - latest_stride_period = self.latest_HS - self.HS_time # compute the latest stride period - self.stride_period_tracker.update(latest_stride_period) # update the stride period estimate + self.in_swing = False # now in stance, i.e. heel strike just occured + self.latest_HS = ( + self.time_method() + ) # record the latest heel strike time + latest_stride_period = ( + self.latest_HS - self.HS_time + ) # compute the latest stride period + self.stride_period_tracker.update( + latest_stride_period + ) # update the stride period estimate self.HS_time = self.latest_HS # check if toe-off event - elif (self.in_swing==False) and (ank_ang > 60): - self.in_swing = True # now in swing, i.e. toe-off just occured + elif (self.in_swing == False) and (ank_ang > 60): + self.in_swing = True # now in swing, i.e. toe-off just occured else: pass @@ -176,9 +186,11 @@ def detect_which_gait_event(self, ank_ang:float): print(asdf) for i in range(20): - asdf.update(i, i+20) + asdf.update(i, i + 20) asdf.return_estimate() print(asdf.return_estimate()) asdf.update(100, 60) print(asdf) + + # test gse_imu with loaded .mat file From 092b402299568937f3b23640012e6fb74000a625 Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Tue, 24 Jun 2025 23:23:52 +0100 Subject: [PATCH 26/27] refactored TR_characterizer to use the flexsea library directly. tested imu-based gait state estimation, but needed proper offset from TR characterization for right exo. --- dephyEB51.py | 5 +- gse_imu.py | 30 ++--- .../TR_characterization_MAIN.py | 103 ++++++++++-------- threading_demo.py | 27 +++-- 4 files changed, 97 insertions(+), 68 deletions(-) diff --git a/dephyEB51.py b/dephyEB51.py index 456e1d8..b8c460d 100644 --- a/dephyEB51.py +++ b/dephyEB51.py @@ -138,7 +138,7 @@ def ankle_angle(self) -> float: # TODO look over ank_ang/100 versus ank_ang*ENC_CLICKS_TO_DEG if self._data is not None: ank_ang_in_deg = self.ank_ang * EB51_CONSTANTS.MOT_ENC_CLICKS_TO_DEG - return float( (self.ank_enc_sign * ank_ang_in_deg) - self.tr_gen.get_offset() ) + return float((self.ank_enc_sign * ank_ang_in_deg) - self.tr_gen.get_offset()) else: LOGGER.debug( msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0." @@ -163,7 +163,7 @@ def update(self): # update gait state estimate self.imu_estimator.update(self.accelz, self.ankle_angle) - def imu_gait_state_estimate(self, var:str)->dict: + def imu_gait_state_estimate(self, var:str): """ Returns the IMU's gait state estimate @@ -174,6 +174,7 @@ def imu_gait_state_estimate(self, var:str)->dict: return state_dict[var] + def assign_id_to_side(self)-> str: """ Determines side (left/right) of the actuator based on previously mapped device ID number. diff --git a/gse_imu.py b/gse_imu.py index c9fa8e1..5bf82de 100644 --- a/gse_imu.py +++ b/gse_imu.py @@ -2,7 +2,7 @@ from math import sqrt from src.utils.filter_utils import MovingAverageFilter -from src.settings.constants import TIME_METHOD +from src.settings.constants import TIME_METHOD, BERTEC_THRESH class IMU_Estimator: @@ -58,6 +58,7 @@ def __init__( self.time_method = time_method self.HS_time: float = self.time_method() + self.HS_time_prev: float = self.time_method() self.in_swing: bool = True self.stride_period_tracker = MovingAverageFilter( initial_value=stride_period_init, size=filter_size @@ -158,26 +159,25 @@ def detect_which_gait_event(self, ank_ang: float): # if some gait event registered if self.activation_state: # check if heel strike event - if self.in_swing and (ank_ang > 20) and (ank_ang < 40): + if self.in_swing and (ank_ang > 30) and (ank_ang < 55): self.in_swing = False # now in stance, i.e. heel strike just occured - self.latest_HS = ( - self.time_method() - ) # record the latest heel strike time - latest_stride_period = ( - self.latest_HS - self.HS_time - ) # compute the latest stride period - self.stride_period_tracker.update( - latest_stride_period - ) # update the stride period estimate - - self.HS_time = self.latest_HS + + # Update HS and HS_prev + self.HS_time_prev = self.HS_time + self.HS_time = self.activations_pitime_local + stride_period_new = self.HS_time - self.HS_time_prev + + stride_period_avg = self.stride_period_tracker.average() + + if abs((stride_period_new - stride_period_avg) / stride_period_avg) < BERTEC_THRESH.ACCEPT_STRIDE_THRESHOLD: # TODO do when pause_event and updatefilters: + self.stride_period_tracker.update(stride_period_new) # update the stride period estimate # check if toe-off event - elif (self.in_swing == False) and (ank_ang > 60): + elif (self.in_swing == False) and (ank_ang > 55): self.in_swing = True # now in swing, i.e. toe-off just occured else: - pass + pass if __name__ == "__main__": diff --git a/src/characterization/transmission_ratio_characterization/TR_characterization_MAIN.py b/src/characterization/transmission_ratio_characterization/TR_characterization_MAIN.py index 9e73544..d2dc89d 100644 --- a/src/characterization/transmission_ratio_characterization/TR_characterization_MAIN.py +++ b/src/characterization/transmission_ratio_characterization/TR_characterization_MAIN.py @@ -8,10 +8,10 @@ from time import sleep from flexsea.device import Device - +from flexsea.fx_enums import FX_CURRENT # from SoftRTloop import FlexibleSleeper -sys.path.insert(0, '/home/pi/VAS_exoboot_controller/') +sys.path.insert(0, '/home/pi/Exoboot-Controller-VAS/') from src.settings.constants import * @@ -41,30 +41,30 @@ def pause_return(self): delay = max(self.period - (current_time - self.last_stop_time), 0) time.sleep(delay) self.stop_time = time.perf_counter() - period = self.stop_time - self.last_stop_time + period = self.stop_time - self.last_stop_time self.last_stop_time = self.stop_time return period def get_active_ports(): """ - To use the exos, it is necessary to define the ports they are going to be connected to. + To use the exos, it is necessary to define the ports they are going to be connected to. These are defined in the ports.yaml file in the flexsea repo """ try: - device_1 = Device(port="/dev/ttyACM0", firmwareVersion="7.2.0", baudRate=230400, logLevel=3) - device_1.open() - side_1 = DEV_ID_TO_SIDE_DICT[device_1.id] - print("Device 1: {}, {}".format(device_1.id, side_1)) + device_1 = Device(port="/dev/ttyACM0", baud_rate=EXO_SETUP_CONST.BAUD_RATE) + device_1.open(freq=500) + side_1 = DEV_ID_TO_SIDE_DICT[device_1.dev_id] + print("Device 1: {}, {}".format(device_1.dev_id, side_1)) except: side_1 = None device_1 = None print("DEVICE 1 NOT FOUND") try: - device_2 = Device(port="/dev/ttyACM1", firmwareVersion="7.2.0", baudRate=230400, logLevel=3) - device_2.open() - side_2 = DEV_ID_TO_SIDE_DICT[device_2.id] - print("Device 2: {}, {}".format(device_2.id, side_2)) + device_2 = Device(port="/dev/ttyACM1", baud_rate=EXO_SETUP_CONST.BAUD_RATE) + device_2.open(freq=500) + side_2 = DEV_ID_TO_SIDE_DICT[device_2.dev_id] + print("Device 2: {}, {}".format(device_2.dev_id, side_2)) except: side_2 = None device_2 = None @@ -86,18 +86,18 @@ class TR_Characterizer: """ Characterize Tranmission ratio of given exoboot """ - def __init__(self, side, flexdevice, current_cmd=BIAS_CURRENT, freq=500, fulldata_prefix=TR_FULLDATA_PREFIX, coefs_prefix=TR_COEFS_PREFIX, date=None): + def __init__(self, side, device, current_cmd=BIAS_CURRENT, freq=500, fulldata_prefix=TR_FULLDATA_PREFIX, coefs_prefix=TR_COEFS_PREFIX, date=None): self.side = side - self.flexdevice = flexdevice + self.device = device self.current_cmd = current_cmd - + # collect speeds self.freq = freq self.printfreq = freq/10 # Get motor/ankle encoder signs - self.motor_sign = DEV_ID_TO_MOTOR_SIGN_DICT[self.flexdevice.id] - self.ank_enc_sign = DEV_ID_TO_ANK_ENC_SIGN_DICT[self.flexdevice.id] + self.motor_sign = DEV_ID_TO_MOTOR_SIGN_DICT[self.device.dev_id] + self.ank_enc_sign = DEV_ID_TO_ANK_ENC_SIGN_DICT[self.device.dev_id] # Set filenames self.date = date if date else datetime.datetime.today().strftime(TR_DATE_FORMATTER) @@ -116,30 +116,33 @@ def collect(self): while this is running. """ print("Starting ankle transmission ratio procedure...\n") - + # Conduct transmission ratio curve characterization procedure and store curve self.motorAngleVec = np.array([]) self.ankleAngleVec = np.array([]) - + iterations = 0 with open(self.fulldata_filename, "w", newline="\n") as f: writer = csv.writer(f) - self.flexdevice.command_motor_current(self.motor_sign * self.current_cmd) + self.device.send_motor_command(FX_CURRENT, self.motor_sign * self.current_cmd) loopsleeper = FlexibleSleeper(period=1/self.freq) lastprint = time.perf_counter() while not self.kill: try: - all_data = self.flexdevice.read(allData=True) - act_pack = all_data[-1] # Newest data + # read device + data = self.device.read() + + ank_ang = data.ank_ang + mot_ang = data.mot_ang iterations += 1 # Ankle direction convention: plantarflexion: increasing angle, dorsiflexion: decreasing angle - current_ank_angle = (self.ank_enc_sign * act_pack['ank_ang'] * ENC_CLICKS_TO_DEG) - self.offset # deg - current_mot_angle = self.motor_sign * act_pack['mot_ang'] * ENC_CLICKS_TO_DEG # deg + current_ank_angle = (self.ank_enc_sign * ank_ang * EB51_CONSTANTS.MOT_ENC_CLICKS_TO_DEG) - self.offset # deg + current_mot_angle = self.motor_sign * mot_ang * EB51_CONSTANTS.MOT_ENC_CLICKS_TO_DEG # deg - act_current = act_pack['mot_cur'] + act_current = data.mot_cur self.motorAngleVec = np.append(self.motorAngleVec, current_mot_angle) self.ankleAngleVec = np.append(self.ankleAngleVec, current_ank_angle) @@ -161,7 +164,7 @@ def collect(self): # fit a 3rd order polynomial to the ankle and motor angles self.motor_angle_curve_coeffs = np.polyfit(self.ankleAngleVec, self.motorAngleVec, 3) - + # polynomial deriv coefficients (derivative of the motor angle vs ankle angle curve yields the TR) self.TR_curve_coeffs = np.polyder(self.motor_angle_curve_coeffs) @@ -170,11 +173,11 @@ def collect(self): print("TR curve") print(str(self.TR_curve_coeffs)) print(self.offset) - + print("Exiting curve characterization procedure") - self.flexdevice.command_motor_current(0) + self.device.send_motor_command(FX_CURRENT, 0) sleep(0.5) - + with open(self.coefs_filename, "w") as file: writer = csv.writer(file, delimiter=",") writer.writerow(self.motor_angle_curve_coeffs) @@ -182,13 +185,16 @@ def collect(self): writer.writerow([self.offset]) print("Collect Finished\n") - + def start(self): - self.flexdevice.command_motor_current(self.motor_sign * self.current_cmd) - + self.device.send_motor_command(FX_CURRENT, self.motor_sign * self.current_cmd) + input("Set ankle angle to maximum dorsiflexion hardstop. Press any key to lock in angle/offset at this ankle position") - act_pack = self.flexdevice.read() - self.offset = self.ank_enc_sign * act_pack['ank_ang'] * ENC_CLICKS_TO_DEG + + data = self.device.read() + + ank_ang = data.ank_ang + self.offset = self.ank_enc_sign * ank_ang * EB51_CONSTANTS.MOT_ENC_CLICKS_TO_DEG print("OFFSET: ", self.offset) input("Press any key to continue") @@ -199,27 +205,36 @@ def stop(self): self.kill = True self.thread.join() +from src.utils.actuator_utils import create_actuators +from exoboots import DephyExoboots if __name__ == "__main__": # Get active ports side_left, device_left, side_right, device_right = get_active_ports() - devices = [device_left, device_right] - sides = [side_left, side_right] - # Get YEAR_MONTH_DAY_HOUR_MINUTE date = datetime.datetime.today().strftime(TR_DATE_FORMATTER) + devices = [device_left, device_right] + sides = [side_left, side_right] + + # Start device streaming and set gains: print("Starting TR Characterization") frequency = 1000 for side, device in zip(sides, devices): - if device: - device.start_streaming(frequency) - device.set_gains(DEFAULT_KP, DEFAULT_KI, DEFAULT_KD, 0, 0, DEFAULT_FF) + if device and side=="right": + + device._start_streaming(frequency, log_en=False) + device.set_gains(DEFAULT_PID_GAINS.KP, + DEFAULT_PID_GAINS.KI, + DEFAULT_PID_GAINS.KD, + 0, + 0, + DEFAULT_PID_GAINS.FF) characterizer = TR_Characterizer(side, device, BIAS_CURRENT, date=date) - + print("Starting {} Characterization".format(side.upper())) characterizer.start() input() #"Press any key to stop TR characterization of exoboot" @@ -228,7 +243,9 @@ def stop(self): # Stop motors for device in devices: if device: - device.stop_motor() + device.send_motor_command(FX_CURRENT, 0) + device._stop_streaming() + device.close() - print("TR Characterization finished. Goodbye") + print("TR Characterization finished. Goodbye") diff --git a/threading_demo.py b/threading_demo.py index 4bb6e6c..e4d9c74 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -118,7 +118,7 @@ def on_pause(self): from src.exo.assistance_calculator import AssistanceCalculator -from src.settings.constants import CONTINUOUS_MODE_FLAG, FLAT_WALK_TIMINGS +from src.settings.constants import CONTINUOUS_MODE_FLAG, FLAT_WALK_TIMINGS, INCLINE_WALK_TIMINGS class ActuatorThread(BaseWorkerThread): @@ -148,9 +148,9 @@ def __init__( # instantiate assistance generator self.assistance_calculator = AssistanceCalculator( - t_rise=FLAT_WALK_TIMINGS.P_RISE, - t_peak=FLAT_WALK_TIMINGS.P_PEAK, - t_fall=FLAT_WALK_TIMINGS.P_FALL, + t_rise=INCLINE_WALK_TIMINGS.P_RISE, + t_peak=INCLINE_WALK_TIMINGS.P_PEAK, + t_fall=INCLINE_WALK_TIMINGS.P_FALL, ) # set-up vars: @@ -183,6 +183,8 @@ def __init__( self.data_logger.track_variable(lambda: self.actuator.imu_gait_state_estimate("HS_time"), "HS_time_imu") self.data_logger.track_variable(lambda: self.actuator.imu_gait_state_estimate("stride_period"), "stride_period_imu") self.data_logger.track_variable(lambda: self.actuator.imu_gait_state_estimate("in_swing"), "in_swing_imu") + # self.data_logger.track_variable(lambda: self.actuator.ankle_angle, "ank_ang") + # self.data_logger.track_variable(lambda: self.actuator.gear_ratio, "gear_ratio") def pre_iterate(self) -> None: """ @@ -232,11 +234,20 @@ def iterate(self) -> None: self.actuator.update() # update actuator states self.data_logger.update() # update logger + # IMU OVERRIDE + # try: + # # full_dict = self.actuator.imu_gait_state_estimate() + # self.HS_time = self.actuator.imu_gait_state_estimate("HS_time") + # self.stride_period = self.actuator.imu_gait_state_estimate("stride_period") + # self.in_swing = self.actuator.imu_gait_state_estimate("in_swing") + # except Exception as e: + # LOGGER.error(f"Ur dumb: {e}") + # obtain time in current stride self.time_in_stride = time.perf_counter() - self.HS_time + LOGGER.debug(f"boo {self.name}{self.actuator.ankle_angle} {self.actuator.gear_ratio}") # TODO add delay compensation - # # acquire torque command based on gait estimate self.torque_command = self.assistance_calculator.torque_generator( @@ -251,8 +262,8 @@ def iterate(self) -> None: # command appropriate current setpoint using DephyExoboots class if self.current_setpoint is not None: - self.actuator.set_motor_current(self.current_setpoint) - # pass + # self.actuator.set_motor_current(self.current_setpoint) + pass else: self.data_logger.warning( f"Unable to command current for {self.actuator.tag}. Skipping." @@ -270,7 +281,7 @@ def post_iterate(self) -> None: "torque_command": self.torque_command, "current_setpoint": self.current_setpoint, "motor_current": self.actuator.motor_current, - "activation_status": self.actuator.imu_gait_state_estimate("activation") + "activation_status": self.actuator.imu_gait_state_estimate("in_swing") }, ) From 662043fddf9e3e1ee60f8d4071cca742b9b1ddef Mon Sep 17 00:00:00 2001 From: Nundini Rawal Date: Wed, 25 Jun 2025 19:10:13 +0100 Subject: [PATCH 27/27] misc edits --- dephyEB51.py | 5 +- gse_bertec.py | 17 +- gse_imu.py | 52 ++-- .../JIM_characterization/JIM_utils.py | 1 + .../TR_characterization_MAIN.py | 2 +- src/settings/constants.py | 4 +- src/settings/constants_dataclasses.py | 6 +- threading_demo.py | 231 +++++++++--------- 8 files changed, 155 insertions(+), 163 deletions(-) diff --git a/dephyEB51.py b/dephyEB51.py index b8c460d..2f70d8f 100644 --- a/dephyEB51.py +++ b/dephyEB51.py @@ -111,7 +111,7 @@ def __init__( self.case_temp_buffer = [] # instantiate transmission ratio getter which uses motor-angle curve coefficients from pre-performed calibration - self.tr_gen = VariableTransmissionRatio(self.side, TEST_TR_FILE) + self.tr_gen = VariableTransmissionRatio(self.side) CONSOLE_LOGGER.info("instantiated variable transmission ratio") # instantiate IMU-based gait-state estimator @@ -163,7 +163,7 @@ def update(self): # update gait state estimate self.imu_estimator.update(self.accelz, self.ankle_angle) - def imu_gait_state_estimate(self, var:str): + def get_imu_gait_state_estimate(self, var:str): """ Returns the IMU's gait state estimate @@ -174,7 +174,6 @@ def imu_gait_state_estimate(self, var:str): return state_dict[var] - def assign_id_to_side(self)-> str: """ Determines side (left/right) of the actuator based on previously mapped device ID number. diff --git a/gse_bertec.py b/gse_bertec.py index 3d2a02a..9640d2f 100644 --- a/gse_bertec.py +++ b/gse_bertec.py @@ -7,7 +7,7 @@ class Bertec_Estimator: """ Stride phase estimation using forceplate thresholding """ - def __init__(self, zmq_subscriber, stride_period_init=1.2, filter_size=10, hs_threshold = 80, to_threshold = 30, time_method=TIME_METHOD): + def __init__(self, zmq_subscriber, stride_period_init=1.2, filter_size=10, hs_threshold = 80, to_threshold = 30): # ZMQ subscriber self.subscriber = zmq_subscriber @@ -15,9 +15,6 @@ def __init__(self, zmq_subscriber, stride_period_init=1.2, filter_size=10, hs_th self.hs_threshold = hs_threshold self.to_threshold = to_threshold - # Time Method - self.time_method = time_method - # State variables self.HS = 0 self.TO = 0 @@ -35,7 +32,7 @@ def return_estimate(self): """ - state_dict = {"HS_time": self.HS, + state_dict = {"HS_time_bertec": self.HS, "stride_period": self.stride_period_tracker.average(), "in_swing": not self.in_contact } @@ -62,14 +59,14 @@ def update(self): force = self.force_prev if force == '' else float(force) # New stride flag - new_stride = False + new_stride_flag = False # Determine state if self.in_contact: if force < self.to_threshold: # New Toe off self.in_contact = False - self.TO = self.time_method() + self.TO = TIME_METHOD() else: # In stance @@ -79,10 +76,10 @@ def update(self): if force >= self.hs_threshold: # New Heel strike self.in_contact = True - new_stride = True + new_stride_flag = True # Record new stride period and update estimate - HS_new = self.time_method() + HS_new = TIME_METHOD() stride_period_new = HS_new - self.HS stride_period_avg = self.stride_period_tracker.average() @@ -99,4 +96,4 @@ def update(self): # Update prev self.force_prev = force - return new_stride, force + return new_stride_flag, force diff --git a/gse_imu.py b/gse_imu.py index 5bf82de..735666e 100644 --- a/gse_imu.py +++ b/gse_imu.py @@ -4,6 +4,11 @@ from src.settings.constants import TIME_METHOD, BERTEC_THRESH +INCLINE_HS_ANK_ANG_UPPER_BOUND = 55 +INCLINE_HS_ANK_ANG_LOWER_BOUND = 30 +FLAT_HS_ANK_ANG_UPPER_BOUND = 40 +FLAT_HS_ANK_ANG_LOWER_BOUND = 20 + class IMU_Estimator: """ @@ -12,7 +17,7 @@ class IMU_Estimator: Outputs: - stride_period: time between two heel-strike activations - - in_swing: flag that indicates that foot is in swing + - in_stance: flag that indicates that foot is in swing - HS_time: timestamp of most recent activation (heel strike) """ @@ -21,7 +26,6 @@ def __init__( self, std_threshold: float = 2, run_len_threshold: int = 10, - time_method: float = TIME_METHOD, stride_period_init: float = 1.20, filter_size: int = 10, ): @@ -31,7 +35,6 @@ def __init__( Args: std_threshold (float): Z-score threshold for activation detection. run_len_threshold (int): Number of consecutive samples below threshold to end activation. - time_method (callable): Function to get the current time (default: TIME_METHOD). """ self.std_threhold = std_threshold @@ -55,11 +58,9 @@ def __init__( self.activations_zscore_peak = [] self.activations_status = [] - self.time_method = time_method - - self.HS_time: float = self.time_method() - self.HS_time_prev: float = self.time_method() - self.in_swing: bool = True + self.HS_time: float = TIME_METHOD() + self.HS_time_prev: float = TIME_METHOD() + self.in_stance: bool = False self.stride_period_tracker = MovingAverageFilter( initial_value=stride_period_init, size=filter_size ) @@ -85,12 +86,10 @@ def return_estimate(self): dict: Dictionary with the current activation state. """ - # state_dict = {"activation": self.activation_state} - state_dict = { "HS_time": self.HS_time, "stride_period": self.stride_period_tracker.average(), - "in_swing": self.in_swing, + "in_swing": not self.in_stance, "activation": self.activation_state, } @@ -128,7 +127,7 @@ def update(self, accel: float, ank_ang: float): # Activation Window if not self.activation_state and self.run_len <= self.run_len_threshold: self.activation_state = True - self.activations_pitime_local = self.time_method() + self.activations_pitime_local = TIME_METHOD() self.activations_zscore_local = self.zscore self.activations_pitime_start.append(self.activations_pitime_local) @@ -140,7 +139,7 @@ def update(self, accel: float, ank_ang: float): self.activations_zscore_peak.append(self.activations_zscore_local) elif self.activation_state and self.zscore > self.activations_zscore_local: - self.activations_pitime_local = self.time_method() + self.activations_pitime_local = TIME_METHOD() self.activations_zscore_local = self.zscore else: @@ -152,32 +151,33 @@ def detect_which_gait_event(self, ank_ang: float): """ Detects gait event depending on activation state. - Updates the last heel strike time, stride period and in_swing flag + Updates the last heel strike time, stride period and in_stance flag """ - # TODO: ensure that only the first activation is detected as heel strike - # if some gait event registered if self.activation_state: # check if heel strike event - if self.in_swing and (ank_ang > 30) and (ank_ang < 55): - self.in_swing = False # now in stance, i.e. heel strike just occured + if (self.in_stance == False) and (ank_ang > FLAT_HS_ANK_ANG_LOWER_BOUND) and (ank_ang < FLAT_HS_ANK_ANG_UPPER_BOUND): + self.in_stance = True # now in stance, i.e. heel strike just occured - # Update HS and HS_prev - self.HS_time_prev = self.HS_time + # get latest HS time self.HS_time = self.activations_pitime_local + + # get new stride period stride_period_new = self.HS_time - self.HS_time_prev + # get averaged stride period stride_period_avg = self.stride_period_tracker.average() + # only feed new stride period into moving average if it's reasonable if abs((stride_period_new - stride_period_avg) / stride_period_avg) < BERTEC_THRESH.ACCEPT_STRIDE_THRESHOLD: # TODO do when pause_event and updatefilters: - self.stride_period_tracker.update(stride_period_new) # update the stride period estimate + self.stride_period_tracker.update(stride_period_new) - # check if toe-off event - elif (self.in_swing == False) and (ank_ang > 55): - self.in_swing = True # now in swing, i.e. toe-off just occured + # update prev HS time to the latest time + self.HS_time_prev = self.HS_time - else: - pass + # check if toe-off event + elif self.in_stance and (ank_ang > FLAT_HS_ANK_ANG_UPPER_BOUND): + self.in_stance = False # now in swing, i.e. toe-off just occured if __name__ == "__main__": diff --git a/src/characterization/JIM_characterization/JIM_utils.py b/src/characterization/JIM_characterization/JIM_utils.py index a835767..e0ed21b 100644 --- a/src/characterization/JIM_characterization/JIM_utils.py +++ b/src/characterization/JIM_characterization/JIM_utils.py @@ -117,6 +117,7 @@ def track_variables_for_JIM_logging(self, logger: Logger) -> None: logger.track_variable(lambda: actuator.motor_current, f"{actuator._tag}_current_mA") logger.track_variable(lambda: actuator.ankle_angle, f"{actuator._tag}_ankang_deg") logger.track_variable(lambda: actuator.case_temperature, f"{actuator._tag}_case_temp_C") + # logger.track_variable(lambda: actuator.mot_volt, f"{actuator._tag}_mot_volt_mV") tracked_vars = logger.get_tracked_variables() print("Tracked variables:", tracked_vars) diff --git a/src/characterization/transmission_ratio_characterization/TR_characterization_MAIN.py b/src/characterization/transmission_ratio_characterization/TR_characterization_MAIN.py index d2dc89d..f845b0f 100644 --- a/src/characterization/transmission_ratio_characterization/TR_characterization_MAIN.py +++ b/src/characterization/transmission_ratio_characterization/TR_characterization_MAIN.py @@ -223,7 +223,7 @@ def stop(self): print("Starting TR Characterization") frequency = 1000 for side, device in zip(sides, devices): - if device and side=="right": + if device: device._start_streaming(frequency, log_en=False) device.set_gains(DEFAULT_PID_GAINS.KP, diff --git a/src/settings/constants.py b/src/settings/constants.py index 67d6e81..1f5a6fc 100644 --- a/src/settings/constants.py +++ b/src/settings/constants.py @@ -77,6 +77,8 @@ id: RIGHT_EXO_IDENTIFIERS.MOTOR_SIGN for id in RIGHT_EXO_IDENTIFIERS.EXO_DEV_IDS } | {id: LEFT_EXO_IDENTIFIERS.MOTOR_SIGN for id in LEFT_EXO_IDENTIFIERS.EXO_DEV_IDS} +# TODO add thread name constants + """ Device Attributes """ EB51_CONSTANTS = EXO_MOTOR_CONSTANTS( MOT_ENC_CLICKS_TO_REV=2**14, @@ -139,7 +141,7 @@ """ Gait State Estimation Toggle""" RUN_BERTEC_GSE:bool = True # if true, an additional thread will be spun up that obtains bertec gait state estimates -USE_SIMULATED_WALKER:bool = False # if true, a simulated walker will output gait state estimates INSTEAD of the bertec in the thread +USE_SIMULATED_WALKER:bool = False # if true, a simulated walker will output gait state estimates INSTEAD of the bertec in the thread """" TIME METHOD """ diff --git a/src/settings/constants_dataclasses.py b/src/settings/constants_dataclasses.py index 2d8fb70..2409cc7 100644 --- a/src/settings/constants_dataclasses.py +++ b/src/settings/constants_dataclasses.py @@ -238,4 +238,8 @@ class STATIC_IP_ADDRESSES: VICON_IP: Vicon ip to connect to Bertec Forceplates for streaming """ RTPLOT_IP: str - VICON_IP: str \ No newline at end of file + VICON_IP: str + + + +# TODO add dataclass for threadnames \ No newline at end of file diff --git a/threading_demo.py b/threading_demo.py index e4d9c74..3fc5533 100644 --- a/threading_demo.py +++ b/threading_demo.py @@ -13,6 +13,7 @@ from opensourceleg.logging import Logger, LogLevel import logging +from src.settings.constants import TIME_METHOD class BaseWorkerThread(threading.Thread, ABC): """ @@ -154,7 +155,7 @@ def __init__( ) # set-up vars: - self.HS_time: float = 0.0 + self.HS_time_bertec: float = 0.0 self.stride_period: float = 1.2 self.in_swing: bool = True self.peak_torque_setpoint: float = 0.0 @@ -162,35 +163,41 @@ def __init__( self.current_setpoint: int = 0 self.time_in_stride: float = 0.0 - self.thread_start_time: float = time.perf_counter() - self.time_since_start: float = 0.0 + # add in vars for imu: + self.HS_time_imu: float = 0.0 + self.stride_period_imu: float = 1.2 + self.in_swing_imu: bool = True + + # var for delay: + self.delay: float = 0.0 + + self.thread_start_time: float = TIME_METHOD() + self.time_since_thread_start: float = 0.0 # track vars for csv logging - self.data_logger.track_variable( - lambda: self.time_since_start, "time_since_start" - ) - self.data_logger.track_variable(lambda: self.HS_time, "HS_time") + self.data_logger.track_variable(lambda: self.time_since_thread_start, "time_since_thread_start") + self.data_logger.track_variable(lambda: self.HS_time_bertec, "HS_time_bertec") self.data_logger.track_variable(lambda: self.stride_period, "stride_period") self.data_logger.track_variable(lambda: self.in_swing, "in_swing_flag_bool") - self.data_logger.track_variable( - lambda: self.peak_torque_setpoint, "peak_torque_setpt" - ) + self.data_logger.track_variable(lambda: self.peak_torque_setpoint, "peak_torque_setpt") self.data_logger.track_variable(lambda: self.torque_command, "torque_command") - self.data_logger.track_variable( - lambda: self.current_setpoint, "current_setpoint" - ) + self.data_logger.track_variable(lambda: self.current_setpoint, "current_setpoint") + # track gse_imu vars - self.data_logger.track_variable(lambda: self.actuator.imu_gait_state_estimate("HS_time"), "HS_time_imu") - self.data_logger.track_variable(lambda: self.actuator.imu_gait_state_estimate("stride_period"), "stride_period_imu") - self.data_logger.track_variable(lambda: self.actuator.imu_gait_state_estimate("in_swing"), "in_swing_imu") - # self.data_logger.track_variable(lambda: self.actuator.ankle_angle, "ank_ang") - # self.data_logger.track_variable(lambda: self.actuator.gear_ratio, "gear_ratio") + self.data_logger.track_variable(lambda: self.actuator.get_imu_gait_state_estimate("HS_time"), "HS_time_imu") + self.data_logger.track_variable(lambda: self.actuator.get_imu_gait_state_estimate("stride_period"), "stride_period_imu") + self.data_logger.track_variable(lambda: self.actuator.get_imu_gait_state_estimate("in_swing"), "in_swing_imu") + self.data_logger.track_variable(lambda: self.actuator.ankle_angle, "ank_ang") + self.data_logger.track_variable(lambda: self.actuator.gear_ratio, "gear_ratio") + self.data_logger.track_variable(lambda: self.actuator.accelz, "accelz") + + # track delay + self.data_logger.track_variable(lambda: self.delay, "delay") def pre_iterate(self) -> None: """ Check inbox for messages from GSE & GUI threads """ - mail_list = self.inbox.get_all_mail() for mail in mail_list: self.decode_message(mail) @@ -201,11 +208,15 @@ def decode_message(self, mail): This method extracts the contents of the message and updates the actuator's state accordingly. """ try: - for key, value in mail.contents.items(): - if CONTINUOUS_MODE_FLAG and key == "peak_torque_setpoint": - self.peak_torque_update_monitor(key, value) - else: - setattr(self, key, value) + if mail.sender == "gse": + for key, value in mail.contents.items(): + if CONTINUOUS_MODE_FLAG and key == "peak_torque_setpoint": + setattr(self, key, value) + else: + self.peak_torque_update_monitor(key, value) + + # Update delay + self.delay = self.HS_time_bertec - self.HS_time_imu except Exception as err: self.data_logger.debug(f"Error decoding message: {err}") @@ -219,7 +230,6 @@ def peak_torque_update_monitor(self, key, value): A new torque will only be felt upon the termination of the current stride. """ # TODO: add in more logic to handle continous vs discrete modes (GUI doesn't send continous stream of data) - if self.in_swing: setattr(self, key, value) @@ -230,31 +240,24 @@ def iterate(self) -> None: It handles the actuator's state updates. """ - self.time_since_start = time.perf_counter() - self.thread_start_time + self.time_since_thread_start = TIME_METHOD() - self.thread_start_time self.actuator.update() # update actuator states self.data_logger.update() # update logger # IMU OVERRIDE - # try: - # # full_dict = self.actuator.imu_gait_state_estimate() - # self.HS_time = self.actuator.imu_gait_state_estimate("HS_time") - # self.stride_period = self.actuator.imu_gait_state_estimate("stride_period") - # self.in_swing = self.actuator.imu_gait_state_estimate("in_swing") - # except Exception as e: - # LOGGER.error(f"Ur dumb: {e}") + self.HS_time_imu = self.actuator.get_imu_gait_state_estimate("HS_time") + self.stride_period_imu = self.actuator.get_imu_gait_state_estimate("stride_period") + self.in_swing_imu = self.actuator.get_imu_gait_state_estimate("in_swing") # obtain time in current stride - self.time_in_stride = time.perf_counter() - self.HS_time - LOGGER.debug(f"boo {self.name}{self.actuator.ankle_angle} {self.actuator.gear_ratio}") - - # TODO add delay compensation + self.time_in_stride = TIME_METHOD() - self.HS_time_imu # acquire torque command based on gait estimate self.torque_command = self.assistance_calculator.torque_generator( current_time=self.time_in_stride, - stride_period=self.stride_period, + stride_period=self.stride_period_imu, peak_torque=float(self.peak_torque_setpoint), - in_swing=self.in_swing, + in_swing=self.in_swing_imu, ) # determine appropriate current setpoint that matches the torque setpoint @@ -262,8 +265,8 @@ def iterate(self) -> None: # command appropriate current setpoint using DephyExoboots class if self.current_setpoint is not None: - # self.actuator.set_motor_current(self.current_setpoint) - pass + self.actuator.set_motor_current(self.current_setpoint) + # pass else: self.data_logger.warning( f"Unable to command current for {self.actuator.tag}. Skipping." @@ -276,12 +279,12 @@ def post_iterate(self) -> None: sender=self.name, recipient="main", contents={ - # "time_since_start": self.time_since_start, + # "time_since_thread_start": self.time_since_thread_start, "peak_torque_setpoint": self.peak_torque_setpoint, "torque_command": self.torque_command, "current_setpoint": self.current_setpoint, "motor_current": self.actuator.motor_current, - "activation_status": self.actuator.imu_gait_state_estimate("in_swing") + "activation_status": self.actuator.get_imu_gait_state_estimate("in_swing") }, ) @@ -330,74 +333,66 @@ def __init__( self.active_actuators = active_actuators self.bertec_estimators = {} - self.time_since_start = 0 - self.thread_start_time = time.perf_counter() + self.time_since_thread_start = 0 + self.thread_start_time = TIME_METHOD() # for each active actuator, initialize GSE Bertec for actuator in self.active_actuators: selected_topic = f"fz_{actuator}" # e.g., 'fz_left' or 'fz_right' - # if USE_SIMULATED_WALKER: - # walker = WalkingSimulator(stride_period=1.20) - # walker.set_percent_toe_off(67) - # self.bertec_estimators[actuator] = walker - - # # track vars for csv logging - # self.data_logger.track_variable( - # lambda: self.time_since_start, f"{actuator}_time_since_start" - # ) - # self.data_logger.track_variable( - # lambda: self.bertec_estimators[actuator].stride_start_time, - # f"{actuator}_HS_time_", - # ) - # self.data_logger.track_variable( - # lambda: self.bertec_estimators[actuator].stride_period, - # f"{actuator}_stride_period", - # ) - # self.data_logger.track_variable( - # lambda: self.bertec_estimators[actuator].in_swing_flag, - # f"{actuator}_in_swing_flag_bool", - # ) - # self.data_logger.track_variable( - # lambda: self.bertec_estimators[actuator].current_time_in_stride, - # f"{actuator}_current_time_in_stride", - # ) - # self.data_logger.track_variable( - # lambda: self.bertec_estimators[actuator].current_percent_gait_cycle, - # f"{actuator}_current_percent_gait_cycle", - # ) - - # else: - bertec_subscriber = Subscriber( - publisher_ip=IP_ADDRESSES.VICON_IP, - topic_filter=selected_topic, - timeout_ms=5, - ) - self.bertec_estimators[actuator] = Bertec_Estimator( - zmq_subscriber=bertec_subscriber - ) + if USE_SIMULATED_WALKER: + walker = WalkingSimulator(stride_period=1.20) + walker.set_percent_toe_off(67) + self.bertec_estimators[actuator] = walker - # # track vars for csv logging - # self.data_logger.track_variable( - # lambda: self.time_since_start, f"{actuator}_time_since_start" - # ) - # self.data_logger.track_variable( - # lambda: self.bertec_estimators[actuator].HS, - # f"{actuator}_HS_time_", - # ) - # self.data_logger.track_variable( - # lambda: self.bertec_estimators[actuator].stride_period_tracker.average(), - # f"{actuator}_stride_period", - # ) - # self.data_logger.track_variable( - # lambda: not self.bertec_estimators[actuator].in_contact, - # f"{actuator}_in_swing_flag_bool", - # ) + # track vars for csv logging + self.data_logger.track_variable( + lambda: self.time_since_thread_start, f"{actuator}_time_since_thread_start" + ) + self.data_logger.track_variable( + lambda: self.bertec_estimators[actuator].stride_start_time, + f"{actuator}_HS_time", + ) + self.data_logger.track_variable( + lambda: self.bertec_estimators[actuator].stride_period, + f"{actuator}_stride_period", + ) + self.data_logger.track_variable( + lambda: self.bertec_estimators[actuator].in_swing_flag, + f"{actuator}_in_swing_flag_bool", + ) + self.data_logger.track_variable( + lambda: self.bertec_estimators[actuator].current_time_in_stride, + f"{actuator}_current_time_in_stride", + ) + self.data_logger.track_variable( + lambda: self.bertec_estimators[actuator].current_percent_gait_cycle, + f"{actuator}_current_percent_gait_cycle", + ) + + else: + bertec_subscriber = Subscriber( + publisher_ip=IP_ADDRESSES.VICON_IP, + topic_filter=selected_topic, + timeout_ms=5, + ) + self.bertec_estimators[actuator] = Bertec_Estimator( + zmq_subscriber=bertec_subscriber + ) + + self.data_logger.track_variable( + lambda: self.time_since_thread_start, + f"time_since_thread_start", + ) + + self.data_logger.track_variable( + lambda: self.bertec_estimators[actuator].force_prev, + f"{actuator}_force_prev", + ) def pre_iterate(self) -> None: pass - def iterate(self): """ Main loop for the actuator thread. @@ -405,34 +400,28 @@ def iterate(self): It updates the gait state estimator and sends the current time in stride and stride period to the actuators. """ - self.time_since_start = time.perf_counter() - self.thread_start_time + self.time_since_thread_start = TIME_METHOD() - self.thread_start_time # for each active actuator, for actuator in self.active_actuators: # Update the gait state estimator for the actuator - self.bertec_estimators[actuator].update() - print(f"asdf {actuator}: {self.bertec_estimators[actuator].return_estimate()}") + new_stride_flag, _force = self.bertec_estimators[actuator].update() # update csv logger self.data_logger.update() # send message to actuator inboxes try: - self.data_logger.debug( - self.bertec_estimators[actuator].return_estimate() - ) - - msg_router.send( - sender=self.name, - recipient=actuator, - contents=self.bertec_estimators[actuator].return_estimate(), - ) + if new_stride_flag: + self.data_logger.debug(self.bertec_estimators[actuator].return_estimate()) + msg_router.send( + sender=self.name, + recipient=actuator, + contents=self.bertec_estimators[actuator].return_estimate(), + ) except: - self.data_logger.debug( - f"UNABLE TO SEND msg to '{actuator}' actuator from GaitStateEstimatorThread. Skipping." - ) - continue + self.data_logger.debug(f"UNABLE TO SEND msg to '{actuator}' actuator from GaitStateEstimatorThread. Skipping.") def post_iterate(self) -> None: pass @@ -475,8 +464,8 @@ def __init__( self.active_actuators = active_actuators self.peak_torque_setpoint: float = 0.0 - self.thread_start_time: float = time.perf_counter() - self.time_since_start: float = 0.0 + self.thread_start_time: float = TIME_METHOD() + self.time_since_thread_start: float = 0.0 # track vars for csv logging self.data_logger.track_variable(lambda: self.peak_torque_setpoint, "torque_setpt") @@ -494,7 +483,7 @@ def iterate(self): If the user doesn't input a new value, the current setpoint is used. """ - self.time_since_start = time.perf_counter() - self.thread_start_time + self.time_since_thread_start = TIME_METHOD() - self.thread_start_time # update csv logging self.data_logger.update() @@ -943,7 +932,7 @@ def update_rt_plots(self) -> list: # set actuator modes & spool belts exoboots.setup_control_modes() - # exoboots.spool_belts() + exoboots.spool_belts() # initialize class responsible for recieving data & rtplotting for the main thread main_thread_receptor = MainThreadMessageReception(actuators)