Initial Commit

code/.vscode/launch.json

@@ -0,0 +1,21 @@
+{
+    // Use IntelliSense to learn about possible attributes.
+    // Hover to view descriptions of existing attributes.
+    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+    "version": "0.2.0",
+    "configurations": [
+        {
+            "name": "Python Debugger: Current File",
+            "type": "debugpy",
+            "request": "launch",
+            "program": "${file}",
+            "console": "integratedTerminal",
+            "env": {
+                "PYDEVD_DISABLE_FILE_VALIDATION": "1"
+            },
+            "args": [
+                "-Xfrozen_modules=off"
+            ]
+        }
+    ]
+}

code/.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "python.REPL.enableREPLSmartSend": false
+}

code/Controller_Input_Elements.py

@@ -0,0 +1,197 @@
+import numpy as np
+
+from Physics_Elements import SharedRigidActuator
+
+from typing import TYPE_CHECKING
+if TYPE_CHECKING:
+    from Object_Sharing import SharedFloat
+    from Physics_Elements import StateSpace, Spring
+
+class RigidActuatorController():
+    def __init__(self, controller_settings: dict):
+        self.controller_settings = controller_settings
+
+        self.attached_sim_target: SharedRigidActuator = None
+
+        self.shared_attributes: SharedRigidActuatorController = None # For plotting if desired
+
+        self.parse_settings()
+
+        self.input_voltage: float = 0.0
+        self.digital_command: int = 0 # 0 means hold position, 1 means control using velocity
+
+        self.controlled_length: float = 0.0
+        self.controlled_vel_scalar: float = 0.0
+        self.controlled_pos = np.array([0.0, 0.0, 0.0])
+        self.controlled_vel = np.array([0.0, 0.0, 0.0])
+
+    def parse_settings(self):
+        self.name = self.controller_settings["name"]
+        self.input_channel = self.controller_settings["analog_input_channel"]
+        self.digital_channel = self.controller_settings["digital_input_channel"]
+        self.max_length = self.controller_settings["max_length"]
+        self.min_length = self.controller_settings["min_length"]
+        self.neutral_length = self.controller_settings.get("neutral_length", 0)
+        self.controls_pos_vel_accel = self.controller_settings["controls_pos/vel/accel"]
+
+    def calc_neutral_pos(self):
+        delta_pos_from_child = -1 * self.neutral_length * self.r_unit_vector
+        self.neutral_pos = self.attached_sim_target.get_child_statespace().get_pos() + delta_pos_from_child
+
+    def get_input_channel(self) -> int:
+        return self.input_channel
+    def get_digital_channel(self) -> int:
+        return self.digital_channel
+
+    def attach_sim_target(self, target: 'SharedRigidActuator'):
+        if not isinstance(target._getvalue(), SharedRigidActuator):
+            raise TypeError(f"target for RigidActuatorController {self.name} is not a SharedRigidActuator")
+        
+        self.attached_sim_target = target
+        self.r_unit_vector = self.get_r_unit_vector()
+        self.calc_neutral_pos()
+        self.controlled_pos = self.neutral_pos
+        self.attached_sim_target.set_parent_statespace_pos(self.controlled_pos)
+
+    def attach_shared_attributes(self, shared_attributes: 'SharedRigidActuatorController'):
+        if not isinstance(shared_attributes._getvalue(), SharedRigidActuatorController):
+            raise TypeError(f"shared_attributes for RigidActuatorController {self.name} is not a SharedRigidActuatorController")
+        self.shared_attributes = shared_attributes
+        
+
+    def get_r_unit_vector(self):
+        return self.attached_sim_target.get_r_unit_vector()
+
+    def set_input_voltage(self, voltage: float):
+        '''After reading a voltage, set it to this controller element'''
+        self.input_voltage = voltage
+
+    def set_digital_command(self, command: int):
+        self.digital_command = command
+
+    def set_controlled_attribute(self):
+        if self.controls_pos_vel_accel == 0: # Control pos
+            self.controlled_length = (self.input_voltage - self.controller_settings["neutral_voltage"]) * self.controller_settings["units_per_volt"] + self.neutral_length
+            self.set_controlled_pos(self.controlled_length)
+        elif self.controls_pos_vel_accel == 1: # Controls vel
+            self.set_controlled_vel()
+        elif self.controls_pos_vel_accel == 2: # Controls accel
+            self.set_controlled_accel()
+
+    def set_controlled_vel(self):
+        # Check if the controlled vel would put us outside our max or min displacement
+            # if it is, set controlled vel to 0 and set our controlled pos manually
+        current_length = self.attached_sim_target.get_length()
+        self.controlled_vel_scalar = (self.input_voltage - self.controller_settings["neutral_voltage"]) * self.controller_settings["units_per_volt"]
+        if current_length > self.max_length:
+            self.set_controlled_pos(self.max_length)
+            if self.controlled_vel_scalar > 0:
+                self.controlled_vel_scalar = 0
+        elif current_length < self.min_length:
+            self.set_controlled_pos(self.min_length)
+            if self.controlled_vel_scalar < 0:
+                self.controlled_vel_scalar = 0
+        self.r_unit_vector = self.get_r_unit_vector()
+        controlled_vel = -1*self.r_unit_vector * self.controlled_vel_scalar # -1 * r vector becaus r vector describes child - parent
+        self.controlled_vel = controlled_vel
+
+    def set_controlled_accel(self): 
+        pass
+
+    def set_controlled_pos(self, length:float):
+        self.r_unit_vector = self.get_r_unit_vector()
+        if length > self.max_length:
+            self.controlled_length = self.max_length
+        elif length < self.min_length:
+            self.controlled_length = self.min_length
+        delta_pos_from_child = self.controlled_length * -1*self.r_unit_vector # -1 * r vector becaus r vector describes child - parent
+        child_pos = self.attached_sim_target.get_child_statespace().get_pos()
+        self.controlled_pos = delta_pos_from_child + child_pos
+
+    def update_shared_attributes(self):
+        if self.shared_attributes is not None:
+            self.shared_attributes.set_digital_command(self.digital_command)
+            self.shared_attributes.set_input_voltage(self.input_voltage)
+            
+            self.shared_attributes.set_controlled_pos(self.controlled_pos)
+            self.shared_attributes.set_controlled_vel(self.controlled_vel)
+            self.shared_attributes.set_controlled_length(self.controlled_length)
+            self.shared_attributes.set_controlled_vel_scalar(self.controlled_vel_scalar)
+        
+
+    def update_sim_target(self):
+        '''Update the sim target with the values we input from the controller'''
+        if self.controls_pos_vel_accel == 0: # Control pos
+            if self.digital_command == 1: # We are actively controlling the actuator
+                self.attached_sim_target.set_parent_statespace_pos(self.controlled_pos)
+        elif self.controls_pos_vel_accel == 1: # Controls vel
+            if self.digital_command == 0: # We are NOT actively controlling the actuator
+                self.attached_sim_target.set_parent_statespace_vel([0,0,0])
+            else: # We are actively controlling the vel
+                self.attached_sim_target.set_parent_statespace_vel(self.controlled_vel)
+        elif self.controls_pos_vel_accel == 2: # Controls accel
+            pass
+        
+
+
+class SharedRigidActuatorController():
+    def __init__(self):
+        self.input_voltage: float = 0.0
+        self.digital_command: int = 0
+
+        self.controlled_length: float = 0.0
+        self.controlled_vel_scalar: float = 0.0
+        self.controlled_pos = np.array([0.0, 0.0, 0.0])
+        self.controlled_vel = np.array([0.0, 0.0, 0.0])
+
+        self.connected_to_plotter: bool = False
+        self.connected_to_visualization: bool = False
+        self.connected_to_sensor: bool = False
+        self.connected_to_controller: bool = False
+
+    def set_input_voltage(self, voltage: float):
+        self.input_voltage = voltage
+    def set_digital_command(self, command: int):
+        self.digital_command = command
+    def set_controlled_pos(self, new_pos):
+        self.controlled_pos = new_pos
+    def set_controlled_vel(self, new_vel):
+        self.controlled_vel = new_vel
+    def set_controlled_length(self, disp: float) -> None:
+        self.controlled_length = disp
+    def set_controlled_vel_scalar(self, vel_scalar: float) -> None:
+        self.controlled_vel_scalar = vel_scalar
+
+    def set_connected_to_plotter(self, state: bool) -> None:
+        self.connected_to_plotter = state
+    def set_connected_to_visualization(self, state: bool) -> None:
+        self.connected_to_visualization = state
+    def set_connected_to_sensor(self, state: bool) -> None:
+        self.connected_to_sensor = state
+    def set_connected_to_controller(self, state: bool) -> None:
+        self.connected_to_controller = state
+    
+
+    def get_connected_to_plotter(self) -> bool:
+        return self.connected_to_plotter
+    def get_connected_to_visualization(self) -> bool:
+        return self.connected_to_visualization
+    def get_connected_to_sensor(self) -> bool:
+        return self.connected_to_sensor
+    def get_connected_to_controller(self) -> bool:
+        return self.connected_to_controller
+
+    def get_controlled_pos(self):
+        return self.controlled_pos
+    def get_controlled_vel(self):
+        return self.controlled_vel
+    def get_controlled_length(self) -> float:
+        return self.controlled_length
+    def get_controlled_vel_scalar(self) -> float:
+        return self.controlled_vel_scalar
+
+
+    def get_input_voltage(self) -> float:
+        return self.input_voltage
+    def get_digital_command(self) -> int:
+        return self.digital_command

code/HITL_Controller.py

@@ -0,0 +1,147 @@
+import time
+from mcculw.ul import ignore_instacal
+
+import threading
+
+from Physics_Elements import Joint, Spring, StateSpace
+from MCC_Interface import MCC3114, MCC202
+
+from typing import List
+from typing import TYPE_CHECKING
+if TYPE_CHECKING:
+    from Sensor_Elements import LoadCell, SharedLoadCellJoint, LoadCellSpring, SharedLoadCellSpring, DisplacementSensor
+    from Controller_Input_Elements import RigidActuatorController
+
+class HITLController():
+    def __init__(self, stop_event, settings: dict):
+        self.stop_event = stop_event
+        self.settings = settings
+
+        self.parse_settings()
+        
+        self.load_cells: List[LoadCell] = []
+        #self.displacement_actuators: List[DisplacementSensor] = []
+        self.displacement_sensors: List[DisplacementSensor] = []
+
+        self.rigid_actuator_controllers: List[RigidActuatorController] = []
+
+    def parse_settings(self):
+        '''Parse the settings to get the settings for the MCC devices'''
+        self.pull_from_sim_period = self.settings["pull_from_sim_period"] / 1000.0
+        self.updated_plotting_shared_attributes_period = self.settings["plotting_update_period"] / 1000.0
+
+    def attach_load_cell(self, load_cell: 'LoadCell'):
+        self.load_cells.append(load_cell)
+
+    def attach_displacement_sensor(self, sensor: 'DisplacementSensor'):
+        self.displacement_sensors.append(sensor)
+
+    def attach_rigid_actuator_controller(self, controller: 'RigidActuatorController'):
+        self.rigid_actuator_controllers.append(controller)
+
+    def update_from_sim(self):
+        # Updates the local sensors based on the sim values
+        for lc in self.load_cells:
+            lc.pull_from_sim()
+        # for rigid_actuator in self.displacement_actuators:
+        #     rigid_actuator.pull_from_sim()
+        for sensor in self.displacement_sensors:
+            sensor.pull_from_sim()
+
+    def update_from_sim_thread(self):
+        while self.spare_stop_event.is_set() == False:
+            self.update_from_sim()
+            time.sleep(self.pull_from_sim_period / 1000)
+
+    def update_internal_sensor_attributes(self):
+        '''Updates internal attributes. This should be updated as fast as possible because it generates
+        sensor noise.'''
+        for lc in self.load_cells:
+            lc.update_internal_attributes()
+        # for rigid_actuator in self.displacement_actuators:
+        #     rigid_actuator.update_internal_attributes()
+        for sensor in self.displacement_sensors:
+            sensor.update_internal_attributes()
+    
+    def update_plotting_shared_attributes(self):
+        for lc in self.load_cells:
+            lc.update_shared_attributes()
+        for rigid_actuator_controller in self.rigid_actuator_controllers:
+            rigid_actuator_controller.update_shared_attributes()
+        # for rigid_actuator in self.displacement_actuators:
+        #     rigid_actuator.update_shared_attributes()
+        for sensor in self.displacement_sensors:
+            sensor.update_shared_attributes()
+
+    def update_plotting_thread(self):
+        while self.spare_stop_event.is_set() == False:
+            self.update_plotting_shared_attributes()
+            time.sleep(self.updated_plotting_shared_attributes_period / 1000)
+
+    def update_mcc3114(self):
+        for lc in self.load_cells:
+            channel = lc.sensor_settings["output_channel"]
+            voltage = lc.get_noisy_voltage_scalar()
+            self.mcc3114.voltage_write(channel, voltage)
+        for sensor in self.displacement_sensors:
+            channel = sensor.sensor_settings["output_channel"]
+            voltage = sensor.get_noisy_voltage_scalar()
+            self.mcc3114.voltage_write(channel, voltage)
+
+    def read_from_mcc202(self):
+        for rigid_actuator_controller in self.rigid_actuator_controllers:
+            input_channel:int = rigid_actuator_controller.get_input_channel()
+            voltage = self.mcc202.voltage_read(channel=input_channel)
+            digital_channel:int = rigid_actuator_controller.get_digital_channel()
+            digital_command = self.mcc202.digital_read(channel=digital_channel)
+            
+            rigid_actuator_controller.set_digital_command(digital_command)
+            rigid_actuator_controller.set_input_voltage(voltage)
+            rigid_actuator_controller.set_controlled_attribute()
+
+    def update_sim_targets(self):
+        for rigid_actuator_controller in self.rigid_actuator_controllers:
+            rigid_actuator_controller.update_sim_target()
+
+    def controller_loop(self):
+        while self.stop_event.is_set() == False:
+            self.loop_time = time.time()
+
+            # Update the internal attributes of the sensors
+            self.update_internal_sensor_attributes()
+
+            # Update MCC3114 (analog output)
+            self.update_mcc3114()
+
+            # Get readings from the MCC202
+            self.read_from_mcc202()
+
+            # Update the shared actuators for the sim to respond to
+            self.update_sim_targets()
+            
+
+    def start_mcc(self):
+        ignore_instacal() # ONLY CALL ONCE
+        self.mcc3114 = MCC3114()
+        self.mcc202 = MCC202()
+
+    def run_process(self):
+        self.start_mcc()
+
+        # Make the threads for interacting with shared elements
+        self.spare_stop_event = threading.Event()
+        self.spare_stop_event.clear()
+
+        updating_plotting_elements_thread = threading.Thread(target=self.update_plotting_thread)
+        update_from_sim_thread = threading.Thread(target=self.update_from_sim_thread)
+
+        updating_plotting_elements_thread.start()
+        update_from_sim_thread.start()
+        
+        self.controller_loop()
+
+        self.spare_stop_event.set()
+
+        updating_plotting_elements_thread.join()
+        update_from_sim_thread.join()
+

code/MCC_Interface.py

@@ -0,0 +1,200 @@
+from mcculw import ul as mcculw
+from mcculw.enums import (AiChanType, BoardInfo, Status, InterfaceType,
+                          FunctionType, InfoType, ScanOptions,
+                          ULRange, AnalogInputMode, DigitalPortType, DigitalIODirection)
+
+import time
+
+
+class MCC3114():
+    def __init__(self):
+        self.range = ULRange.BIP10VOLTS
+        self.board_num = self.setup_device()
+        if self.board_num == -1:
+            print("Could not setup MCC3114")
+
+        self.config_outputs()
+
+        self.current_voltage_outputs: list[float] = [0.0]*16
+        
+        self.set_all_to_zero()
+
+    def __del__(self):
+        self.set_all_to_zero()
+
+    def setup_device(self) -> int:
+        '''Sets up the device without Instacal configuration files'''
+        board_num = 0
+        board_index = 0
+        find_device = "USB-3114"
+        board_num = -1
+        dev_list = mcculw.get_daq_device_inventory(InterfaceType.USB)
+        if len(dev_list) > 0:
+            for device in dev_list:
+                if str(device) == find_device:
+                    board_num = board_index
+                    mcculw.create_daq_device(board_num, device)
+                board_index = board_index + 1
+            if board_num == -1:
+                print(f"Device {find_device} not found")
+                return board_num
+        else:
+            print("No devices detected")
+            return board_num
+        return board_num
+    
+    def config_outputs(self):
+        if self.board_num == -1:
+            return
+        for ch in range(16):
+            mcculw.set_config(
+                InfoType.BOARDINFO, self.board_num, ch,
+                BoardInfo.DACRANGE, self.range
+            )
+
+
+    def set_all_to_zero(self):
+        '''Make the voltage outputs be 0.0 V when the program exits or we delete the object'''
+        if self.board_num == -1:
+            return
+        for i in range(16):
+            self.voltage_write(channel=i, voltage=0.0)
+    
+    def voltage_write(self, channel:int, voltage: float):
+        if self.board_num == -1:
+            return
+        if channel < 0 or channel > 15:
+            print(f"Channel: {channel} is out of range for the MCC USB-3114. Valid range is [0, 15]")
+            return
+        if voltage < -10:
+            print(f"Voltage: {voltage} is out of range for the MCC USB-3114 in this configuration. Valid range is [-10, 10] V")
+            voltage = -10
+        if voltage > 10:
+            print(f"Voltage: {voltage} is out of range for the MCC USB-3114 in this configuration. Valid range is [-10, 10] V")
+            voltage = 10
+        try:
+            mcculw.v_out(self.board_num, channel, self.range, voltage)
+            self.current_voltage_outputs[channel] = voltage
+        except Exception as exc:
+            print(f"Exception occurred doing voltage write to MCC USB-3114: {exc}")
+
+
+
+
+class MCC202():
+    def __init__(self):
+        self.range = ULRange.BIP10VOLTS
+        self.digital_type = DigitalPortType.AUXPORT
+        self.board_num = self.setup_device()
+
+        if self.board_num == -1:
+            print("Could not setup MCC202")
+
+        self.current_analog_inputs: list[float] = [0.0] * 8
+        self.current_digital_inputs: list[int] = [0] * 8
+
+    def __del__(self):
+        pass  # No specific cleanup required for inputs
+
+    def setup_device(self) -> int:
+        '''Sets up the device without Instacal configuration files'''
+        board_num = 0
+        board_index = 0
+        find_device = "USB-202"
+        board_num = -1
+        dev_list = mcculw.get_daq_device_inventory(InterfaceType.USB)
+        if len(dev_list) > 0:
+            for device in dev_list:
+                if str(device) == find_device:
+                    board_num = board_index
+                    mcculw.create_daq_device(board_num, device)
+                board_index = board_index + 1
+            if board_num == -1:
+                print(f"Device {find_device} not found")
+                return board_num
+        else:
+            print("No devices detected")
+            return board_num
+        return board_num
+    
+    def config_inputs(self):
+        if self.board_num == -1:
+            return
+        for ch in range(8):
+            mcculw.set_config(
+                InfoType.BOARDINFO, self.board_num, ch,
+                BoardInfo.RANGE, self.range
+            )
+        for ch in range(8):
+            mcculw.d_config_port(self.board_num, self.digital_type, DigitalIODirection.IN)
+
+    def print_all_channels(self):
+        for i in range(8):
+            print(f"Analog channel {i} reads {self.current_analog_inputs[i]} Volts")
+        for i in range(8):
+            print(f"Digital channel {i} reads {self.current_digital_inputs[i]}")
+
+    def voltage_read(self, channel: int) -> float:
+        if self.board_num == -1:
+            return 0.0
+        if channel < 0 or channel > 7:
+            print(f"Channel: {channel} is out of range for the MCC USB-202. Valid range is [0, 7]")
+            return 0.0
+        try:
+            voltage = mcculw.v_in(self.board_num, channel, self.range)
+            self.current_analog_inputs[channel] = voltage
+            return voltage
+        except Exception as exc:
+            print(f"Exception occurred doing voltage read from MCC USB-202: {exc}")
+            return 0.0
+    
+    def read_all_analog_channels(self):
+        '''Read all analog input channels and update the current_analog_inputs list'''
+        for i in range(8):
+            self.voltage_read(i)
+        
+    def digital_read(self, channel: int) -> int:
+        if self.board_num == -1:
+            return -1
+        if channel < 0 or channel > 7:
+            print(f"Digital channel: {channel} is out of range for the MCC USB-202. Valid range is [0, 7]")
+            return -1
+        try:
+            state = mcculw.d_bit_in(self.board_num, self.digital_type, channel)
+            self.current_digital_inputs[channel] = state
+            return state
+        except Exception as exc:
+            print(f"Exception occurred doing digital read from MCC USB-202: {exc}")
+            return -1
+    
+    def read_all_digital_channels(self):
+        '''Read all digital input channels and update the current_digital_inputs list'''
+        for i in range(8):
+            self.digital_read(i)    
+    
+
+    def read_all_channels(self):
+        for i in range(8):
+            self.voltage_read(i)
+            self.digital_read(i)
+
+
+def main():
+    mcculw.ignore_instacal() # ONLY CALL ONCE
+    #mcc3114 = MCC3114()
+
+    mcc202 = MCC202()
+
+    while True:
+        mcc202.read_all_channels()
+        mcc202.print_all_channels()
+        time.sleep(3)
+
+
+    #mcc3114.voltage_write(1, 5)
+    #mcc3114.voltage_write(7, 5)
+    #time.sleep(100)
+    
+
+if __name__ == "__main__":
+    main()

code/Object_Sharing.py

@@ -0,0 +1,9 @@
+class SharedFloat:
+    def __init__(self, initial_value:float=0.0):
+        self.value = initial_value
+
+    def get(self):
+        return self.value
+
+    def set(self, new_value):
+        self.value = new_value

code/Physics_Elements.py

@@ -0,0 +1,711 @@
+from typing import List, Callable
+import numpy as np
+
+
+from typing import TYPE_CHECKING
+if TYPE_CHECKING:
+    from Physics_Elements import StateSpace, Joint, Spring, SharedRigidActuator, SharedSpring
+
+ZERO_VECTOR = np.array([0.0, 0.0, 0.0])
+
+
+'''
+TODO
+1. Change spring_force to just force
+    Include getters and setters, lest important for attributes
+2. Add Object_Sharing objects to this file
+'''
+
+
+def stiffness_function_const_then_rigid(length, unstretched_length):
+    percent_length = 0.9 # For the first bit of length in compression, it follors k*x
+    k = 1000
+
+    if length < (1-percent_length)* unstretched_length:
+        # It is really compressed, so make k = k *10
+        k = k + k*10/(1 + np.exp(-10*(unstretched_length - length - percent_length*unstretched_length)))
+    return k
+
+def damping_force_x(mass: 'Joint', t):
+    x_vel = mass.statespace.get_vel()[0]
+    a = 1
+    b = 0.01
+    return x_vel*a*-1 + x_vel**2 * b*-1
+
+def damping_force_y(mass: 'Joint', t):
+    y_vel = mass.statespace.get_vel()[1]
+    a = 1
+    b = 0.01
+    return y_vel*a*-1 + y_vel**2 * b*-1
+
+def damping_force_z(mass: 'Joint', t):
+    z_vel = mass.statespace.get_vel()[2]
+    a = 1
+    b = 0.01
+    return z_vel*a*-1 + z_vel**2 * b*-1
+
+'''
+TODO
+'''
+class StateSpace():
+    def __init__(self, pos=np.copy(ZERO_VECTOR), vel=np.copy(ZERO_VECTOR), accel=np.copy(ZERO_VECTOR), force=np.copy(ZERO_VECTOR), integration_method="simple"):
+        self.pos = pos
+        self.vel = vel
+        self.accel = accel
+
+        self.force = force
+
+        self.integration_method = self.parse_integration_method(integration_method)
+
+        self.max_saved = 2
+        self.previous_positions: list[list[float, float, float]] = []
+        self.previous_vel: list[list[float, float, float]] = []
+        self.previous_accel: list[list[float, float, float]] = []
+
+        self.verlet_constants = [
+            [],
+            [],
+            [-1, 2, 1],
+            [0, -1, 2, 1],
+            [1/11, -4/11, -6/11, 20/11, 12/11],
+        ]
+
+    def parse_integration_method(self, method_str):
+        if method_str == "simple":
+            return 1
+        elif method_str == "verlet":
+            return 2
+        elif method_str == "adams-bashforth":
+            return 3
+
+
+    def integrate(self, delta_t):
+        '''
+        NOTE: This is run after accel has been updated. 
+        '''
+        if self.integration_method == 1:
+            self.vel = self.vel + self.accel * delta_t
+            self.pos = self.pos + self.vel * delta_t
+        elif self.integration_method == 2:
+            # Verlet integration
+            self.save_last()
+            if delta_t >= 0.005:
+                self.vel = self.vel + self.accel * delta_t
+                self.pos = self.pos + self.vel * delta_t
+            else:
+                self.vel = self.vel + self.accel * delta_t
+                self.verlet_integration(delta_t)
+        elif self.integration_method == 3: # Adams-Bashforth
+            self.save_last()
+            if delta_t >= 0.005:
+                self.vel = self.vel + self.accel * delta_t
+                self.pos = self.pos + self.vel * delta_t
+            else:
+                self.adams_bashforth_integration(delta_t)
+
+    def print(self):
+        print(f"Pos: {self.pos}, Vel: {self.vel}, Accel: {self.accel}")
+
+    def adams_bashforth_integration(self, delta_t):
+        '''
+        Testing.
+        2 points seems to damp the system. 3 does better at maintaining. Above 3 it is worse
+        '''
+        num_prev_accel = len(self.previous_accel)
+        match num_prev_accel:
+            case 0 | 1:
+                self.vel = self.vel + self.accel * delta_t
+            case 2:
+                self.vel = self.vel + delta_t * (1.5 * self.previous_accel[-1] - 0.5 * self.previous_accel[-2])
+            case 3:
+                self.vel = self.vel + delta_t * (23/12 * self.previous_accel[-1] - 4/3 * self.previous_accel[-2] + 5/12 * self.previous_accel[-3])
+            case 4:
+                self.vel = self.vel + delta_t * (55/24 * self.previous_accel[-1] - 59/24 * self.previous_accel[-2] + 37/24 * self.previous_accel[-3] - 9/24 * self.previous_accel[-4])
+            case 5:
+                self.vel = self.vel + delta_t * ((1901 * self.previous_accel[-1] - 2774 * self.previous_accel[-2] + 2616 * self.previous_accel[-3] - 1274 * self.previous_accel[-4] + 251 * self.previous_accel[-5]) / 720)
+            case _:
+                self.vel = self.vel + self.accel * delta_t
+        self.pos = self.pos + self.vel * delta_t
+
+    def verlet_integration(self, delta_t):
+        num_prev_positions = len(self.previous_positions)
+        match num_prev_positions:
+            case 0 | 1:
+                self.pos = self.pos + self.vel * delta_t
+            case 2:
+                new_pos = np.array([0.0, 0.0, 0.0])
+                for i in range(2):
+                    new_pos += self.verlet_constants[2][i] * self.previous_positions[i]
+                new_pos += self.verlet_constants[2][-1] * delta_t * self.accel * delta_t
+                self.pos = new_pos
+            case 3:
+                new_pos = np.array([0.0, 0.0, 0.0])
+                for i in range(3):
+                    new_pos += self.verlet_constants[3][i] * self.previous_positions[i]
+                new_pos += self.verlet_constants[3][-1] * delta_t * self.accel * delta_t
+                self.pos = new_pos
+            case 4:
+                new_pos = np.array([0.0, 0.0, 0.0])
+                for i in range(4):
+                    new_pos += self.verlet_constants[4][i] * self.previous_positions[i]
+                new_pos += self.verlet_constants[4][-1] * delta_t * self.accel * delta_t
+                self.pos = new_pos
+            case _:
+                self.pos = self.pos + self.vel * delta_t
+
+    def save_last(self):
+        self.previous_positions.append(np.copy(self.pos))
+        self.previous_positions = self.previous_positions[-1*self.max_saved:]
+        self.previous_vel.append(np.copy(self.vel))
+        self.previous_vel = self.previous_vel[-1*self.max_saved:]
+        self.previous_accel.append(np.copy(self.accel))
+        self.previous_accel = self.previous_accel[-1*self.max_saved:]
+
+    def save_last_statespace(self):
+        if self.last_statespace is None:
+            self.last_statespace = StateSpace()
+        self.last_statespace.pos = np.copy(self.pos)
+        self.last_statespace.vel = np.copy(self.vel)
+        self.last_statespace.accel = np.copy(self.accel)
+        
+    def set_pos(self, new_pos):
+        self.pos = new_pos
+    def set_vel(self, new_vel):
+        self.vel = new_vel
+    def set_accel(self, new_accel):
+        self.accel = new_accel
+    def set_force(self, new_force):
+        self.force = new_force
+    
+    def get_pos(self):
+        return self.pos
+    def get_vel(self):
+        return self.vel
+    def get_accel(self):
+        return self.accel
+    def get_force(self):
+        return self.force
+
+class Joint():
+    def __init__(self, pos=np.copy(ZERO_VECTOR), mass: float=0.1, fixed: bool=False, name: str="Joint", integration_method:str="simple"):
+        self.name = name
+        self.statespace = StateSpace(pos, ZERO_VECTOR, ZERO_VECTOR, ZERO_VECTOR, integration_method=integration_method)
+
+        self.connected_springs: List[Spring] = []
+        self.constant_external_forces: List[List[float, float, float]] = []
+        
+        self.connected_actuators: List[RigidActuator] = []
+
+        self.variable_external_forces = [[], [], []]
+
+        self.fixed = fixed
+        self.connected_to_rigid_actuator = False
+        self.mass = mass
+
+        self.parent_joints: List[Joint] = []
+        self.child_joints: List[Joint] = []
+
+        self.damping = False
+
+        self.sharing_attributes = False
+        self.shared_attributes = None
+
+    def is_same(self, other_joint: 'Joint'):
+        if self.name == other_joint.get_name():
+            return True
+        return False
+
+    def get_statespace(self):
+        return self.statespace
+
+    def get_pos(self):
+        return self.statespace.get_pos()
+    
+    def get_vel(self):
+        return self.statespace.get_vel()
+    
+    def get_accel(self):
+        return self.statespace.get_accel()
+
+    def set_state_space_pos(self, new_pos):
+        self.statespace.set_pos(new_pos)
+
+    def set_state_space_vel(self, new_vel):
+        self.statespace.set_vel(new_vel)
+    
+    def set_connected_to_rigid_actuator(self, state):
+        self.connected_to_rigid_actuator = state
+
+    def get_connected_springs(self):
+        return self.connected_springs
+    
+    def get_connected_actuators(self):
+        return self.connected_actuators
+    
+    def get_name(self):
+        return self.name
+
+    def add_spring(self, new_spring):
+        self.connected_springs.append(new_spring)
+
+    def add_actuator(self, new_actuator):
+        self.connected_actuators.append(new_actuator)
+    
+    def add_constant_force(self, new_force):
+        self.constant_external_forces.append(new_force)
+
+    def integrate_statespace(self, delta_t):
+        self.statespace.integrate(delta_t)
+
+    def add_gravity(self):
+        gravity_force = np.array([
+            0,
+            -9.81 * self.mass,
+            0
+        ])
+        self.add_constant_force(gravity_force)
+
+    def add_variable_force(self, force_vect: list):
+        if force_vect[0] is not None:
+            self.variable_external_forces[0].append(force_vect[0])
+        if force_vect[1] is not None:
+            self.variable_external_forces[1].append(force_vect[1])
+        if force_vect[2] is not None:
+            self.variable_external_forces[2].append(force_vect[2])
+
+    def calc_net_spring_force(self):
+        net_spring_force = 0
+        for spring in self.get_connected_springs():
+            if self.is_same(spring.get_parent_joint()):
+                spring_force = spring.get_spring_force() # This is the force the spring exerts on the joint ASSUMING self is parent_joint of the spring
+            else: 
+                spring_force = -1*spring.get_spring_force()
+            net_spring_force += spring_force
+        return net_spring_force
+
+    def calc_net_constant_external_force(self):
+        net_external_force = np.copy(ZERO_VECTOR)
+        for force in self.constant_external_forces:
+            net_external_force += force    
+        return net_external_force
+    
+    def calc_net_variable_external_force(self, t):
+        net_variable_external_force = np.array([0.0, 0.0, 0.0])
+        for i in range(3):
+            for func in self.variable_external_forces[i]:
+                force = func(t)
+                net_variable_external_force[i] += force
+        return net_variable_external_force
+    
+    def calc_net_force(self, t):
+        net_force = np.copy(ZERO_VECTOR)
+        if len(self.constant_external_forces) != 0:
+            net_force += self.calc_net_constant_external_force()
+        if len(self.variable_external_forces) != 0:
+            net_force += self.calc_net_variable_external_force(t)
+        
+        net_spring_force = self.calc_net_spring_force()
+        
+        if self.damping == True:
+            net_force += self.calc_damping(net_spring_force)
+
+        net_force += net_spring_force
+
+        self.statespace.set_force(net_force)
+        return self.statespace.get_force()
+    
+    def add_damping(self, vel_ratio=0.25, mom_ratio=None):
+        self.damping = True
+        if mom_ratio is not None:
+            self.damping_vel_ratio = self.mass * mom_ratio
+        else:
+            self.damping_vel_ratio = vel_ratio
+
+    def calc_damping(self, spring_force):
+        vel_damping = self.damping_vel_ratio * (self.statespace.get_vel()) * -1
+
+        return vel_damping
+
+    def calc_accel(self):
+        if self.fixed == False and self.connected_to_rigid_actuator == False:
+            self.statespace.set_accel(self.statespace.get_force() / self.mass)
+    
+    def is_in_list(self, joints: list['Joint']):
+        for j in joints:
+            if self.is_same(j):
+                return True
+        return False
+
+    def find_parent_joints(self):
+        for spring in self.connected_springs:
+            if self.is_same(spring.get_child_joint()):
+                parent_joint = spring.get_parent_joint()
+                if not parent_joint.is_in_list(self.parent_joints):
+                    self.parent_joints.append(parent_joint)
+
+    def find_child_joints(self):
+        for spring in self.connected_springs:
+            if self.is_same(spring.get_parent_joint()):
+                child_joint = spring.get_child_joint()
+                if not child_joint.is_in_list(self.child_joints):
+                    self.child_joints.append(child_joint)
+
+    def print_attributes(self):
+        print(f"Joint: {self.name}")
+        print(f"Force: {self.statespace.get_force()}")
+        print(f"Accel: {self.statespace.get_accel()}")
+        print(f"Vel: {self.statespace.get_vel()}")
+        print(f"Pos: {self.statespace.get_pos()}")
+
+    def attach_shared_attributes(self, shared_attributes: 'SharedJoint'):
+        if not isinstance(shared_attributes._getvalue(), SharedJoint):
+            raise TypeError(f"shared_attributes for Joint {self.name} is not a SharedJoint")
+        self.sharing_attributes = True
+        self.shared_attributes = shared_attributes
+
+        self.shared_attributes.set_name(f"{self.name}")
+
+        self.update_shared_attributes()
+
+    def update_shared_attributes(self):
+        if self.shared_attributes is not None:
+            self.shared_attributes.set_statespace(self.statespace)
+
+class Spring():
+    def __init__(self, parent_joint: 'Joint', child_joint: 'Joint', unstretched_length: float=0, constant_stiffness: float = None, stiffness_func: Callable[[float, float], float]=None, name: str="Spring"):
+        self.name = name
+
+        self.parent_joint = parent_joint
+        self.child_joint = child_joint
+        
+        self.unstretched_length = unstretched_length
+        self.stiffness_func = stiffness_func
+        self.constant_stiffness = constant_stiffness
+        self.current_stiffness = 0.0
+
+        self.vector = self.calc_r_vector()
+        self.length = self.calc_length()
+        self.unit_vector: list[float] = self.calc_r_unit_vector()
+        self.spring_force_scalar: float = 0.0
+        self.spring_force: list[float] = self.calc_spring_force()
+        
+
+        self.parent_joint.add_spring(self)
+        self.child_joint.add_spring(self)
+
+        self.sharing_attributes = False
+        self.shared_attributes: 'SharedSpring' = None
+
+    def get_name(self):
+        return self.name
+
+    def get_parent_joint(self):
+        return self.parent_joint
+    
+    def get_child_joint(self):
+        return self.child_joint
+    
+    def get_spring_force(self):
+        return self.spring_force
+    
+    def calc_r_vector(self):
+        return self.child_joint.get_statespace().get_pos() - self.parent_joint.get_statespace().get_pos()
+
+    def calc_length(self):
+        return ((self.vector[0])**2 + (self.vector[1])**2 + (self.vector[2])**2) ** 0.5
+        
+    def calc_r_unit_vector(self):
+        return self.vector / self.length
+    
+    def calc_stiffness(self):
+        if self.stiffness_func is not None:
+            return self.stiffness_func(self.length, self.unstretched_length)
+        else:
+            return self.constant_stiffness
+    
+    def calc_spring_force(self):
+        '''Positive force is in tension. Aka, the spring PULLS on the other object'''
+        self.length = self.calc_length()
+        del_length = self.length - self.unstretched_length
+        self.current_stiffness = self.calc_stiffness()
+        self.spring_force_scalar = del_length * self.current_stiffness
+
+        self.vector = self.calc_r_vector()
+        self.r_unit_vector = self.calc_r_unit_vector()
+        self.spring_force = self.spring_force_scalar * self.r_unit_vector
+        return self.spring_force
+    
+    def print_attributes(self):
+        print(f"Spring: {self.name}")
+        print(f"Length: {self.length}")
+        print(f"Spring Force: {self.spring_force}")
+    
+    def get_pos(self):
+        return self.parent_statespace.get_pos()
+    def get_axis_vector(self):
+        return self.child_statespace.get_pos() - self.parent_statespace.get_pos()
+
+    def attach_shared_attributes(self, shared_attributes: 'SharedSpring'):
+        if not isinstance(shared_attributes._getvalue(), SharedSpring):
+            raise TypeError(f"shared_attributes for Spring {self.name} is not a SharedSpring")
+        self.sharing_attributes = True
+        self.shared_attributes = shared_attributes
+
+        self.update_shared_attributes()
+    
+    def update_shared_attributes(self):
+        if self.shared_attributes is not None:
+            self.shared_attributes.set_child_statespace(self.child_joint.get_statespace())
+            self.shared_attributes.set_parent_statespace(self.parent_joint.get_statespace())
+            self.shared_attributes.set_spring_force(self.spring_force)
+            self.shared_attributes.set_spring_force_scalar(self.spring_force_scalar)
+            self.shared_attributes.set_length(self.length)
+            self.shared_attributes.set_unstretched_length(self.unstretched_length)
+            self.shared_attributes.set_stiffness(self.current_stiffness)
+
+    def is_same(self, other_spring: 'Spring'):
+        if self.name == other_spring.get_name():
+            return True
+        return False
+
+
+    def is_in_list(self, springs: list['Spring']):
+        for j in springs:
+            if self.is_same(j):
+                return True
+        return False
+
+'''
+It connects 2 joints. 
+1 joint is completely fixed. Cannot move. Ever. It will be referred to as "grounded".
+The other joint is contrained to the grounded joint. The actuator controls the vector displacement <x,y,z> from the grounded joint
+'''
+class RigidActuator():
+    def __init__(self, parent_joint: 'Joint', grounded_joint: 'Joint', name: str = "Rigid Actuator", max_length:float=1, min_length:float=0.1, control_code=0):
+        self.name = name
+
+        self.parent_joint = parent_joint
+        self.grounded_joint = grounded_joint # Same as child joint
+
+        self.parent_joint.add_actuator(self)
+        self.parent_joint.set_connected_to_rigid_actuator(True)
+        self.grounded_joint.add_actuator(self)
+        self.grounded_joint.set_connected_to_rigid_actuator(True)
+
+        self.max_length = max_length
+        self.min_length = min_length
+        self.control_code = control_code # 0 for pos, 1 for vel, 2 for accel
+
+        self.calc_r_unit_vector()
+
+        self.shared_attributes: 'SharedRigidActuator' = None
+
+    def get_name(self):
+        return self.name
+
+    def get_parent_joint(self):
+        return self.parent_joint
+    
+    def get_child_joint(self):
+        return self.grounded_joint
+    
+    def is_same(self, other: 'RigidActuator'):
+        if self.name == other.get_name():
+            return True
+        return False
+
+    def is_in_list(self, actuators: list['RigidActuator']):
+        for j in actuators:
+            if self.is_same(j):
+                return True
+        return False
+    
+    def calc_r_vector(self):
+        return self.grounded_joint.get_statespace().get_pos() - self.parent_joint.get_statespace().get_pos()
+    
+    def calc_length(self):
+        return ((self.vector[0])**2 + (self.vector[1])**2 + (self.vector[2])**2) ** 0.5
+        
+    def calc_r_unit_vector(self):
+        self.vector = self.calc_r_vector()
+        self.length = self.calc_length()
+        return self.vector / self.length
+    
+    def get_pos(self):
+        return self.parent_joint.get_statespace().get_pos()
+    
+    def get_axis_vector(self):
+        return self.grounded_joint.get_statespace().get_pos() - self.parent_joint.get_statespace().get_pos()
+    
+    def get_r_unit_vector(self):
+        return self.calc_r_unit_vector()
+
+    def update_shared_attributes(self):
+        # These 2 lines are used to make sure we set the internal properties correctly
+        self.vector = self.calc_r_vector()
+        self.length = self.calc_length()
+        # END
+        if self.shared_attributes is not None:
+            self.shared_attributes.set_child_statespace(self.grounded_joint.get_statespace())
+            self.shared_attributes.set_parent_statespace(self.parent_joint.get_statespace())
+            self.shared_attributes.set_length(self.length)
+
+    def attach_shared_attributes(self, shared_attributes: 'SharedRigidActuator'):
+        if not isinstance(shared_attributes._getvalue(), SharedRigidActuator):
+            raise TypeError(f"shared_attributes for RigidActuator {self.name} is not a SharedRigidActuator")
+        self.sharing_attributes = True
+        self.shared_attributes = shared_attributes
+
+        self.update_shared_attributes()
+
+    def set_parent_pos(self, new_pos):
+        '''Set the position of the parent joint's statestpace'''
+        self.parent_joint.set_state_space_pos(new_pos)
+
+    def update_from_controller(self):
+        if self.control_code == 0: # Controller controls pos
+            self.set_pos_to_controlled_pos()
+        elif self.control_code == 1: # Controller controls vel
+            self.set_vel_to_controlled_vel()
+        elif self.control_code == 2: # Controls controls accel
+            pass
+
+    def set_vel_to_controlled_vel(self):
+        controlled_vel = self.shared_attributes.get_vel()
+        self.parent_joint.set_state_space_vel(controlled_vel)
+
+    def set_pos_to_controlled_pos(self):
+        controlled_pos = self.shared_attributes.get_pos()
+        self.parent_joint.set_state_space_pos(controlled_pos)
+
+
+class SharedPhysicsElement():
+    def __init__(self) -> None:
+        self.name: str = None
+        self.statespace: StateSpace = None
+
+        self.connected_to_plotter: bool = False
+        self.connected_to_visualization: bool = False
+        self.connected_to_sensor: bool = False
+        self.connected_to_controller: bool = False
+    
+    def set_name(self, name:str) -> None:
+        self.name = name
+    def set_statespace(self, statespace:StateSpace) -> None:
+        self.statespace = statespace
+    def set_connected_to_plotter(self, state: bool) -> None:
+        self.connected_to_plotter = state
+    def set_connected_to_visualization(self, state: bool) -> None:
+        self.connected_to_visualization = state
+    def set_connected_to_sensor(self, state: bool) -> None:
+        self.connected_to_sensor = state
+    def set_connected_to_controller(self, state: bool) -> None:
+        self.connected_to_controller = state
+
+    def get_name(self) -> str:
+        return self.name
+    def get_statespace(self) -> StateSpace:
+        return self.statespace
+    def get_pos(self) -> List[float]:
+        return self.statespace.get_pos()
+    def get_vel(self) -> List[float]:
+        return self.statespace.get_vel()
+    def get_accel(self) -> List[float]:
+        return self.statespace.get_accel()
+    def get_force(self) -> List[float]:
+        return self.statespace.get_force()
+    def get_connected_to_plotter(self) -> bool:
+        return self.connected_to_plotter
+    def get_connected_to_visualization(self) -> bool:
+        return self.connected_to_visualization
+    def get_connected_to_sensor(self) -> bool:
+        return self.connected_to_sensor
+    def get_connected_to_controller(self) -> bool:
+        return self.connected_to_controller
+    
+class SharedJoint(SharedPhysicsElement):
+    def _init__(self) -> None:
+        super().__init__()
+
+class SharedSpring(SharedPhysicsElement):
+    '''self.statespace is the statespace of the parent'''
+    def _init__(self) -> None:
+        super().__init__()
+        self.child_statespace: StateSpace = None
+
+        self.unstretched_length: float = None
+        self.length: float = None
+        self.stiffness: float = None
+
+        self.spring_force: float = None
+
+    def set_parent_statespace(self, statespace:StateSpace) -> None:
+        self.statespace = statespace
+    def set_child_statespace(self, statespace:StateSpace) -> None:
+        self.child_statespace = statespace
+    def set_unstretched_length(self, unstretched_length: float) -> None:
+        self.unstretched_length = unstretched_length
+    def set_length(self, length: float) -> None:
+        self.length = length
+    def set_stiffness(self, stiffness: float) -> None:
+        self.stiffness = stiffness
+    def set_spring_force(self, set_spring_force: float) -> None:
+        self.spring_force = set_spring_force
+    def set_spring_force_scalar(self, scalar_force: float) -> None:
+        self.spring_force_scalar = scalar_force
+    
+    def get_parent_statespace(self) -> StateSpace:
+        return self.statespace
+    def get_child_statespace(self) -> StateSpace:
+        return self.child_statespace
+    def get_unstretched_length(self) -> float:
+        return self.unstretched_length
+    def get_length(self) -> float:
+        return self.length
+    def get_stiffness(self) -> float:
+        return self.stiffness
+    def get_spring_force(self) -> list[float]:
+        return self.spring_force
+    def get_spring_force_scalar(self) -> float:
+        return self.spring_force_scalar
+    def get_axis_vector(self) -> List[float]:
+        return self.child_statespace.get_pos() - self.statespace.get_pos()
+
+
+class SharedRigidActuator(SharedPhysicsElement):
+    '''self.statespace is the statespace of the parent'''
+    def _init__(self) -> None:
+        super().__init__()
+        self.child_statespace: StateSpace = None
+
+        self.length: float = None
+
+    def set_parent_statespace(self, statespace:StateSpace) -> None:
+        self.statespace = statespace
+    def set_parent_statespace_pos(self, new):
+        self.statespace.set_pos(new)
+    def set_parent_statespace_vel(self, new):
+        self.statespace.set_vel(new)
+    def set_child_statespace(self, statespace:StateSpace) -> None:
+        self.child_statespace = statespace
+    def set_length(self, length: float) -> None:
+        self.length = length
+    
+    def get_parent_statespace(self) -> StateSpace:
+        return self.statespace
+    def get_child_statespace(self) -> StateSpace:
+        return self.child_statespace
+    def get_length(self) -> float:
+        return self.length
+    def get_axis_vector(self) -> List[float]:
+        return self.child_statespace.get_pos() - self.statespace.get_pos()
+    def get_r_unit_vector(self):
+        return self.calc_r_unit_vector()
+    def calc_r_vector(self):
+        return self.child_statespace.get_pos() - self.statespace.get_pos()
+    def calc_length(self):
+        return ((self.vector[0])**2 + (self.vector[1])**2 + (self.vector[2])**2) ** 0.5  
+    def calc_r_unit_vector(self):
+        self.vector = self.calc_r_vector()
+        self.length = self.calc_length()
+        return self.vector / self.length

code/Sensor_Elements.py

@@ -0,0 +1,451 @@
+import numpy as np
+from math import log, copysign
+
+from Physics_Elements import SharedRigidActuator, SharedJoint, SharedSpring
+
+from typing import TYPE_CHECKING
+if TYPE_CHECKING:
+    from Object_Sharing import SharedFloat
+    from Physics_Elements import StateSpace, Spring
+
+
+
+'''
+TODO
+1.
+2. Better noise generation
+    Non-linearity, hysteresis, background noise, AC noise
+'''
+
+class LoadCell():
+    '''Provides net force feedback.'''
+    def __init__(self, sensor_settings: dict={}):
+        self.sensor_settings = sensor_settings
+
+        self.attached_sim_source = None
+
+        self.shared_attributes: SharedLoadCell = None # For plotting if desired
+
+        self.true_force_vector = np.array([0.0, 0.0, 0.0]) # Current real force on the joint
+        self.true_force_scalar: float = 0.0
+        self.last_true_force_scalars: list[float] = []           # Stores the last true force for hysteresis reasons
+        self.true_voltage_scalar:float = 0.0
+
+        self.noisy_force_vector = np.array([0.0, 0.0, 0.0])
+        self.noisy_force_scalar: float = 0.0
+        self.noisy_voltage_scalar: float = 0.0
+
+        self.parse_settings()
+
+        self.calc_noise_parameters()
+
+    def parse_settings(self):
+        self.name = self.sensor_settings["name"]
+        self.scale = self.sensor_settings["mV/V"] / 1000.0
+        self.excitation = self.sensor_settings["excitation"]
+        self.full_scale_force = self.sensor_settings["full_scale_force"]
+        self.output_channel = self.sensor_settings["output_channel"]
+
+        self.noise_settings = self.sensor_settings.get("noise_settings", dict())
+
+        self.full_scale_voltage = self.scale * self.excitation
+
+    def calc_noise_parameters(self):
+        # Calculate static error
+        self.static_error_stdev = self.full_scale_voltage * self.noise_settings.get("static_error_band", 0) / 100
+
+        # Non-linearity
+        self.nonlinearity_exponent = self.calc_nonlinearity_exponent()
+
+        # Hysteresis
+        self.hysteresis = self.noise_settings.get("hysteresis", 0) / 100
+
+        # Repeatability
+        self.repeatability_offset = np.random.normal(0, self.full_scale_voltage * self.noise_settings.get("repeatability", 0) / 100)
+
+        # Thermal applied at a given true voltage
+        self.thermal_error = self.noise_settings.get("thermal_error", 0) / 100 * self.noise_settings.get("temperature_offset", 0)
+
+
+    def calc_nonlinearity_exponent(self):
+        voltage = 10
+        if self.full_scale_voltage != 1:
+            self.full_scale_voltage
+        error = self.noise_settings.get("non-linearity", 0) / 100
+        b = log(1 + error, voltage)
+        return b
+
+    def attach_sim_source(self, sim_source):
+        self.attached_sim_source = sim_source
+
+    def attach_shared_attributes(self, shared_attributes):
+        self.shared_attributes = shared_attributes
+
+    def update_shared_attributes(self):
+        if self.shared_attributes is not None:
+            self.shared_attributes.set_true_force_vector(self.true_force_vector)
+            self.shared_attributes.set_true_force_scalar(self.true_force_scalar)
+            self.shared_attributes.set_true_voltage_scalar(self.true_voltage_scalar)
+
+            self.shared_attributes.set_noisy_voltage_scalar(self.noisy_voltage_scalar)
+
+    def update_internal_attributes(self):
+        self.calc_sensor_force_scalar()
+        self.calc_true_voltage_scalar()
+
+        # Calc noise
+        self.calc_noisy_voltage_scalar()
+
+    def pull_from_sim(self):
+        '''Gets the real net force from the shared attribute updated by the sim'''
+        # Must be overridden
+        pass
+    
+    def calc_sensor_force_scalar(self):
+        self.last_true_force_scalars.append(self.true_force_scalar)
+        self.last_true_force_scalars = self.last_true_force_scalars[-5:]
+        self.true_force_scalar = ((self.true_force_vector[0])**2 + (self.true_force_vector[1])**2 + (self.true_force_vector[2])**2)**0.5
+
+    def get_sensor_force_scalar(self):
+        return self.true_force_scalar
+
+    def get_sensor_force_vector(self):
+        return self.true_force_vector
+    
+    def get_true_voltage_scalar(self) -> float:
+        return self.true_voltage_scalar
+    
+    def get_noisy_voltage_scalar(self) -> float:
+        return self.noisy_voltage_scalar
+    
+    def calc_true_voltage_scalar(self):
+        force_fraction = self.true_force_scalar / self.full_scale_force
+        full_force_voltage = self.excitation * self.scale
+        self.true_voltage_scalar = force_fraction * full_force_voltage
+
+    def calc_noisy_voltage_scalar(self):
+        # Calculate static error
+        static_error = np.random.normal(0, self.static_error_stdev)
+        # Non-linearity
+        if self.true_voltage_scalar > 1E-5:
+            nonlinearity_multiplier = self.true_voltage_scalar**(self.nonlinearity_exponent)
+        else:
+            nonlinearity_multiplier = 1
+        # Hysteresis
+        average_last = np.average(self.last_true_force_scalars)
+        #hysteresis_error = (self.true_force_scalar - average_last) / self.true_force_scalar * self.hysteresis * self.full_scale_voltage
+        hysteresis_error = copysign(1, self.true_force_scalar - average_last) * self.hysteresis * self.full_scale_voltage
+        # Repeatability
+        # self.repeatability_offset
+
+        # Thermal
+        thermal_error = self.thermal_error * self.true_voltage_scalar
+
+        self.noisy_voltage_scalar = self.true_voltage_scalar*nonlinearity_multiplier + static_error + hysteresis_error + self.repeatability_offset + thermal_error
+
+class LoadCellJoint(LoadCell):
+    def __init__(self, sensor_settings: dict={}):
+        super().__init__(sensor_settings=sensor_settings)
+        self.attached_sim_source: SharedJoint = None
+    
+    def attach_sim_source(self, sim_source: 'SharedJoint'):
+        if not isinstance(sim_source._getvalue(), SharedJoint):
+            raise TypeError(f"sim_source for LoadCellJoint {self.name} is not a SharedJoint")
+        self.attached_sim_source = sim_source
+
+    def pull_from_sim(self):
+        self.true_force_vector = self.attached_sim_source.get_force()
+
+class LoadCellSpring(LoadCell):
+    '''Provides feedback on the force along the axis of a spring.'''
+    def __init__(self, sensor_settings: dict={}):
+        super().__init__(sensor_settings=sensor_settings)
+        self.attached_sim_source: SharedSpring = None
+
+        self.unstretched_length: float = 0.0
+        self.true_length: float = 0.0
+
+    def attach_sim_source(self, sim_source: 'SharedSpring'):
+        if not isinstance(sim_source._getvalue(), SharedSpring):
+            raise TypeError(f"sim_source for LoadCellSpring {self.name} is not a SharedSpring")
+        self.attached_sim_source = sim_source
+        self.unstretched_length = sim_source.get_unstretched_length()
+
+    def pull_from_sim(self):
+        self.true_force_vector = self.attached_sim_source.get_spring_force()
+        self.calc_sensor_force_scalar()
+        self.true_length = self.attached_sim_source.get_length()
+
+    def calc_sensor_force_scalar(self):
+        self.last_true_force_scalars.append(self.true_force_scalar)
+        self.last_true_force_scalars = self.last_true_force_scalars[-5:]
+        self.true_force_scalar = ((self.true_force_vector[0])**2 + (self.true_force_vector[1])**2 + (self.true_force_vector[2])**2)**0.5
+
+        if self.true_length >= self.unstretched_length: # Tension is positive
+            self.true_force_scalar *= 1
+        else:
+            self.true_force_scalar*= -1
+
+    def calc_true_voltage_scalar(self):
+        '''Include the possibility to be negative if in tension'''
+        force_fraction = self.true_force_scalar / self.full_scale_force
+        full_force_voltage = self.excitation * self.scale
+        self.true_voltage_scalar = force_fraction * full_force_voltage
+
+
+
+class SharedLoadCell():
+    def __init__(self):
+        self.true_force_vector = np.array([0.0, 0.0, 0.0]) # Current real force on the joint
+        self.true_force_scalar: float = 0.0
+        self.true_voltage_scalar: float = 0.0
+
+        self.noisy_force_vector = np.array([0.0, 0.0, 0.0]) # Current real force on the joint
+        self.noisy_force_scalar: float = 0.0
+        self.noisy_voltage_scalar: float = 0.0
+
+        self.connected_to_plotter: bool = False
+        self.connected_to_visualization: bool = False
+        self.connected_to_sensor: bool = False
+        self.connected_to_controller: bool = False
+
+    def set_true_force_vector(self, true_force_vector):
+        self.true_force_vector = true_force_vector
+    
+    def get_true_force_vector(self) -> float:
+        return self.true_force_vector
+    
+    def set_true_force_scalar(self, true_force_scalar):
+        self.true_force_scalar = true_force_scalar
+    
+    def get_true_force_scalar(self) -> float:
+        return self.true_force_scalar
+    
+    def set_true_voltage_scalar(self, true_voltage_scalar):
+        self.true_voltage_scalar = true_voltage_scalar
+    
+    def get_true_voltage_scalar(self) -> float:
+        return self.true_voltage_scalar
+    
+    def set_noisy_voltage_scalar(self, noisy_voltage_scalar):
+        self.noisy_voltage_scalar = noisy_voltage_scalar
+    
+    def get_noisy_voltage_scalar(self) -> float:
+        return self.noisy_voltage_scalar
+    
+    def set_connected_to_plotter(self, state: bool) -> None:
+        self.connected_to_plotter = state
+    def set_connected_to_visualization(self, state: bool) -> None:
+        self.connected_to_visualization = state
+    def set_connected_to_sensor(self, state: bool) -> None:
+        self.connected_to_sensor = state
+    def set_connected_to_controller(self, state: bool) -> None:
+        self.connected_to_controller = state
+
+    def get_connected_to_plotter(self) -> bool:
+        return self.connected_to_plotter
+    def get_connected_to_visualization(self) -> bool:
+        return self.connected_to_visualization
+    def get_connected_to_sensor(self) -> bool:
+        return self.connected_to_sensor
+    def get_connected_to_controller(self) -> bool:
+        return self.connected_to_controller
+
+class SharedLoadCellJoint(SharedLoadCell):
+    def __init__(self):
+        super().__init__()
+
+class SharedLoadCellSpring(SharedLoadCell):
+    def __init__(self):
+        super().__init__()
+
+
+
+class DisplacementSensor():
+    '''Measures the displacement between 2 joints.
+    The output is the non-negative scalar distance between parent and child'''
+    def __init__(self, parent_joint: 'SharedJoint', child_joint: 'SharedJoint', sensor_settings: dict={}):
+        self.sensor_settings = sensor_settings
+        self.parse_settings()
+        self.calc_noise_parameters()
+
+        if not isinstance(parent_joint._getvalue(), SharedJoint):
+            raise TypeError(f"parent_joint for DisplacementSensor {self.name} is not a SharedJoint")
+        if not isinstance(child_joint._getvalue(), SharedJoint):
+            raise TypeError(f"child_joint for DisplacementSensor {self.name} is not a SharedJoint")
+
+        parent_joint.set_connected_to_sensor(True)
+        child_joint.set_connected_to_sensor(True)
+
+        
+        self.sim_source_parent:SharedJoint = parent_joint
+        self.sim_source_child:SharedJoint = child_joint
+        
+    
+        self.shared_attributes: SharedDisplacementSensor = None # For plotting if desired
+
+        self.length: float = 0.0                                # distance from child to parent
+        self.true_disp_scalar: float = 0.0                      # length - neutral_length
+        self.last_true_disp_scalars: list[float] = []           # Stores the last true displacements for hysteresis reasons
+        self.calc_sensor_true_disp()
+
+        self.true_voltage: float = 0.0
+        self.noisy_voltage: float = 0.0
+
+    def parse_settings(self):
+        self.name = self.sensor_settings["name"]
+        self.scale = self.sensor_settings["volts_per_meter"]
+        self.output_channel = self.sensor_settings["output_channel"]
+        self.neutral_length = self.sensor_settings["neutral_length"]
+        self.neutral_voltage = self.sensor_settings.get("neutral_voltage", 0)
+
+        self.noise_settings = self.sensor_settings.get("noise_settings", dict())
+
+        self.full_scale_voltage = self.scale * (self.sensor_settings["max_length"] - self.neutral_length)
+
+    def calc_noise_parameters(self):
+        # Calculate static error
+        self.static_error_stdev = self.full_scale_voltage * self.noise_settings.get("static_error_band", 0) / 100
+
+        # Non-linearity
+        self.nonlinearity_exponent = self.calc_nonlinearity_exponent()
+
+        # Hysteresis
+        self.hysteresis = self.noise_settings.get("hysteresis", 0) / 100
+
+        # Repeatability
+        self.repeatability_offset = np.random.normal(0, self.full_scale_voltage * self.noise_settings.get("repeatability", 0) / 100)
+
+        # Thermal applied at a given true voltage
+        self.thermal_error = self.noise_settings.get("thermal_error", 0) / 100 * self.noise_settings.get("temperature_offset", 0)
+    
+    def calc_nonlinearity_exponent(self):
+        voltage = 10
+        if self.full_scale_voltage != 1:
+            self.full_scale_voltage
+        error = self.noise_settings.get("non-linearity", 0) / 100
+        b = log(1 + error, voltage)
+        return b
+    
+    def attach_shared_attributes(self, shared_attributes):
+        if not isinstance(shared_attributes._getvalue(), SharedDisplacementSensor):
+            raise TypeError(f"shared_attributes for DisplacementSensor {self.name} is not a SharedDisplacementSensor")
+        self.shared_attributes = shared_attributes
+        self.shared_attributes.set_neutral_length(self.neutral_length)
+
+    def pull_from_sim(self):
+        '''Gets the real displacement from the shared attribute updated by the sim'''
+        parent_pos = self.sim_source_parent.get_pos()
+        child_pos = self.sim_source_child.get_pos()
+        
+        axis_vector = child_pos - parent_pos
+
+        self.length = ((axis_vector[0])**2 + (axis_vector[1])**2 + (axis_vector[2])**2)**0.5
+
+    def calc_sensor_true_disp(self):
+        self.last_true_disp_scalars.append(self.true_disp_scalar)
+        self.last_true_disp_scalars = self.last_true_disp_scalars[-5:]
+        self.true_disp_scalar = self.length - self.neutral_length
+
+    def calc_true_voltage(self):
+        self.true_voltage = self.true_disp_scalar * self.scale
+
+    def calc_noisy_voltage(self):
+        # Calculate static error
+        static_error = np.random.normal(0, self.static_error_stdev)
+        # Non-linearity
+        if self.true_voltage > 1E-5:
+            nonlinearity_multiplier = self.true_voltage**(self.nonlinearity_exponent)
+        else:
+            nonlinearity_multiplier = 1
+        # Hysteresis
+        average_last = np.average(self.last_true_disp_scalars)
+        #hysteresis_error = (self.true_force_scalar - average_last) / self.true_force_scalar * self.hysteresis * self.full_scale_voltage
+        hysteresis_error = copysign(1, self.true_disp_scalar - average_last) * self.hysteresis * self.full_scale_voltage
+        # Repeatability
+        # self.repeatability_offset
+
+        # Thermal
+        thermal_error = self.thermal_error * self.true_voltage
+
+        self.noisy_voltage = self.true_voltage*nonlinearity_multiplier + static_error + hysteresis_error + self.repeatability_offset + thermal_error
+
+    def update_shared_attributes(self):
+        if self.shared_attributes is not None:
+            self.shared_attributes.set_current_length(self.length)
+            self.shared_attributes.set_true_voltage(self.true_voltage)
+            self.shared_attributes.set_noisy_voltage(self.noisy_voltage)
+            self.shared_attributes.set_displacement(self.true_disp_scalar)
+
+    def update_internal_attributes(self):
+        self.calc_sensor_true_disp()
+        self.calc_true_voltage()
+        self.calc_noisy_voltage()
+
+    def get_true_voltage_scalar(self) -> float:
+        return self.true_voltage
+    
+    def get_noisy_voltage_scalar(self) -> float:
+        return self.noisy_voltage
+
+class SharedDisplacementSensor():
+    def __init__(self):
+        self.neutral_length: float = 0.0
+        self.length: float = 0.0
+        self.displacement: float = 0.0
+
+        self.true_voltage: float = 0.0
+        self.noisy_voltage: float = 0.0
+
+        self.connected_to_plotter: bool = False
+        self.connected_to_visualization: bool = False
+        self.connected_to_sensor: bool = False
+        self.connected_to_controller: bool = False
+    
+    def set_neutral_length(self, new:float):
+        self.neutral_length = new
+
+    def set_current_length(self, new:float):
+        self.length = new
+    
+    def set_displacement(self, new:float):
+        self.displacement = new
+
+    def set_true_voltage(self, new:float):
+        self.true_voltage = new
+
+    def set_noisy_voltage(self, new:float):
+        self.noisy_voltage = new
+
+    def set_connected_to_plotter(self, state: bool) -> None:
+        self.connected_to_plotter = state
+    def set_connected_to_visualization(self, state: bool) -> None:
+        self.connected_to_visualization = state
+    def set_connected_to_sensor(self, state: bool) -> None:
+        self.connected_to_sensor = state
+    def set_connected_to_controller(self, state: bool) -> None:
+        self.connected_to_controller = state
+
+    def get_connected_to_plotter(self) -> bool:
+        return self.connected_to_plotter
+    def get_connected_to_visualization(self) -> bool:
+        return self.connected_to_visualization
+    def get_connected_to_sensor(self) -> bool:
+        return self.connected_to_sensor
+    def get_connected_to_controller(self) -> bool:
+        return self.connected_to_controller
+    
+    def get_neutral_length(self) -> float:
+        return self.neutral_length
+    
+    def get_displacement(self) -> float:
+        return self.displacement
+
+    def get_current_length(self) -> float:
+        return self.length
+    
+    def get_true_voltage(self) -> float:
+        return self.true_voltage
+
+    def get_noisy_voltage(self) -> float:
+        return self.noisy_voltage

code/Simulation.py

@@ -0,0 +1,236 @@
+import time
+import threading
+import numpy as np
+
+from typing import List
+from Visualization import Visualization
+from Physics_Elements import Joint, Spring, RigidActuator
+
+class BFSSimulation():
+    def __init__(self, parent_joint: Joint, settings:dict):
+        if not isinstance(parent_joint, Joint):
+            raise TypeError(f"parent_joint for BFSSimulation is not a Joint")
+        
+        self.parent_joint = parent_joint
+        self.duration = settings["duration"]
+        self.delta_t = settings["delta_t"]
+        self.plotting_update_period = settings["plotting_update_period"]
+        self.sensor_update_period = settings["sensor_update_period"]
+        self.controller_pull_period = settings["controller_pull_period"]
+
+        self.joints: List[Joint] = []
+        self.springs: List[Spring] = []
+        self.actuators: List[RigidActuator] = []
+        self.rigid_actuators: List[RigidActuator] = []
+
+        self.get_joints_bfs()
+        #self.get_parent_joints_for_every_joint()
+
+        # Plotting or viz elements are updated slower than sensor
+        self.plotting_or_viz_only_shared = []
+        self.sensor_shared = []
+        self.controller_shared = []
+        self.sort_shared_attributes_into_frequencies()
+
+        self.vis:Visualization = None
+
+        self.attached_shared_time:float = None
+
+        self.sim_time:float = 0.0
+
+    def is_in_joints(self, joint: Joint):
+        for j in self.joints:
+            if joint.is_same(j):
+                return True
+        return False
+    
+    def is_in_queue(self, joint: Joint, queue):
+        for j in queue:
+            if joint.is_same(j):
+                return True
+        return False
+
+    def get_joints_bfs(self):
+        queue: List[Joint] = []
+
+        queue.append(self.parent_joint)
+
+        while queue:
+            parent_joint: Joint
+            parent_joint = queue.pop(0)
+            if parent_joint is None:
+                continue
+            if not parent_joint.is_in_list(self.joints):
+                self.joints.append(parent_joint)
+
+            connected_springs = parent_joint.get_connected_springs()
+            
+            for spring in connected_springs:
+                if not spring.is_in_list(self.springs):
+                    self.springs.append(spring)
+                child_joint = spring.get_child_joint()
+                if not self.is_in_joints(child_joint) and not self.is_in_queue(child_joint, queue):
+                    queue.append(child_joint)
+            
+            connected_actuators = parent_joint.get_connected_actuators()
+            actuator: RigidActuator = None
+            for actuator in connected_actuators:
+                if actuator not in self.actuators:
+                    self.rigid_actuators.append(actuator)
+                    if actuator.get_child_joint() not in self.joints and actuator.get_child_joint() not in queue:
+                        queue.append(actuator.get_child_joint())
+
+
+    def get_parent_joints_for_every_joint(self):
+        for joint in self.joints:
+            joint.find_parent_joints()
+
+    def get_child_joints_for_every_joint(self):
+        for joint in self.joints:
+            joint.find_child_joints()
+
+    def update_actuators_from_controller(self):
+        for rigid_actuator in self.rigid_actuators:
+            rigid_actuator.update_from_controller()
+    
+    def pull_actuator_positions_thread(self):
+        while self.spare_stop_event.is_set() == False:
+            self.update_actuators_from_controller()
+            time.sleep(self.controller_pull_period)
+
+    def update_sensor_attributes(self):
+        '''Updates the joints, springs, and actuators that are attached to sensors'''
+        for obj in self.sensor_shared:
+            obj.update_shared_attributes()
+
+    def update_sensor_attributes_thread(self):
+        while self.spare_stop_event.is_set() == False:
+            self.update_sensor_attributes()
+            time.sleep(self.sensor_update_period)
+
+    def update_plotting_or_viz_only_attributes(self):
+        '''Updates the joints, springs, and actuators that are only attached for plotting or visualization'''
+        for obj in self.plotting_or_viz_only_shared:
+            obj.update_shared_attributes()
+
+    def update_plotting_or_viz_only_attributes_thread(self):
+        while self.spare_stop_event.is_set() == False:
+            self.update_plotting_or_viz_only_attributes()
+            time.sleep(self.plotting_update_period)
+
+    def sort_shared_attributes_into_frequencies(self):
+        '''For each shared attribute connected to anything, put them in lists so we know how fast we need to update them.'''
+        for joint in self.joints:
+            shared = joint.shared_attributes
+            if shared is None:
+                continue
+            plot = shared.get_connected_to_plotter()
+            vis = shared.get_connected_to_visualization()
+            sens = shared.get_connected_to_sensor()
+            contr = shared.get_connected_to_controller()
+
+            if sens:
+                self.sensor_shared.append(joint)
+            elif plot or vis:
+                self.plotting_or_viz_only_shared.append(joint)
+            
+            if contr:
+                self.controller_shared.append(joint)
+        
+        for spring in self.springs:
+            shared = spring.shared_attributes
+            if shared is None:
+                continue
+            plot = shared.get_connected_to_plotter()
+            vis = shared.get_connected_to_visualization()
+            sens = shared.get_connected_to_sensor()
+            contr = shared.get_connected_to_controller()
+
+            if sens:
+                self.sensor_shared.append(spring)
+            elif plot or vis:
+                self.plotting_or_viz_only_shared.append(spring)
+            
+            if contr:
+                self.controller_shared.append(spring)
+
+        for actuator in self.rigid_actuators:
+            shared = actuator.shared_attributes
+            if shared is None:
+                continue
+            plot = shared.get_connected_to_plotter()
+            vis = shared.get_connected_to_visualization()
+            sens = shared.get_connected_to_sensor()
+            contr = shared.get_connected_to_controller()
+
+            if sens:
+                self.sensor_shared.append(actuator)
+            elif plot or vis:
+                self.plotting_or_viz_only_shared.append(actuator)
+            
+            if contr:
+                self.controller_shared.append(actuator)
+
+    def attach_shared_time(self, attached_shared_time):
+        self.attached_shared_time = attached_shared_time
+
+    def run_process(self):
+
+        time_values = []
+
+        self.sim_time = 0
+        #time_last = time.perf_counter()
+        time_last = time.time()
+        count = 0
+
+        self.spare_stop_event = threading.Event()
+        self.spare_stop_event.clear()
+        pull_actuator_thread = threading.Thread(target=self.pull_actuator_positions_thread)
+        pull_actuator_thread.start()
+        update_plotting_or_viz_only_thread = threading.Thread(target=self.update_plotting_or_viz_only_attributes_thread)
+        update_plotting_or_viz_only_thread.start()
+        update_sensor_thread = threading.Thread(target=self.update_sensor_attributes_thread)
+        update_sensor_thread.start()
+
+        while self.sim_time < self.duration:
+            count += 1
+            if self.delta_t is None:
+                #new_time = time.perf_counter()
+                new_time = time.time()
+                delta_time = new_time - time_last
+                time_last = new_time
+            
+            for spring in self.springs:
+                spring.calc_spring_force()
+            
+            joint: Joint
+            for joint in self.joints:
+                if joint.fixed == True:
+                    continue
+                # Get joint forces
+                joint.calc_net_force(self.sim_time)
+                # Get joint accels.
+                joint.calc_accel()
+                # Integrate joint vel and pos
+                if self.delta_t is None:
+                    joint.integrate_statespace(delta_time)
+                else:
+                    joint.integrate_statespace(self.delta_t)
+
+            
+            time_values.append(self.sim_time)
+
+            if self.delta_t is None:
+                self.sim_time += delta_time
+            else:
+                self.sim_time += self.delta_t
+
+            # Update the shared sim time as fast as possible
+            self.attached_shared_time.set(self.sim_time)
+        
+        self.spare_stop_event.set()
+        pull_actuator_thread.join()
+        update_plotting_or_viz_only_thread.join()
+        update_sensor_thread.join()
+
+        print(f"Average delta_t = {self.sim_time / count}")

code/Visualization.py

@@ -0,0 +1,594 @@
+import vpython
+import time
+from copy import deepcopy
+
+import multiprocessing
+import threading
+
+import pyqtgraph as pg
+from pyqtgraph.Qt import QtWidgets, QtCore
+
+
+from Physics_Elements import Joint, Spring, SharedRigidActuator, SharedSpring, SharedJoint
+from Sensor_Elements import SharedLoadCellJoint, SharedLoadCellSpring, SharedDisplacementSensor
+from Controller_Input_Elements import SharedRigidActuatorController
+
+from typing import TYPE_CHECKING
+if TYPE_CHECKING:
+    from Object_Sharing import SharedFloat
+
+
+'''
+TODO
+2. Make this in a separate thread
+'''
+class Visualization():
+    def __init__(self, stop_event, scene_settings:dict):
+        self.stop_event = stop_event
+        self.scene_settings = scene_settings
+
+        self.attached_attributes = []
+        self.shared_attributes_object_settings = []
+
+        self.vpython_objects = []
+
+    def draw_scene(self):
+        vpython.scene = vpython.canvas(width=self.scene_settings["canvas_width"], height=self.scene_settings["canvas_height"])
+        side = self.scene_settings["cube_size"]
+        thk = self.scene_settings["wall_thickness"]
+
+        wall_length = side + 2*thk
+
+        
+
+        wall_bottom = vpython.box(pos=vpython.vector(0, -thk/2, 0), size=vpython.vector(wall_length, thk, wall_length), color=vpython.color.gray(0.9))
+
+        wall_left = vpython.box(pos=vpython.vector(-(side + thk)/2, side/2, 0), size=vpython.vector(thk, side, side), color=vpython.color.gray(0.7))
+        wall_right = vpython.box(pos=vpython.vector((side + thk)/2, side/2, 0), size=vpython.vector(thk, side, side), color=vpython.color.gray(0.7))
+        
+        wall_back = vpython.box(pos=vpython.vector(0, (side)/2, -(side+ thk)/2), size=vpython.vector(wall_length, side, thk), color=vpython.color.gray(0.7))
+
+    def attach_shared_attributes(self, shared_attributes, object_settings):
+        self.attached_attributes.append(shared_attributes)
+        self.shared_attributes_object_settings.append(object_settings)
+
+        shared_attributes.set_connected_to_visualization(True)
+
+    def generate_vpython_objects(self):
+        for i in range(len(self.attached_attributes)):
+            object_settings = self.shared_attributes_object_settings[i]
+            
+            if object_settings["type"] == "sphere":
+                shared_attribute:Joint = self.attached_attributes[i]
+                object_pos = deepcopy(shared_attribute.get_pos())
+                object_pos = vpython.vector(object_pos[0], object_pos[1], object_pos[2])
+
+                object_color = self.return_vpython_color(object_settings["color"])
+
+                self.vpython_objects.append(vpython.sphere(pos=object_pos, radius=object_settings["radius"], color=object_color))
+            elif object_settings["type"] == "helix":
+                shared_attribute:Spring = self.attached_attributes[i]
+                object_pos = deepcopy(shared_attribute.get_pos())
+                object_pos = vpython.vector(object_pos[0], object_pos[1], object_pos[2])
+
+                object_axis = deepcopy(shared_attribute.get_axis_vector())
+                object_axis = vpython.vector(object_axis[0], object_axis[1], object_axis[2])
+
+                object_color = self.return_vpython_color(object_settings["color"])
+
+                self.vpython_objects.append(vpython.helix(pos=object_pos, axis=object_axis, radius=object_settings["radius"], thickness=object_settings["thickness"], color=object_color))
+            elif object_settings["type"] == "cylinder":
+                shared_attribute:Spring = self.attached_attributes[i]
+                object_pos = deepcopy(shared_attribute.get_pos())
+                object_pos = vpython.vector(object_pos[0], object_pos[1], object_pos[2])
+
+                object_axis = deepcopy(shared_attribute.get_axis_vector())
+                object_axis = vpython.vector(object_axis[0], object_axis[1], object_axis[2])
+
+                object_color = self.return_vpython_color(object_settings["color"])
+
+                self.vpython_objects.append(vpython.cylinder(pos=object_pos, axis=object_axis, radius=object_settings["radius"], color=object_color))
+        
+    def return_vpython_color(self, color_str):
+        if color_str == "blue":
+            return vpython.color.blue
+        elif color_str == "black":
+            return vpython.color.black
+        elif color_str == "red":
+            return vpython.color.red
+        elif color_str == "green":
+            return vpython.color.green
+        elif color_str == "white":
+            return vpython.color.white
+
+    def update_scene(self):
+        for i in range(len(self.vpython_objects)):
+            element = self.vpython_objects[i]
+            shared_attributes = self.attached_attributes[i]
+            if isinstance(element, vpython.sphere):
+                updated_pos = shared_attributes.get_pos()
+                element.pos = deepcopy(vpython.vector(*(updated_pos)))
+            elif isinstance(element, vpython.helix):
+                updated_pos = shared_attributes.get_pos()
+                element.pos = deepcopy(vpython.vector(*(updated_pos)))
+                updated_axis = shared_attributes.get_axis_vector()
+                element.axis = deepcopy(vpython.vector(*(updated_axis)))
+            elif isinstance(element, vpython.cylinder):
+                updated_pos = shared_attributes.get_pos()
+                element.pos = deepcopy(vpython.vector(*(updated_pos)))
+                updated_axis = shared_attributes.get_axis_vector()
+                element.axis = deepcopy(vpython.vector(*(updated_axis)))
+
+    def run_process(self):
+        # Draw scene
+        self.draw_scene()
+        
+        # Generate vpython objects from settings
+        self.generate_vpython_objects()
+
+        
+
+        while self.stop_event.is_set() == False:
+            # Update pos and axes of all vpython objects
+            self.update_scene()
+
+
+
+class Plotter():
+    def __init__(self, plot_settings: dict, stop_event: multiprocessing.Event):
+        self.plot_settings = plot_settings
+        self.title = plot_settings["title"]
+        self.window_length = self.plot_settings["window_length"]
+        self.update_interval = self.plot_settings["update_interval"]
+        self.pull_interval = self.plot_settings["pull_interval"]
+        self.stop_event: multiprocessing.Event = stop_event
+
+        self.attached_attributes = []
+        self.shared_attributes_plot_settings = []
+
+        self.pull_data_stop_event: threading.Event = None
+
+        self.shared_x: SharedFloat = None
+
+        self.plot_setup = {}
+
+    def attach_shared_attributes(self, shared_attributes, plot_settings):
+        self.attached_attributes.append(shared_attributes)
+        self.shared_attributes_plot_settings.append(plot_settings)
+
+        shared_attributes.set_connected_to_plotter(True)
+
+    def attach_shared_x(self, shared_x):
+        self.shared_x = shared_x
+    
+    def pull_data_thread(self):
+        while self.pull_data_stop_event.is_set() == False:
+            self.pull_data()
+            time.sleep(self.pull_interval / 1000)
+
+    def pull_data(self):
+        self.raw_xdata.append(self.shared_x.get())
+
+        for i in range(len(self.shared_attributes_plot_settings)):
+            foo = self.attached_attributes[i]._getvalue()
+            for key, value in self.shared_attributes_plot_settings[i].items():
+                if isinstance(foo, SharedLoadCellJoint):
+                    bar: SharedLoadCellJoint = self.attached_attributes[i]
+                    if key == "force":
+                        force_vector = bar.get_true_force_vector()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel: str = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[2])
+                    elif key == "true_voltage":
+                        voltage_scalar = bar.get_true_voltage_scalar()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(voltage_scalar)
+                    elif key == "noisy_voltage":
+                        voltage_scalar = bar.get_noisy_voltage_scalar()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(voltage_scalar)
+                elif isinstance(foo, SharedLoadCellSpring):
+                    bar: SharedLoadCellSpring = self.attached_attributes[i]
+                    if key == "force":
+                        force_vector = bar.get_true_force_vector()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[2])
+                            elif subkey == "scalar":
+                                force_scalar = bar.get_true_force_scalar()
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_scalar)
+                    elif key == "true_voltage":
+                        voltage_scalar = bar.get_true_voltage_scalar()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(voltage_scalar)
+                    elif key == "noisy_voltage":
+                        voltage_scalar = bar.get_noisy_voltage_scalar()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(voltage_scalar)
+                elif isinstance(foo, SharedRigidActuator):
+                    bar: SharedRigidActuator = self.attached_attributes[i]
+                    if key == "pos":
+                        pos_vector = bar.get_pos()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(pos_vector[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(pos_vector[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(pos_vector[2])
+                    if key == "vel":
+                        pos_vector = self.attached_attributes[i].get_vel()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(pos_vector[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(pos_vector[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(pos_vector[2])
+                elif isinstance(foo, SharedRigidActuatorController):
+                    bar: SharedRigidActuatorController = self.attached_attributes[i]
+                    if key == "input_voltage":
+                        input_voltage = bar.get_input_voltage()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(input_voltage)
+                    elif key == "digital_input":
+                        digital_input = bar.get_digital_command()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(digital_input)
+                    elif key == "pos":
+                        controlled_vel = bar.get_controlled_pos()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(controlled_vel[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(controlled_vel[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(controlled_vel[2])
+                    elif key == "vel":
+                        controlled_vel = bar.get_controlled_vel()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(controlled_vel[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(controlled_vel[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(controlled_vel[2])
+                            if subkey == "scalar":
+                                vel_scalar = bar.get_controlled_vel_scalar()
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(vel_scalar)
+                    elif key == "disp":
+                        disp_scalar = bar.get_controlled_length()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(disp_scalar)
+                elif isinstance(foo, SharedDisplacementSensor):
+                    bar: SharedDisplacementSensor = self.attached_attributes[i]
+                    if key == "voltage":
+                        for subkey, settings in value.items():
+                            if subkey == "noisy":
+                                voltage_scalar = bar.get_noisy_voltage()
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(voltage_scalar)
+                            elif subkey == "true":
+                                voltage_scalar = bar.get_true_voltage()
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(voltage_scalar)
+                    if key == "disp":
+                        disp_scalar = bar.get_displacement()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(disp_scalar)
+                    elif key == "length":
+                        length_scalar = bar.get_current_length()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(length_scalar)
+                elif isinstance(foo, SharedSpring):
+                    bar: SharedSpring = self.attached_attributes[i]
+                    if key == "force":
+                        force_vector = bar.get_spring_force()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[2])
+                            elif subkey == "scalar":
+                                force_scalar = bar.get_spring_force_scalar()
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_scalar)
+                    elif key == "length":
+                        length = bar.get_length()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(length)
+                elif isinstance(foo, SharedJoint):
+                    bar: SharedJoint = self.attached_attributes[i]
+                    if key == "accel":
+                        accel_vector = bar.get_accel()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(accel_vector[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(accel_vector[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(accel_vector[2])
+                    elif key == "vel":
+                        vel_vector = bar.get_vel()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(vel_vector[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(vel_vector[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(vel_vector[2])
+                    elif key == "pos":
+                        pos_vector = bar.get_pos()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(pos_vector[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(pos_vector[1])
+                            if subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(pos_vector[2])
+                    elif key == "force":
+                        force_vector = bar.get_spring_force()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[2])
+
+                            
+    def update_live_window(self):
+        if self.stop_event.is_set():
+            self.timer.stop()
+            self.pull_data_stop_event.set()
+            return
+        
+        final_time = self.raw_xdata[-1]
+        target_time = final_time - self.window_length
+        closest_index = min(range(len(self.raw_xdata)), key=lambda i: abs(self.raw_xdata[i] - target_time))
+        self.window_xdata = self.raw_xdata[closest_index:]
+
+        # Attach the data to the appropriate sublines in the subplots
+        for i in range(self.num_plots):  
+            for j in range(len(self.subplot_keys[i])):
+                window_ydata = self.raw_ydata[i][j][closest_index:]
+                if self.plot_settings["step_or_plot"] == "plot":
+                    if len(self.window_xdata) == len(window_ydata):
+                        self.subplot_curves[i][j].setData(self.window_xdata, window_ydata)
+                    elif len(self.window_xdata) > len(window_ydata):
+                        self.subplot_curves[i][j].setData(self.window_xdata[:-1], window_ydata)
+                elif self.plot_settings["step_or_plot"] == "step":
+                    if len(self.window_xdata) == len(window_ydata):
+                        self.subplot_curves[i][j].setData(self.window_xdata, window_ydata[:-1])
+                    elif len(self.window_xdata) > len(window_ydata):
+                        self.subplot_curves[i][j].setData(self.window_xdata[:-1], window_ydata[:-1])
+
+    def generate_plots(self):
+        # Create the application
+        self.app = QtWidgets.QApplication([])
+
+        # Create a plot window
+        self.win = pg.GraphicsLayoutWidget(show=True, title=self.title)
+        self.win.resize(1000, 600)
+        self.win.setWindowTitle(self.plot_settings["title"])
+
+        self.num_plots = self.plot_settings["num_subplots"]
+
+        self.plots = []
+
+        for i in range(self.num_plots):
+            plot = self.win.addPlot(title=self.plot_settings["subplots"][i]["ylabel"])
+            plot.setLabel('left', self.plot_settings["subplots"][i]["ylabel"])
+            plot.addLegend()
+            self.plots.append(plot)
+
+            # Move us to the next row for the subplot
+            if i < self.num_plots - 1:
+                self.win.nextRow()
+        
+    def generate_curves(self):
+        self.subplot_keys = [[] for _ in range(self.num_plots)] # list for each subplot
+        self.subplot_curves = [[] for _ in range(self.num_plots)] # list for each subplot
+        self.raw_xdata = []
+        self.raw_ydata = [[] for _ in range(self.num_plots)] # list for each subplot
+        self.raw_data_map = {}
+        for setting in self.shared_attributes_plot_settings:
+            '''
+            Each setting looks like {
+                "accel": {
+                    "x": {
+                        "subplot": 1,
+                        "ylabel": "Main Mass x-Accel"
+                    },
+                    "y": {
+                        "subplot": 1,
+                        "ylabel": "Main Mass y-Accel"
+                    }
+                },
+                "vel": {
+                    "x": {
+                        "subplot": 2,
+                        "ylabel": "Main Mass x-Vel"
+                    }
+                },
+                "pos": {
+                    "x": {
+                        "subplot": 3,
+                        "ylabel": "Main Mass x-Vel"
+                    }
+                }
+            }
+            '''
+            for key, value in setting.items():
+                ''' key would be like "accel"
+                    value would be like {
+                    "x": {
+                        "subplot": 1,
+                        "ylabel": "Main Mass x-Accel"
+                    },
+                    "y": {
+                        "subplot": 1,
+                        "ylabel": "Main Mass y-Accel"
+                    }
+                }
+                '''
+                for line, line_settings in value.items():
+                    ''' line would be like "x"
+                    line_settings would be like {
+                        "subplot": 1,
+                        "ylabel": "Main Mass x-Accel",
+                        "color": "w"
+                    }
+                    '''
+                    subplot = line_settings["subplot"]
+                    label = line_settings["ylabel"]
+                    color = line_settings.get("color", "w")  # Default to white if no color is specified
+                    self.subplot_keys[subplot].append(label)
+                    if self.plot_settings["step_or_plot"] == "plot":
+                        self.subplot_curves[subplot].append(self.plots[subplot].plot(name=label, pen=pg.mkPen(color=color)))
+                    elif self.plot_settings["step_or_plot"] == "step":
+                        self.subplot_curves[subplot].append(self.plots[subplot].plot(name=label, stepMode=True, pen=pg.mkPen(color=color)))
+
+                    self.raw_data_map[label] = (subplot, len(self.raw_ydata[subplot]))
+                    self.raw_ydata[subplot].append([])
+
+        self.window_xdata = []
+        self.window_ydata = [[] for _ in range(self.num_plots)] # list for each subplot
+
+    def show_plot(self):
+        pass
+
+    def run_process(self):
+        # Generate the figure and subplots
+        self.generate_plots()
+
+        # Generate the curves
+        self.generate_curves()
+
+        self.pull_data_stop_event = threading.Event()
+        self.pull_data_stop_event.clear()
+        pull_data_thread = threading.Thread(target=self.pull_data_thread)
+        pull_data_thread.start()
+
+        self.timer = QtCore.QTimer()
+        self.timer.timeout.connect(self.update_live_window)
+        self.timer.start(self.update_interval)
+
+        # Start the Qt event loop
+        QtWidgets.QApplication.instance().exec_()
+
+        pull_data_thread.join()
+        

code/_example_1d_mass_spring_oscillator.py

@@ -0,0 +1,268 @@
+import numpy as np
+import time
+import multiprocessing
+from multiprocessing.managers import BaseManager
+
+
+from Visualization import Visualization, Plotter
+from Simulation import BFSSimulation
+from Physics_Elements import Joint, Spring, SharedJoint, SharedSpring
+from Object_Sharing import SharedFloat
+
+
+
+
+
+def main():
+    '''Setting up the BaseManager so data can be shared between processes'''
+    base_manager = BaseManager()
+
+    BaseManager.register('SharedFloat', SharedFloat)
+    BaseManager.register('SharedJoint', SharedJoint)
+    BaseManager.register('SharedSpring', SharedSpring)
+
+    base_manager.start()
+
+
+
+
+
+
+    '''Creating instances for plotter and visualizer'''
+    # Setup the plotter and visualization processes
+    stop_event = multiprocessing.Event()
+    stop_event.clear()
+
+    # Create a synchronized time between the processes so the plotter and physics sim are in sync
+    shared_sim_time:SharedFloat = base_manager.SharedFloat()
+    shared_sim_time.set(0.0) # Set the initial time = 0.0 seconds
+
+
+    # Create a real-time plotter for physics plotting. 1 for the mass and the other for the spring
+    mass_plotter = Plotter(
+        stop_event = stop_event,
+        plot_settings = {
+            "title": "Joint Plotter",
+            "xlabel": "Time (s)",
+            "num_subplots": 3,
+            "step_or_plot": "plot",
+            "subplots": [
+                {
+                    "ylabel": "Position (m)"
+                },
+                {
+                    "ylabel": "Velocity (m/s)"
+                },
+                {
+                    "ylabel": "Accel (m/s/s)"
+                }
+            ],
+            "window_length": 100,               # Keep 100 seconds visible
+            "pull_interval": 10,                # Pull data every 10 millisecond
+            "update_interval": 100              # Update the graph every 100 milliseconds
+        }
+    )
+    mass_plotter.attach_shared_x(shared_sim_time)
+
+    spring_plotter = Plotter(
+        stop_event = stop_event,
+        plot_settings = {
+            "title": "Spring Plotter",
+            "xlabel": "Time (s)",
+            "num_subplots": 2,
+            "step_or_plot": "plot",
+            "subplots": [
+                {
+                    "ylabel": "Length (m)"
+                },
+                {
+                    "ylabel": "Force (N)"
+                }
+            ],
+            "window_length": 100,               # Keep 100 seconds visible
+            "pull_interval": 10,                # Pull data every 10 milliseconds
+            "update_interval": 100              # Update the graph every 100 milliseconds
+        }
+    )
+    spring_plotter.attach_shared_x(shared_sim_time)
+    
+
+    # Create a 3D visualization for the entire model
+    visualizer = Visualization(
+        stop_event = stop_event,
+        scene_settings = {
+            "canvas_width": 1600,
+            "canvas_height": 1000,
+            "wall_thickness": 0.1,
+            "cube_size": 20
+        }
+    )
+
+
+
+
+
+
+    '''Setting up physics simulation'''
+    # Create the physics elements
+    main_mass = Joint(
+        pos = np.array([0, 5, 0]),
+        mass = 5,
+        fixed = False,
+        name = "Main mass",
+        integration_method = "adams-bashforth"
+    )
+
+    # Before we create a spring, we need to create the Joint on the wall for the Spring to mount to
+    wall_joint = Joint(
+        pos = np.array([-10, 5, 0]),
+        mass = 1,                           # Does not matter because it is fixed
+        fixed = True,                       # We do not want it to ever move
+        name = "Wall Joint"
+    )
+
+
+    spring = Spring(
+        parent_joint = main_mass,
+        child_joint = wall_joint,
+        unstretched_length = 13,
+        constant_stiffness = 100,
+        name = "Spring"
+    )
+
+
+
+
+
+
+    '''Adding items to be plotted'''
+    # Since we want to plot the physics of the mass and spring, we need shared attributes for them
+    shared_main_mass: SharedJoint = base_manager.SharedJoint()
+    main_mass.attach_shared_attributes(shared_main_mass)
+
+    shared_spring: SharedSpring = base_manager.SharedSpring()
+    spring.attach_shared_attributes(shared_spring)
+
+    # Attach the shared elements to the plotter
+    mass_plotter.attach_shared_attributes(
+        shared_attributes = shared_main_mass,
+        plot_settings = {
+            "pos": {
+                "x": {
+                    "subplot": 0,
+                    "ylabel": "Main Poss x-Pos",
+                    "color": "r"
+                }
+            },
+            "vel": {
+                "x": {
+                    "subplot": 1,
+                    "ylabel": "Main Poss x-Vel",
+                    "color": "g"
+                }
+            },
+            "accel": {
+                "x": {
+                    "subplot": 2,
+                    "ylabel": "Main Poss x-Accel",
+                    "color": "b"
+                }
+            }
+        }
+    )
+
+    spring_plotter.attach_shared_attributes(
+        shared_attributes = shared_spring,
+        plot_settings = {
+            "length": {
+                "scalar": {
+                    "subplot": 0,
+                    "ylabel": "Spring Length",
+                    "color": "r"
+                }
+            },
+            "force": {
+                "x": {
+                    "subplot": 1,
+                    "ylabel": "Spring x-Force",
+                    "color": "g"
+                }
+            }
+        }
+    )
+
+
+
+
+
+
+
+    '''Adding items to be visualized'''
+    # We aready have shared_main_mass and shared_spring from the plotting, so there is no need to make more shared elements
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_main_mass,
+        object_settings = {
+            "type": "sphere",
+            "color": "red",
+            "radius": 0.5,
+        }
+    )
+    
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_spring,
+        object_settings = {
+            "type": "helix",
+            "color": "white",
+            "radius": 0.5,
+            "thickness": 0.1
+        }
+    )
+
+
+
+
+    
+
+    '''Create the physics simulation'''
+    simulation = BFSSimulation(
+        parent_joint = main_mass,           # Because spring is a child of main_mass, it will be reached by BFS
+        settings = {
+            "duration": 1000,               # Run the sim for 1000 seconds
+            "delta_t": None,                # Run in real-time
+            "plotting_update_period": 0.01, # Update the plottable elements every 0.01 seconds
+            "sensor_update_period": 0.01,      # Update the sensor elements every second (because we have none, this doesn't matter)
+            "controller_pull_period": 0.01     # Update the controller elements every seconds (again, we have none)
+        }
+    )
+    simulation.attach_shared_time(shared_sim_time)
+    
+
+
+
+
+    '''Setup the processes and run them'''
+    mass_plotter_process = multiprocessing.Process(target=mass_plotter.run_process)
+    spring_plotter_process = multiprocessing.Process(target=spring_plotter.run_process)
+    visualization_process = multiprocessing.Process(target=visualizer.run_process)
+    simulation_process = multiprocessing.Process(target=simulation.run_process)
+
+    mass_plotter_process.start()
+    spring_plotter_process.start()
+    visualization_process.start()
+
+    time.sleep(5)                           # Give the plotters and visualizer time to startup before the physics sim runs.
+
+    # Join the process
+    simulation_process.start()
+    simulation_process.join()               # This blocks until the simulation finishes
+
+
+    mass_plotter_process.join()
+    spring_plotter_process.join()
+    visualization_process.join()
+
+    # Close the manager
+    base_manager.shutdown()
+
+if __name__ == "__main__":
+    main()

code/_example_1d_mass_spring_oscillator_with_actuator.py

@@ -0,0 +1,430 @@
+import numpy as np
+import time
+import multiprocessing
+from multiprocessing.managers import BaseManager
+
+
+from Visualization import Visualization, Plotter
+from Simulation import BFSSimulation
+from Physics_Elements import Joint, Spring, RigidActuator, SharedJoint, SharedSpring, SharedRigidActuator
+
+from HITL_Controller import HITLController
+from Controller_Input_Elements import RigidActuatorController, SharedRigidActuatorController
+from Sensor_Elements import DisplacementSensor, LoadCellSpring, SharedDisplacementSensor, SharedLoadCellSpring
+
+from Object_Sharing import SharedFloat
+
+
+
+
+
+def main():
+    '''Setting up the BaseManager so data can be shared between processes'''
+    base_manager = BaseManager()
+
+    BaseManager.register('SharedFloat', SharedFloat)
+    BaseManager.register('SharedJoint', SharedJoint)
+    BaseManager.register('SharedSpring', SharedSpring)
+
+    BaseManager.register('SharedRigidActuator', SharedRigidActuator)
+
+    BaseManager.register('SharedRigidActuatorController', SharedRigidActuatorController)
+
+    BaseManager.register('SharedDisplacementSensor', SharedDisplacementSensor)
+    BaseManager.register('SharedLoadCellSpring', SharedLoadCellSpring)
+
+    base_manager.start()
+
+
+
+
+
+
+    '''Creating instances for plotter and visualizer'''
+    # Setup the plotter and visualization processes
+    stop_event = multiprocessing.Event()
+    stop_event.clear()
+
+    # Create a synchronized time between the processes so the plotter and physics sim are in sync
+    shared_sim_time:SharedFloat = base_manager.SharedFloat()
+    shared_sim_time.set(0.0) # Set the initial time = 0.0 seconds
+
+
+    # Create a real-time plotter for physics plotting.
+    physics_plotter = Plotter(
+        stop_event = stop_event,
+        plot_settings = {
+            "title": "Physics Plotter",
+            "xlabel": "Time (s)",
+            "num_subplots": 4,
+            "step_or_plot": "plot",
+            "subplots": [
+                {
+                    "ylabel": "Position (m)"
+                },
+                {
+                    "ylabel": "Velocity (m/s)"
+                },
+                {
+                    "ylabel": "Accel (m/s/s)"
+                },
+                {
+                    "ylabel": "Force (N)"
+                },
+            ],
+            "window_length": 10,               # Keep 100 seconds visible
+            "pull_interval": 10,                # Pull data every 10 millisecond
+            "update_interval": 100              # Update the graph every 100 milliseconds
+        }
+    )
+    physics_plotter.attach_shared_x(shared_sim_time)
+
+    # Create a real-time plotter for the controller input plotting.
+    controller_plotter = Plotter(
+        stop_event = stop_event,
+        plot_settings = {
+            "title": "Controller Plotter",
+            "xlabel": "Time (s)",
+            "num_subplots": 3,
+            "step_or_plot": "plot",
+            "subplots": [
+                {
+                    "ylabel": "Controller Input (V)",
+                },
+                {
+                    "ylabel": "Digital Input"
+                },
+                {
+                    "ylabel": "Controlled Pos (m)",
+                },
+            ],
+            "window_length": 10,               # Keep 100 seconds visible
+            "pull_interval": 10,                # Pull data every 10 millisecond
+            "update_interval": 100              # Update the graph every 100 milliseconds
+        }
+    )
+    controller_plotter.attach_shared_x(shared_sim_time)
+
+    # Create a real-time plotter for the sensor output plotting
+    sensor_plotter = Plotter(stop_event=stop_event, plot_settings={
+        "title": "Sensor Plotter",
+        "xlabel": "Time (s)",
+        "num_subplots": 2,
+        "step_or_plot": "plot",
+        "subplots": [
+            {
+                "ylabel": "Actuator Length (m)",
+            },
+            {
+                "ylabel": "Sensor Output (V)",
+            },
+        ],
+        "window_length": 10, # Max data points to keep on the plot
+        "pull_interval": 10, # in milliseconds
+        "update_interval": 100, # In milliseconds.
+        })
+    sensor_plotter.attach_shared_x(shared_sim_time)
+
+    # Create a 3D visualization for the entire model
+    visualizer = Visualization(
+        stop_event = stop_event,
+        scene_settings = {
+            "canvas_width": 1600,
+            "canvas_height": 1000,
+            "wall_thickness": 0.1,
+            "cube_size": 20
+        }
+    )
+
+
+    '''Creating an instance for the HITLController'''
+    hitl_controller = HITLController(
+        stop_event = stop_event,
+        settings = {
+            "pull_from_sim_period": 5,
+            "plotting_update_period": 10
+        }
+    )
+
+
+
+    '''Setting up physics simulation'''
+    # Create the physics elements
+    main_mass = Joint(
+        pos = np.array([0, 5, 0]),
+        mass = 5,
+        fixed = False,
+        name = "Main mass",
+        integration_method = "adams-bashforth"
+    )
+
+    # Since we want to plot the physics of the mass we need shared attributes for it
+    shared_main_mass: SharedJoint = base_manager.SharedJoint()
+    main_mass.attach_shared_attributes(shared_main_mass)
+    physics_plotter.attach_shared_attributes(
+        shared_attributes = shared_main_mass,
+        plot_settings = {
+            "pos": {
+                "x": {
+                    "subplot": 0,
+                    "ylabel": "Main Poss x-Pos",
+                    "color": "r"
+                }
+            },
+            "vel": {
+                "x": {
+                    "subplot": 1,
+                    "ylabel": "Main Poss x-Vel",
+                    "color": "g"
+                }
+            },
+            "accel": {
+                "x": {
+                    "subplot": 2,
+                    "ylabel": "Main Poss x-Accel",
+                    "color": "b"
+                }
+            }
+        }
+    )
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_main_mass,
+        object_settings = {
+            "type": "sphere",
+            "color": "red",
+            "radius": 0.5,
+        }
+    )
+    
+    # Before we create a spring, we need to create the Joint on the wall for the Spring to mount to
+    north_wall_joint = Joint(
+        pos = np.array([10, 5, 0]),
+        mass = 1,                           # Does not matter because it is fixed
+        fixed = True,                       # We do not want it to ever move
+        name = "North Wall Joint"
+    )
+
+    north_spring = Spring(
+        parent_joint = main_mass,
+        child_joint = north_wall_joint,
+        unstretched_length = 13,
+        constant_stiffness = 100,
+        name = "North Spring"
+    )
+    shared_north_spring: SharedSpring = base_manager.SharedSpring()
+    north_spring.attach_shared_attributes(shared_north_spring)
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_north_spring,
+        object_settings = {
+            "type": "helix",
+            "color": "white",
+            "radius": 0.5,
+            "thickness": 0.1
+        }
+    )
+
+    # Setting up the actuator. This requires the "grounded_joint" to fix to. We also want a spring in between the actuator and main_mass
+    actuator_joint = Joint(
+        pos = np.array([-5, 5, 0]),
+        mass = 1,                           # Does not matter because it is fixed to an actuator
+        fixed = False,                      # We do not want it to ever move
+        name = "Actuator Joint"
+    )
+    
+    actuator_spring = Spring(
+        parent_joint = main_mass,
+        child_joint = actuator_joint,
+        unstretched_length = 5,
+        constant_stiffness = 150,
+        name = "Actuator Spring"
+    )
+    shared_actuator_spring: SharedSpring = base_manager.SharedSpring()
+    actuator_spring.attach_shared_attributes(shared_actuator_spring)
+    physics_plotter.attach_shared_attributes(
+        shared_attributes = shared_actuator_spring,
+        plot_settings = {
+            "force": {
+                "scalar": {
+                    "subplot": 3,
+                    "ylabel": "Actuator Spring Force",
+                    "color": "r"
+                }
+            },
+        }
+    )
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_actuator_spring,
+        object_settings = {
+            "type": "helix",
+            "color": "white",
+            "radius": 0.5,
+            "thickness": 0.1
+        }
+    )
+
+    south_wall_joint = Joint(
+        pos = np.array([-10, 5, 0]),
+        mass = 1,                           # Does not matter because it is fixed
+        fixed = True,                       # We do not want it to ever move
+        name = "South Wall Joint"
+    )
+
+    rigid_actuator = RigidActuator(
+        parent_joint = actuator_joint,
+        grounded_joint = south_wall_joint,
+        name = "Rigid Actuator",
+        max_length = 8,
+        min_length = 2,
+        control_code = 0                    # Position Control
+    )
+    shared_rigid_actuator: SharedRigidActuator = base_manager.SharedRigidActuator()
+    rigid_actuator.attach_shared_attributes(shared_rigid_actuator)
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_rigid_actuator,
+        object_settings = {
+            "type": "cylinder",
+            "color": "black",
+            "radius": 0.5,
+        }
+    )
+
+    # We have an actuator, but it currently does nothing without the RigidActuatorController
+        # We will add the Controller and a DisplacementSensor for something in the real-world to control with
+    rigid_actuator_controller = RigidActuatorController(
+        controller_settings = {
+            "name": "Rigid Actuator Controller",
+            "analog_input_channel": 0,
+            "digital_input_channel": 0,
+            "units_per_volt": 2,                    # 2 meters per volt
+            "neutral_length": 5,
+            "neutral_voltage": 2.5,                 # At 2.5V, go to neutral_length of 5
+            "controls_pos/vel/accel": 0,            # Controls position
+            "max_length": 8,
+            "min_length": 2
+        }
+    )
+    rigid_actuator_controller.attach_sim_target(shared_rigid_actuator)
+    hitl_controller.attach_rigid_actuator_controller(rigid_actuator_controller)
+
+    shared_rigid_actuator_controller: SharedRigidActuatorController = base_manager.SharedRigidActuatorController()
+    rigid_actuator_controller.attach_shared_attributes(shared_rigid_actuator_controller)
+    controller_plotter.attach_shared_attributes(
+        shared_attributes = shared_rigid_actuator_controller,
+        plot_settings = {
+            "input_voltage": {
+                "scalar": {
+                    "subplot": 0,
+                    "ylabel": f"Actuator Controller Voltage",
+                    "color": "b"
+                }
+            },
+            "digital_input": {
+                "scalar": {
+                    "subplot": 1,
+                    "ylabel": f"Actuator Controller CTRL Input",
+                    "color": "b"
+                }
+            },
+            "disp": {
+                "scalar": {
+                    "subplot": 2,
+                    "ylabel": f"Actuator Controller Displacement",
+                    "color": "b"
+                }
+            }
+        }
+    )
+
+    # For the displacement sensor, we need 2 shared joints. 1 is the grounded joint of the actuator. the other is the parent
+    shared_south_wall_joint: SharedJoint = base_manager.SharedJoint()
+    south_wall_joint.attach_shared_attributes(shared_south_wall_joint)
+    shared_actuator_joint: SharedJoint = base_manager.SharedJoint()
+    actuator_joint.attach_shared_attributes(shared_actuator_joint)
+    rigid_actuator_displacement_sensor = DisplacementSensor(
+        parent_joint = shared_actuator_joint,
+        child_joint = shared_south_wall_joint,
+        sensor_settings = {
+            "name": f"Actuator Displacement Sensor",
+            "output_channel": 0,
+            "volts_per_meter": 1,
+            "neutral_length": 0,            
+            "neutral_voltage": 0,           # Voltage at neutral length
+            "max_length": 8,
+        }
+    )
+    shared_rigid_actuator_displacement_sensor: SharedDisplacementSensor = base_manager.SharedDisplacementSensor()
+    rigid_actuator_displacement_sensor.attach_shared_attributes(shared_rigid_actuator_displacement_sensor)
+    sensor_plotter.attach_shared_attributes(shared_attributes=shared_rigid_actuator_displacement_sensor,
+        plot_settings = {
+            "length": {
+                    "scalar": {
+                        "subplot": 0,
+                        "ylabel": f"Actuator Displacement Length",
+                        "color": "b"
+                    }
+            },
+            "voltage": {
+                "true": {               # true or noisy because we didn't add noise
+                    "subplot": 1,
+                    "ylabel": f"Actuator Displacement Voltage",
+                    "color": "b"
+                }
+            },
+        }
+    )
+    hitl_controller.attach_displacement_sensor(rigid_actuator_displacement_sensor)
+
+    
+
+    '''Create the physics simulation'''
+    simulation = BFSSimulation(
+        parent_joint = main_mass,           # Because spring is a child of main_mass, it will be reached by BFS
+        settings = {
+            "duration": 1000,               # Run the sim for 1000 seconds
+            "delta_t": None,                # Run in real-time
+            "plotting_update_period": 0.01, # Update the plottable elements every 0.01 seconds
+            "sensor_update_period": 0.01,      # Update the sensor elements every second (because we have none, this doesn't matter)
+            "controller_pull_period": 0.01     # Update the controller elements every seconds (again, we have none)
+        }
+    )
+    simulation.attach_shared_time(shared_sim_time)
+    
+
+
+
+
+    '''Setup the processes and run them'''
+    physics_plotter_process = multiprocessing.Process(target=physics_plotter.run_process)
+    controller_plotter_process = multiprocessing.Process(target=controller_plotter.run_process)
+    sensor_plotter_process = multiprocessing.Process(target=sensor_plotter.run_process)
+    visualization_process = multiprocessing.Process(target=visualizer.run_process)
+    simulation_process = multiprocessing.Process(target=simulation.run_process)
+
+    hitl_controller_process = multiprocessing.Process(target=hitl_controller.run_process)
+    
+    physics_plotter_process.start()
+    controller_plotter_process.start()
+    sensor_plotter_process.start()
+    visualization_process.start()
+
+    hitl_controller_process.start()
+
+    time.sleep(5)                           # Give the plotters and visualizer time to startup before the physics sim runs.
+
+    # Join the process
+    simulation_process.start()
+    simulation_process.join()               # This blocks until the simulation finishes
+
+
+    physics_plotter_process.join()
+    controller_plotter_process.join()
+    sensor_plotter_process.join()
+    visualization_process.join()
+
+    hitl_controller_process.join()
+
+    # Close the manager
+    base_manager.shutdown()
+
+if __name__ == "__main__":
+    main()

code/_example_2d_mass_spring_oscillator_with_actuators.py

@@ -0,0 +1,649 @@
+import numpy as np
+import time
+import multiprocessing
+from multiprocessing.managers import BaseManager
+
+
+from Visualization import Visualization, Plotter
+from Simulation import BFSSimulation
+from Physics_Elements import Joint, Spring, RigidActuator, SharedJoint, SharedSpring, SharedRigidActuator
+
+from HITL_Controller import HITLController
+from Controller_Input_Elements import RigidActuatorController, SharedRigidActuatorController
+from Sensor_Elements import DisplacementSensor, LoadCellSpring, SharedDisplacementSensor, SharedLoadCellSpring
+
+from Object_Sharing import SharedFloat
+
+
+
+
+
+def main():
+    '''Setting up the BaseManager so data can be shared between processes'''
+    base_manager = BaseManager()
+
+    BaseManager.register('SharedFloat', SharedFloat)
+    BaseManager.register('SharedJoint', SharedJoint)
+    BaseManager.register('SharedSpring', SharedSpring)
+
+    BaseManager.register('SharedRigidActuator', SharedRigidActuator)
+
+    BaseManager.register('SharedRigidActuatorController', SharedRigidActuatorController)
+
+    BaseManager.register('SharedDisplacementSensor', SharedDisplacementSensor)
+    BaseManager.register('SharedLoadCellSpring', SharedLoadCellSpring)
+
+    base_manager.start()
+
+
+
+
+
+
+    '''Creating instances for plotter and visualizer'''
+    # Setup the plotter and visualization processes
+    stop_event = multiprocessing.Event()
+    stop_event.clear()
+
+    # Create a synchronized time between the processes so the plotter and physics sim are in sync
+    shared_sim_time:SharedFloat = base_manager.SharedFloat()
+    shared_sim_time.set(0.0) # Set the initial time = 0.0 seconds
+
+
+    # Create a real-time plotter for physics plotting.
+    physics_plotter = Plotter(
+        stop_event = stop_event,
+        plot_settings = {
+            "title": "Physics Plotter",
+            "xlabel": "Time (s)",
+            "num_subplots": 4,
+            "step_or_plot": "plot",
+            "subplots": [
+                {
+                    "ylabel": "Position (m)"
+                },
+                {
+                    "ylabel": "Velocity (m/s)"
+                },
+                {
+                    "ylabel": "Accel (m/s/s)"
+                },
+                {
+                    "ylabel": "Force (N)"
+                },
+            ],
+            "window_length": 10,               # Keep 100 seconds visible
+            "pull_interval": 10,                # Pull data every 10 millisecond
+            "update_interval": 100              # Update the graph every 100 milliseconds
+        }
+    )
+    physics_plotter.attach_shared_x(shared_sim_time)
+
+    # Create a real-time plotter for the controller input plotting.
+    controller_plotter = Plotter(
+        stop_event = stop_event,
+        plot_settings = {
+            "title": "Controller Plotter",
+            "xlabel": "Time (s)",
+            "num_subplots": 3,
+            "step_or_plot": "plot",
+            "subplots": [
+                {
+                    "ylabel": "Controller Input (V)",
+                },
+                {
+                    "ylabel": "Digital Input"
+                },
+                {
+                    "ylabel": "Controlled Pos (m)",
+                },
+            ],
+            "window_length": 10,               # Keep 100 seconds visible
+            "pull_interval": 10,                # Pull data every 10 millisecond
+            "update_interval": 100              # Update the graph every 100 milliseconds
+        }
+    )
+    controller_plotter.attach_shared_x(shared_sim_time)
+
+    # Create a real-time plotter for the sensor output plotting
+    sensor_plotter = Plotter(stop_event=stop_event, plot_settings={
+        "title": "Sensor Plotter",
+        "xlabel": "Time (s)",
+        "num_subplots": 3,
+        "step_or_plot": "plot",
+        "subplots": [
+            {
+                "ylabel": "Actuator Length (m)",
+            },
+            {
+                "ylabel": "Load Cell Force (N)",
+            },
+            {
+                "ylabel": "Sensor Output (V)",
+            },
+        ],
+        "window_length": 10, # Max data points to keep on the plot
+        "pull_interval": 10, # in milliseconds
+        "update_interval": 100, # In milliseconds.
+        })
+    sensor_plotter.attach_shared_x(shared_sim_time)
+
+    # Create a 3D visualization for the entire model
+    visualizer = Visualization(
+        stop_event = stop_event,
+        scene_settings = {
+            "canvas_width": 1600,
+            "canvas_height": 1000,
+            "wall_thickness": 0.1,
+            "cube_size": 20
+        }
+    )
+
+
+    '''Creating an instance for the HITLController'''
+    hitl_controller = HITLController(
+        stop_event = stop_event,
+        settings = {
+            "pull_from_sim_period": 5,
+            "plotting_update_period": 10
+        }
+    )
+
+
+
+    '''Setting up physics simulation'''
+    # Create the physics elements
+    main_mass = Joint(
+        pos = np.array([0, 10, 0]),
+        mass = 5,
+        fixed = False,
+        name = "Main mass",
+        integration_method = "adams-bashforth"
+    )
+    main_mass.add_damping(mom_ratio=0.0)
+
+    # Since we want to plot the physics of the mass we need shared attributes for it
+    shared_main_mass: SharedJoint = base_manager.SharedJoint()
+    main_mass.attach_shared_attributes(shared_main_mass)
+    physics_plotter.attach_shared_attributes(
+        shared_attributes = shared_main_mass,
+        plot_settings = {
+            "pos": {
+                "x": {
+                    "subplot": 0,
+                    "ylabel": "Main Poss x-Pos",
+                    "color": "r"
+                }
+            },
+            "vel": {
+                "x": {
+                    "subplot": 1,
+                    "ylabel": "Main Poss x-Vel",
+                    "color": "g"
+                }
+            },
+            "accel": {
+                "x": {
+                    "subplot": 2,
+                    "ylabel": "Main Poss x-Accel",
+                    "color": "b"
+                }
+            }
+        }
+    )
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_main_mass,
+        object_settings = {
+            "type": "sphere",
+            "color": "red",
+            "radius": 0.5,
+        }
+    )
+    
+    # Before we create a spring, we need to create the Joint on the wall for the Spring to mount to
+    north_wall_joint = Joint(
+        pos = np.array([10, 15, 0]),
+        mass = 1,                           # Does not matter because it is fixed
+        fixed = True,                       # We do not want it to ever move
+        name = "North Wall Joint"
+    )
+
+    north_spring = Spring(
+        parent_joint = main_mass,
+        child_joint = north_wall_joint,
+        unstretched_length = 13,
+        constant_stiffness = 100,
+        name = "North Spring"
+    )
+    shared_north_spring: SharedSpring = base_manager.SharedSpring()
+    north_spring.attach_shared_attributes(shared_north_spring)
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_north_spring,
+        object_settings = {
+            "type": "helix",
+            "color": "white",
+            "radius": 0.5,
+            "thickness": 0.1
+        }
+    )
+
+    west_wall_joint = Joint(
+        pos = np.array([0, 15, -10]),
+        mass = 1,                           # Does not matter because it is fixed
+        fixed = True,                       # We do not want it to ever move
+        name = "West Wall Joint"
+    )
+
+    west_spring = Spring(
+        parent_joint = main_mass,
+        child_joint = west_wall_joint,
+        unstretched_length = 9,
+        constant_stiffness = 100,
+        name = "West Spring"
+    )
+    shared_west_spring: SharedSpring = base_manager.SharedSpring()
+    west_spring.attach_shared_attributes(shared_west_spring)
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_west_spring,
+        object_settings = {
+            "type": "helix",
+            "color": "white",
+            "radius": 0.5,
+            "thickness": 0.1
+        }
+    )
+
+    # Setting up the actuator. This requires the "grounded_joint" to fix to. We also want a spring in between the actuator and main_mass
+    actuator_joint = Joint(
+        pos = np.array([-5, 10, 0]),
+        mass = 1,                           # Does not matter because it is fixed to an actuator
+        fixed = False,                      # We do not want it to ever move
+        name = "Actuator Joint"
+    )
+    
+    actuator_spring = Spring(
+        parent_joint = main_mass,
+        child_joint = actuator_joint,
+        unstretched_length = 7,
+        constant_stiffness = 150,
+        name = "Actuator Spring"
+    )
+    shared_actuator_spring: SharedSpring = base_manager.SharedSpring()
+    actuator_spring.attach_shared_attributes(shared_actuator_spring)
+    physics_plotter.attach_shared_attributes(
+        shared_attributes = shared_actuator_spring,
+        plot_settings = {
+            "force": {
+                "scalar": {
+                    "subplot": 3,
+                    "ylabel": "Actuator Spring Force",
+                    "color": "b"
+                }
+            },
+        }
+    )
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_actuator_spring,
+        object_settings = {
+            "type": "helix",
+            "color": "white",
+            "radius": 0.5,
+            "thickness": 0.1
+        }
+    )
+
+    south_wall_joint = Joint(
+        pos = np.array([-10, 10, 0]),
+        mass = 1,                           # Does not matter because it is fixed
+        fixed = True,                       # We do not want it to ever move
+        name = "South Wall Joint"
+    )
+
+    rigid_actuator = RigidActuator(
+        parent_joint = actuator_joint,
+        grounded_joint = south_wall_joint,
+        name = "Rigid Actuator",
+        max_length = 8,
+        min_length = 2,
+        control_code = 0                    # Position Control
+    )
+    shared_rigid_actuator: SharedRigidActuator = base_manager.SharedRigidActuator()
+    rigid_actuator.attach_shared_attributes(shared_rigid_actuator)
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_rigid_actuator,
+        object_settings = {
+            "type": "cylinder",
+            "color": "black",
+            "radius": 0.5,
+        }
+    )
+
+    # We have an actuator, but it currently does nothing without the RigidActuatorController
+        # We will add the Controller and a DisplacementSensor for something in the real-world to control with
+    rigid_actuator_controller = RigidActuatorController(
+        controller_settings = {
+            "name": "Rigid Actuator Controller",
+            "analog_input_channel": 0,
+            "digital_input_channel": 0,
+            "units_per_volt": 2,                    # 2 meters per volt
+            "neutral_length": 5,
+            "neutral_voltage": 2.5,                 # At 2.5V, go to neutral_length of 5
+            "controls_pos/vel/accel": 0,            # Controls position
+            "max_length": 8,
+            "min_length": 2
+        }
+    )
+    rigid_actuator_controller.attach_sim_target(shared_rigid_actuator)
+    hitl_controller.attach_rigid_actuator_controller(rigid_actuator_controller)
+
+    shared_rigid_actuator_controller: SharedRigidActuatorController = base_manager.SharedRigidActuatorController()
+    rigid_actuator_controller.attach_shared_attributes(shared_rigid_actuator_controller)
+    controller_plotter.attach_shared_attributes(
+        shared_attributes = shared_rigid_actuator_controller,
+        plot_settings = {
+            "input_voltage": {
+                "scalar": {
+                    "subplot": 0,
+                    "ylabel": f"Actuator Controller Voltage",
+                    "color": "r"
+                }
+            },
+            "digital_input": {
+                "scalar": {
+                    "subplot": 1,
+                    "ylabel": f"Actuator Controller CTRL Input",
+                    "color": "r"
+                }
+            },
+            "disp": {
+                "scalar": {
+                    "subplot": 2,
+                    "ylabel": f"Actuator Controller Displacement",
+                    "color": "r"
+                }
+            }
+        }
+    )
+
+    # For the displacement sensor, we need 2 shared joints. 1 is the grounded joint of the actuator. the other is the parent
+    shared_south_wall_joint: SharedJoint = base_manager.SharedJoint()
+    south_wall_joint.attach_shared_attributes(shared_south_wall_joint)
+    shared_actuator_joint: SharedJoint = base_manager.SharedJoint()
+    actuator_joint.attach_shared_attributes(shared_actuator_joint)
+    rigid_actuator_displacement_sensor = DisplacementSensor(
+        parent_joint = shared_actuator_joint,
+        child_joint = shared_south_wall_joint,
+        sensor_settings = {
+            "name": f"Actuator Displacement Sensor",
+            "output_channel": 0,
+            "volts_per_meter": 1,
+            "neutral_length": 0,            
+            "neutral_voltage": 0,           # Voltage at neutral length
+            "max_length": 8,
+        }
+    )
+    shared_rigid_actuator_displacement_sensor: SharedDisplacementSensor = base_manager.SharedDisplacementSensor()
+    rigid_actuator_displacement_sensor.attach_shared_attributes(shared_rigid_actuator_displacement_sensor)
+    sensor_plotter.attach_shared_attributes(shared_attributes=shared_rigid_actuator_displacement_sensor,
+        plot_settings = {
+            "length": {
+                    "scalar": {
+                        "subplot": 0,
+                        "ylabel": f"Actuator Displacement Length",
+                        "color": "r"
+                    }
+            },
+            "voltage": {
+                "true": {               # true or noisy because we didn't add noise
+                    "subplot": 2,
+                    "ylabel": f"Actuator Displacement Voltage",
+                    "color": "r"
+                }
+            },
+        }
+    )
+    hitl_controller.attach_displacement_sensor(rigid_actuator_displacement_sensor)
+
+
+    '''Second actuator'''
+
+    bottom_actuator_joint = Joint(
+        pos = np.array([0, 5, 0]),
+        mass = 1,                           # Does not matter because it is fixed to an actuator
+        fixed = False,                      # We do not want it to ever move
+        name = "Bottom Actuator Joint"
+    )
+    
+    bottom_actuator_spring = Spring(
+        parent_joint = main_mass,
+        child_joint = bottom_actuator_joint,
+        unstretched_length = 7.5,
+        constant_stiffness = 1000,
+        name = "Bottom Actuator Spring"
+    )
+    shared_bottom_actuator_spring: SharedSpring = base_manager.SharedSpring()
+    bottom_actuator_spring.attach_shared_attributes(shared_bottom_actuator_spring)
+    physics_plotter.attach_shared_attributes(
+        shared_attributes = shared_bottom_actuator_spring,
+        plot_settings = {
+            "force": {
+                "scalar": {
+                    "subplot": 3,
+                    "ylabel": "Bottom Actuator Spring Force",
+                    "color": "r"
+                }
+            },
+        }
+    )
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_bottom_actuator_spring,
+        object_settings = {
+            "type": "helix",
+            "color": "white",
+            "radius": 0.5,
+            "thickness": 0.1
+        }
+    )
+
+
+    # LoadCell on the bottom spring
+    bottom_spring_loadcell = LoadCellSpring(sensor_settings = {
+        "name": "Bottom Spring LC",
+        "mV/V": 1000,
+        "excitation": 10,
+        "full_scale_force": 5000,      # What is the max force on the load cell
+        "output_channel": 8          # The output channel that the load cell outputs through the MCC3114. Must be between [0, 15]
+        }
+    )
+    bottom_spring_loadcell.attach_sim_source(shared_bottom_actuator_spring)                   # Point the LoadCellSpring to pull physics values from the SharedSpring
+    shared_bottom_spring_loadcell: SharedLoadCellSpring = base_manager.SharedLoadCellSpring()
+    bottom_spring_loadcell.attach_shared_attributes(shared_bottom_spring_loadcell)
+    hitl_controller.attach_load_cell(bottom_spring_loadcell)
+
+    sensor_plotter.attach_shared_attributes(shared_attributes = shared_bottom_spring_loadcell,
+        plot_settings = {
+            "force": {
+            "scalar": {
+                "subplot": 1,     # Must be in range [0, num_subplots-1]
+                "ylabel": "Bottom Spring LC-Force",
+                "color": "g"        # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w"
+            },
+        },
+        "noisy_voltage": {
+            "scalar": {
+                "subplot": 2,     # Must be in range [0, num_subplots-1]
+                "ylabel": "Bottom Spring LC-Voltage",
+                "color": "g"        # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w"
+            }
+        }
+        }    
+    )
+    
+
+
+    bottom_wall_joint = Joint(
+        pos = np.array([0, 0, 0]),
+        mass = 1,                           # Does not matter because it is fixed
+        fixed = True,                       # We do not want it to ever move
+        name = "Bottom Wall Joint"
+    )
+
+    bottom_rigid_actuator = RigidActuator(
+        parent_joint = bottom_actuator_joint,
+        grounded_joint = bottom_wall_joint,
+        name = "Bottom Rigid Actuator",
+        max_length = 10,
+        min_length = 1,
+        control_code = 1                    # Position Control
+    )
+    shared_bottom_rigid_actuator: SharedRigidActuator = base_manager.SharedRigidActuator()
+    bottom_rigid_actuator.attach_shared_attributes(shared_bottom_rigid_actuator)
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_bottom_rigid_actuator,
+        object_settings = {
+            "type": "cylinder",
+            "color": "black",
+            "radius": 0.5,
+        }
+    )
+
+    # We have an actuator, but it currently does nothing without the RigidActuatorController
+        # We will add the Controller and a DisplacementSensor for something in the real-world to control with
+    bottom_rigid_actuator_controller = RigidActuatorController(
+        controller_settings = {
+            "name": "Bottom Rigid Actuator Controller",
+            "analog_input_channel": 1,
+            "digital_input_channel": 2,
+            "units_per_volt": 2,                    # 2 meters per volt
+            "neutral_length": 1.5,
+            "neutral_voltage": 2.5,                 # At 2.5V, go to neutral_length of 5
+            "controls_pos/vel/accel": 1,            # Controls position
+            "max_length": 10,
+            "min_length": 1
+        }
+    )
+    bottom_rigid_actuator_controller.attach_sim_target(shared_bottom_rigid_actuator)
+    hitl_controller.attach_rigid_actuator_controller(bottom_rigid_actuator_controller)
+
+    shared_bottom_rigid_actuator_controller: SharedRigidActuatorController = base_manager.SharedRigidActuatorController()
+    bottom_rigid_actuator_controller.attach_shared_attributes(shared_bottom_rigid_actuator_controller)
+    controller_plotter.attach_shared_attributes(
+        shared_attributes = shared_bottom_rigid_actuator_controller,
+        plot_settings = {
+            "input_voltage": {
+                "scalar": {
+                    "subplot": 0,
+                    "ylabel": f"Bottom Actuator Controller Voltage",
+                    "color": "b"
+                }
+            },
+            "digital_input": {
+                "scalar": {
+                    "subplot": 1,
+                    "ylabel": f"Bottom Actuator Controller CTRL Input",
+                    "color": "b"
+                }
+            },
+            "disp": {
+                "scalar": {
+                    "subplot": 2,
+                    "ylabel": f"Bottom Actuator Controller Displacement",
+                    "color": "b"
+                }
+            }
+        }
+    )
+
+    # For the displacement sensor, we need 2 shared joints. 1 is the grounded joint of the actuator. the other is the parent
+    shared_bottom_wall_joint: SharedJoint = base_manager.SharedJoint()
+    bottom_wall_joint.attach_shared_attributes(shared_bottom_wall_joint)
+    shared_bottom_actuator_joint: SharedJoint = base_manager.SharedJoint()
+    bottom_actuator_joint.attach_shared_attributes(shared_bottom_actuator_joint)
+    bottom_rigid_actuator_displacement_sensor = DisplacementSensor(
+        parent_joint = shared_bottom_actuator_joint,
+        child_joint = shared_bottom_wall_joint,
+        sensor_settings = {
+            "name": f"Bottom Actuator Displacement Sensor",
+            "output_channel": 1,
+            "volts_per_meter": 1,
+            "neutral_length": 0,            
+            "neutral_voltage": 0,           # Voltage at neutral length
+            "max_length": 8,
+        }
+    )
+    shared_bottom_rigid_actuator_displacement_sensor: SharedDisplacementSensor = base_manager.SharedDisplacementSensor()
+    bottom_rigid_actuator_displacement_sensor.attach_shared_attributes(shared_bottom_rigid_actuator_displacement_sensor)
+    sensor_plotter.attach_shared_attributes(shared_attributes=shared_bottom_rigid_actuator_displacement_sensor,
+        plot_settings = {
+            "length": {
+                    "scalar": {
+                        "subplot": 0,
+                        "ylabel": f"Bottom Actuator Displacement Length",
+                        "color": "b"
+                    }
+            },
+            "voltage": {
+                "true": {               # true or noisy because we didn't add noise
+                    "subplot": 2,
+                    "ylabel": f"Bottom Actuator Displacement Voltage",
+                    "color": "b"
+                }
+            },
+        }
+    )
+    hitl_controller.attach_displacement_sensor(bottom_rigid_actuator_displacement_sensor)
+
+    
+
+    '''Create the physics simulation'''
+    simulation = BFSSimulation(
+        parent_joint = main_mass,           # Because spring is a child of main_mass, it will be reached by BFS
+        settings = {
+            "duration": 1000,               # Run the sim for 1000 seconds
+            "delta_t": None,                # Run in real-time
+            "plotting_update_period": 0.01, # Update the plottable elements every 0.01 seconds
+            "sensor_update_period": 0.01,      # Update the sensor elements every second (because we have none, this doesn't matter)
+            "controller_pull_period": 0.01     # Update the controller elements every seconds (again, we have none)
+        }
+    )
+    simulation.attach_shared_time(shared_sim_time)
+    
+
+
+
+
+    '''Setup the processes and run them'''
+    physics_plotter_process = multiprocessing.Process(target=physics_plotter.run_process)
+    controller_plotter_process = multiprocessing.Process(target=controller_plotter.run_process)
+    sensor_plotter_process = multiprocessing.Process(target=sensor_plotter.run_process)
+    visualization_process = multiprocessing.Process(target=visualizer.run_process)
+    simulation_process = multiprocessing.Process(target=simulation.run_process)
+
+    hitl_controller_process = multiprocessing.Process(target=hitl_controller.run_process)
+    
+    physics_plotter_process.start()
+    controller_plotter_process.start()
+    sensor_plotter_process.start()
+    visualization_process.start()
+
+    hitl_controller_process.start()
+
+    time.sleep(5)                           # Give the plotters and visualizer time to startup before the physics sim runs.
+
+    # Join the process
+    simulation_process.start()
+    simulation_process.join()               # This blocks until the simulation finishes
+
+
+    physics_plotter_process.join()
+    controller_plotter_process.join()
+    sensor_plotter_process.join()
+    visualization_process.join()
+
+    hitl_controller_process.join()
+
+    # Close the manager
+    base_manager.shutdown()
+
+if __name__ == "__main__":
+    main()

code/_example_stage_1_test.py

@@ -0,0 +1,514 @@
+import numpy as np
+import multiprocessing
+from multiprocessing.managers import BaseManager
+
+import time
+
+# if TYPE_CHECKING:
+from Visualization import Visualization, Plotter
+from Simulation import BFSSimulation
+from Physics_Elements import Joint, Spring, RigidActuator, SharedRigidActuator, SharedJoint, SharedSpring, SharedRigidActuator
+from Object_Sharing import SharedFloat
+from HITL_Controller import HITLController
+from Sensor_Elements import SharedLoadCellJoint, LoadCellSpring, SharedLoadCellSpring, DisplacementSensor, SharedDisplacementSensor
+from Controller_Input_Elements import RigidActuatorController, SharedRigidActuatorController
+
+
+
+
+steel_elastic_modulus = 200E9 # 200GPa
+gravity = 9.81 # m/s^2
+
+# Stage setup
+stage_mass = 500_000 # Mass in kilograms
+stage_weight = stage_mass * gravity
+max_thrust = 1.5E7 # Max thrust in Newtons. Currently 15_000_000 N
+stage_diameter = 5.5 # meters
+stage_height = 60 # Meters
+wall_thickness = 0.03 # Meters
+    
+# Actuator setup
+rope_area = 5E-4 # Area in m^2. Currently 5 cm^2
+rope_force_at_neutral_actuator = (max_thrust - stage_weight) / 4.0
+
+lateral_offset = 2 # Meters away from the stage wall
+    
+actuator_neutral_length = 2
+actuator_min_length = 0.5
+actuator_max_length = 2.5
+
+actuator_controller_neutral_voltage = 0
+actuator_controller_mps_per_volt = 1 # meters per second per volt for the RMS controller to command the sim
+
+actuator_displacement_sensor_neutral_voltage = 0
+actuator_displacement_sensor_neutral_length = 0
+
+actuator_displacement_sensor_volts_per_meter = 3 # Volts per meter for the actuator displacement sensor to output
+    
+
+rope_volts_per_newton = 1E-6 # 1 volt is 1 million newtons
+
+# Stage calculations for stiffness of rope supporting main mass
+stage_wall_area = np.pi / 4.0 * (stage_diameter**2 - (stage_diameter - wall_thickness*2)**2)
+stage_stiffness = steel_elastic_modulus * stage_wall_area / stage_height # k = EA/l
+stage_unstretched_length = stage_height - stage_weight / stage_stiffness
+
+# Actuator calculations
+rope_stiffness = steel_elastic_modulus * rope_area / stage_height # Estimate due to stageheight != rope length but it is close enough
+rope_unstretched_length = rope_force_at_neutral_actuator / rope_stiffness + 56.45
+actuator_lateral_offset = stage_diameter / 2 + lateral_offset
+
+actuator_angle = np.arctan(stage_height/actuator_lateral_offset)
+actuator_parent_height = actuator_neutral_length * np.sin(actuator_angle)
+actuator_parent_lateral_offset = actuator_neutral_length * np.cos(actuator_angle)
+
+def sinusoid_force1(time):
+    return 100_000*np.sin(0.1*time)
+def sinusoid_force2(time):
+    return 125_000*np.sin(0.2*time+0.3)
+
+def rope_stiffness_func(length, unstretched_length):
+    if length > unstretched_length:
+        return rope_stiffness
+    else:
+        return 0
+
+def stage_setup_1(_max_run_time=100):
+    max_run_time = _max_run_time
+
+
+    # SIM SETUP #############################################
+    manager = BaseManager()
+    BaseManager.register('SharedFloat', SharedFloat)
+    BaseManager.register('SharedJoint', SharedJoint)
+    BaseManager.register('SharedSpring', SharedSpring)
+    BaseManager.register('SharedRigidActuator', SharedRigidActuator)
+
+    BaseManager.register('SharedLoadCellJoint', SharedLoadCellJoint)
+    BaseManager.register('SharedLoadCellSpring', SharedLoadCellSpring)
+
+    BaseManager.register('SharedRigidActuatorController', SharedRigidActuatorController)
+
+    BaseManager.register('SharedDisplacementSensor', SharedDisplacementSensor)
+
+    
+    manager.start()
+
+    stop_event = multiprocessing.Event()
+    stop_event.clear()
+
+    shared_sim_time = manager.SharedFloat()
+    shared_sim_time.set(0.0)
+
+    
+    physics_plotter = Plotter(stop_event=stop_event, plot_settings={
+        "title": "Physics Plotter",
+        "xlabel": "Time (s)",
+        "num_subplots": 5,
+        "step_or_plot": "plot",
+        "subplots": [
+            {
+                "ylabel": "Accel (m/s/s)",
+            },
+            {
+                "ylabel": "Vel (m/s)",
+            },
+            {
+                "ylabel": "Pos (m)",
+            },
+            {
+                "ylabel": "Pos 2 (m)",
+            },
+            {
+                "ylabel": "Force (N)",
+            },
+        ],
+        "window_length": 100,
+        "pull_interval": 10, # in milliseconds
+        "update_interval": 100, # In milliseconds
+        })
+    physics_plotter.attach_shared_x(shared_sim_time)
+
+    sensor_plotter = Plotter(stop_event=stop_event, plot_settings={
+        "title": "Sensor Plotter",
+        "xlabel": "Time (s)",
+        "num_subplots": 4,
+        "step_or_plot": "plot",
+        "subplots": [
+            {
+                "ylabel": "Spring Force (N)",
+            },
+            {
+                "ylabel": "Actuator Length (m)",
+            },
+            {
+                "ylabel": "Actuator Position Output (V)",
+            },
+            {
+                "ylabel": "Load Cell Output (V)",
+            },
+        ],
+        "window_length": 100, # Max data points to keep on the plot
+        "pull_interval": 10, # in milliseconds
+        "update_interval": 100, # In milliseconds.
+        })
+    sensor_plotter.attach_shared_x(shared_sim_time)
+
+    controller_plotter = Plotter(stop_event=stop_event, plot_settings={
+        "title": "Controller Plotter",
+        "xlabel": "Time (s)",
+        "num_subplots": 3,
+        "step_or_plot": "plot",
+        "subplots": [
+            {
+                "ylabel": "Controller Input (V)",
+            },
+            {
+                "ylabel": "Digital Input"
+            },
+            {
+                "ylabel": "Controlled Vel (m/s)",
+            },
+        ],
+        "window_length": 100, # Max data points to keep on the plot
+        "pull_interval": 10, # in milliseconds
+        "update_interval": 100, # In milliseconds.
+        })
+    controller_plotter.attach_shared_x(shared_sim_time)
+
+    visualizer = Visualization(stop_event=stop_event, scene_settings={
+        "canvas_width": 1600,
+        "canvas_height": 1000,
+        "wall_thickness": 0.1,
+        "cube_size": 100
+    })
+
+    hitl_controller = HITLController(stop_event=stop_event, settings={
+        "pull_from_sim_period": 5, # Pull values for the sensors every XXX milliseconds. Shouldn't be faster than the update_period for the sim
+        "plotting_update_period": 10, # Update the shared attributes (for plotting) every YYY milliseconds
+    })
+
+
+    # SIM ELEMENTS SETUP
+    # Changes height to 59.78258707863 rather than 60m to that it is in equilibrium
+    main_mass = Joint(np.array([0, 60, 0]), mass=stage_mass, fixed=False, name="Stage Mass", integration_method="adams-bashforth")
+    main_mass_shared:SharedJoint = manager.SharedJoint()
+    main_mass.attach_shared_attributes(main_mass_shared)
+    main_mass.add_damping(mom_ratio=1.5)
+    main_mass.add_gravity()
+    main_mass.add_variable_force([sinusoid_force1, None, sinusoid_force2])
+    physics_plotter.attach_shared_attributes(main_mass_shared, plot_settings={
+        "accel": {
+            "x": {
+                "subplot": 0,
+                "ylabel": "Main Mass x-Accel",
+                "color": "r"
+            },
+            "y": {
+                "subplot": 0,
+                "ylabel": "Main Mass y-Accel",
+                "color": "g"
+            },
+            "z": {
+                "subplot": 0,
+                "ylabel": "Main Mass z-Accel",
+                "color": "b"
+            },
+        },
+        "vel": {
+            "x": {
+                "subplot": 1,
+                "ylabel": "Main Mass x-Vel",
+                "color": "r"
+            },
+            "y": {
+                "subplot": 1,
+                "ylabel": "Main Mass y-Vel",
+                "color": "g"
+            },
+            "z": {
+                "subplot": 1,
+                "ylabel": "Main Mass z-Vel",
+                "color": "b"
+            }
+        },
+        "pos": {
+            "x": {
+                "subplot": 2,
+                "ylabel": "Main Mass x-Pos",
+                "color": "r"
+            },
+            "y": {
+                "subplot": 3,
+                "ylabel": "Main Mass y-Pos",
+                "color": "g"
+            },
+            "z": {
+                "subplot": 2,
+                "ylabel": "Main Mass z-Pos",
+                "color": "b"
+            }
+        }
+    })
+    visualizer.attach_shared_attributes(main_mass_shared, object_settings={
+        "type": "sphere",
+        "color": "red",
+        "radius": stage_diameter/2.0
+    })
+
+    support_spring_joint = Joint(np.array([0, stage_height*2, 0]), mass=0, fixed=True, name="Support Spring Joint")
+
+    support_spring = Spring(parent_joint=main_mass, child_joint=support_spring_joint, unstretched_length=stage_unstretched_length, constant_stiffness=stage_stiffness, name="Support Spring")
+    support_spring_shared_attributes:SharedSpring = manager.SharedSpring()
+    support_spring.attach_shared_attributes(support_spring_shared_attributes)
+    physics_plotter.attach_shared_attributes(support_spring_shared_attributes, plot_settings={
+        "force": {
+            "y": {
+                "subplot": 4,
+                "ylabel": "Support Spring y-Force",
+                "color": "w"
+            }
+        }
+    })
+
+
+    # Setting up the actuators and ropes
+    for i in range(4):
+        if i == 0:
+            floor_x_pos = actuator_lateral_offset
+            floor_z_pos = 0
+            parent_x_pos = actuator_lateral_offset - actuator_parent_lateral_offset
+            parent_z_pos = 0
+            name = "East Actuator"
+            name2 = "East Spring"
+            color = "r"
+        elif i == 1:
+            floor_x_pos = 0
+            floor_z_pos = actuator_lateral_offset
+            parent_x_pos = 0
+            parent_z_pos = actuator_lateral_offset - actuator_parent_lateral_offset
+            name = "South Actuator"
+            name2 = "South Spring"
+            color = "g"
+        elif i == 2:
+            floor_x_pos = -1*actuator_lateral_offset
+            floor_z_pos = 0
+            parent_x_pos = -1*actuator_lateral_offset + actuator_parent_lateral_offset
+            parent_z_pos = 0
+            name = "West Actuator"
+            name2 = "West Spring"
+            color = "b"
+        elif i == 3:
+            floor_x_pos = 0
+            floor_z_pos = -1*actuator_lateral_offset
+            parent_x_pos = 0
+            parent_z_pos = -1*actuator_lateral_offset + actuator_parent_lateral_offset
+            name = "North Actuator"
+            name2 = "North Spring"
+            color = "w"
+
+        actuator_spring_joint = Joint(np.array([parent_x_pos, actuator_parent_height, parent_z_pos]), mass=0.001, fixed=False, name=f"{name} Spring Joint")
+        actuator_floor_joint = Joint(np.array([floor_x_pos, 0, floor_z_pos]), mass=0.001, fixed=False, name=f"{name} Floor Joint")
+
+        #rope = Spring(parent_joint=main_mass, child_joint=actuator_spring_joint, unstretched_length=rope_unstretched_length, constant_stiffness=rope_stiffness, name=name2)
+        rope = Spring(parent_joint=main_mass, child_joint=actuator_spring_joint, unstretched_length=rope_unstretched_length, stiffness_func=rope_stiffness_func, name=name2)
+        rope_shared:SharedSpring = manager.SharedSpring()
+        rope.attach_shared_attributes(rope_shared)
+        visualizer.attach_shared_attributes(rope_shared, object_settings={
+            "type": "helix",
+            "color": "white",
+            "radius": 0.1,
+            "thickness": 0.1
+        })
+
+        rope_lc = LoadCellSpring(sensor_settings={
+            "name": f"{name2} LC",
+            "mV/V": 1000,
+            "excitation": 10,
+            "full_scale_force": (1/rope_volts_per_newton * 10),
+            "output_channel": i*4 + 3,
+            "noise_settings": {
+                "static_error_band": 0.15,      # % of FSO
+                "non-linearity": 0.15,          # % of FSO
+                "hysteresis": 0.07,             # % of FSO
+                "repeatability": 0.02,          # % of FSO
+                "thermal_error": 0.005,         # % of reading per degree F
+                "temperature_offset": 10        # Degrees F off from calibration temperature 
+            }
+        })
+        rope_lc.attach_sim_source(rope_shared)
+        rope_lc_shared_attributes = manager.SharedLoadCellSpring()
+        rope_lc.attach_shared_attributes(rope_lc_shared_attributes)
+        hitl_controller.attach_load_cell(rope_lc)
+        sensor_plotter.attach_shared_attributes(rope_lc_shared_attributes, plot_settings={
+            "force": {
+                "scalar": {
+                    "subplot": 0,
+                    "ylabel": f"{name2} LC Force",
+                    "color": color
+                }
+            },
+            "noisy_voltage": {
+                "scalar": {
+                    "subplot": 3,
+                    "ylabel": f"{name2} LC Noisy-Voltage",
+                    "color": color
+                }
+            },
+        })  
+
+        actuator = RigidActuator(actuator_spring_joint, actuator_floor_joint, name=name, max_length=actuator_max_length, min_length=actuator_min_length, control_code=1)
+        actuator_shared_attributes:SharedRigidActuator = manager.SharedRigidActuator()
+        actuator.attach_shared_attributes(actuator_shared_attributes)
+        visualizer.attach_shared_attributes(actuator_shared_attributes, object_settings={
+            "type": "cylinder",
+            "color": "black",
+            "radius": 0.3
+        })
+
+        shared_actuator_spring_joint: SharedJoint = manager.SharedJoint()
+        actuator_spring_joint.attach_shared_attributes(shared_actuator_spring_joint)
+
+        shared_actuator_floor_joint: SharedJoint = manager.SharedJoint()
+        actuator_floor_joint.attach_shared_attributes(shared_actuator_floor_joint)
+
+        actuator_disp_sensor = DisplacementSensor(parent_joint=shared_actuator_spring_joint, child_joint=shared_actuator_floor_joint, sensor_settings={
+            "name": f"{name} Sensor",
+            "output_channel": i*4 + 2,
+            "volts_per_meter": actuator_displacement_sensor_volts_per_meter,
+            "neutral_length": actuator_displacement_sensor_neutral_length, # Length at neutral voltage
+            "neutral_voltage": actuator_displacement_sensor_neutral_voltage,
+            "max_length": actuator_max_length,
+            "noise_settings": {
+                "static_error_band": 0.15,      # % of FSO
+                "non-linearity": 0.15,          # % of FSO
+                "hysteresis": 0.07,             # % of FSO
+                "repeatability": 0.02,          # % of FSO
+                "thermal_error": 0.005,         # % of reading per degree F
+                "temperature_offset": 10        # Degrees F off from calibration temperature 
+            }
+        })
+        actuator_disp_sensor_shared:SharedDisplacementSensor = manager.SharedDisplacementSensor()
+        actuator_disp_sensor.attach_shared_attributes(actuator_disp_sensor_shared)
+        sensor_plotter.attach_shared_attributes(actuator_disp_sensor_shared, plot_settings={
+            "length": {
+                    "scalar": {
+                        "subplot": 1,
+                        "ylabel": f"{name} Length",
+                        "color": color
+                    }
+            },
+            "voltage": {
+                "noisy": {
+                    "subplot": 2,
+                    "ylabel": f"{name} Noisy-Voltage",
+                    "color": color
+                }
+            },
+        })
+        hitl_controller.attach_displacement_sensor(actuator_disp_sensor)
+
+        actuator_controller = RigidActuatorController(controller_settings={
+            "name": "Main Actuator Controller",
+            "analog_input_channel": i,
+            "digital_input_channel": i,
+            "units_per_volt": actuator_controller_mps_per_volt, # How far the actuator moves per input volt (m/s or m/s/s if controlling vel or accel)
+            "neutral_length": actuator_neutral_length, # Length at neutral voltage
+            "neutral_voltage": actuator_controller_neutral_voltage, # Voltage to go to neutral length
+            "controls_pos/vel/accel": 1, # 0 = controls pos, 1 = controls vel, 2 = controls accel,
+            "max_length": actuator_max_length,
+            "min_length": actuator_min_length
+        })
+        actuator_controller_shared_attributes = manager.SharedRigidActuatorController() # For plotting
+        actuator_controller.attach_shared_attributes(actuator_controller_shared_attributes)
+
+        actuator_controller.attach_sim_target(actuator_shared_attributes) # So the controller can update sim elements
+
+        hitl_controller.attach_rigid_actuator_controller(actuator_controller)
+        controller_plotter.attach_shared_attributes(actuator_controller_shared_attributes, plot_settings={
+            "input_voltage": {
+                "scalar": {
+                    "subplot": 0,
+                    "ylabel": f"{name} Controller Voltage",
+                    "color": color
+                }
+            },
+            "digital_input": {
+                "scalar": {
+                    "subplot": 1,
+                    "ylabel": f"{name} Controller CTRL Input",
+                    "color": color
+                }
+            },
+            "vel": {
+                "x": {
+                    "subplot": 2,
+                    "ylabel": f"{name} Controlled x-Vel",
+                    "color": color
+                }
+            }
+        })
+
+
+    # PROCESSES #############################################################
+    # Make the simulation process
+    simulation = BFSSimulation(parent_joint=main_mass, settings={
+        "duration": max_run_time,
+        "delta_t": None,
+        "shared_update_period": 0.1, # not used
+        "plotting_update_period": 0.01,
+        "sensor_update_period": 0.01,
+        "controller_pull_period": 0.01
+    })
+    simulation.attach_shared_time(shared_sim_time)
+    simulation_process = multiprocessing.Process(target=simulation.run_process)
+
+    # Start the processes
+    # Start the visualization process
+    visualization_process = multiprocessing.Process(target=visualizer.run_process)
+    visualization_process.start()
+    
+    # Start the physics_plotter process
+    physics_plotter_process = multiprocessing.Process(target=physics_plotter.run_process)
+    physics_plotter_process.start()
+
+    # Start the sensor physics_plotter process
+    sensor_plotter_process = multiprocessing.Process(target=sensor_plotter.run_process)
+    sensor_plotter_process.start()
+
+    # Start the sensor physics_plotter process
+    controller_plotter_process = multiprocessing.Process(target=controller_plotter.run_process)
+    controller_plotter_process.start()
+
+    # Start the HITL interface process
+    hitl_process = multiprocessing.Process(target=hitl_controller.run_process)
+    hitl_process.start()
+
+    time.sleep(5)
+    simulation_process.start()
+
+    # Join the processes
+    simulation_process.join()
+    stop_event.set()
+
+    visualization_process.join()
+    physics_plotter_process.join()
+    sensor_plotter_process.join()
+    controller_plotter_process.join()
+    hitl_process.join()
+
+    # Close the manager
+    manager.shutdown()
+
+
+def main():
+    max_run_time = 10000
+    #multiprocessed_double_mass_spring(100)
+    #single_spring(max_run_time)
+
+    stage_setup_1(max_run_time)
+
+
+if __name__ == "__main__":
+    main()
+

code/deprecated/1d_Mass_Spring.py

@@ -0,0 +1,112 @@
+import matplotlib.pyplot as plt
+from copy import deepcopy
+
+class StateSpace():
+    def __init__(self, pos, vel, accel):
+        self.pos = pos
+        self.vel = vel
+        self.accel = accel
+
+    def print(self):
+        print(f"Pos: {self.pos}, Vel: {self.vel}, Accel: {self.accel}")
+
+class Block():
+    def __init__(self):    
+        self.mass = 5
+
+        self.statespace = StateSpace(5,0,0)
+        
+        self.net_force = 0
+
+
+class Spring():
+    def __init__(self):
+        self.mass = 1
+
+        self.pos1 = 0
+        self.pos2 = 10
+        self.vel = 0
+        self.accel = 0
+
+        self.length = self.pos2 - self.pos1
+        self.zero_length = 10
+
+        self.k = 10
+        self.del_l = self.length - self.zero_length
+        self.force = self.del_l * self.k
+    
+    def get_force(self):
+        self.length = self.pos2 - self.pos1
+        self.del_l = self.length - self.zero_length
+        self.force = self.del_l * self.k
+        return self.force
+
+applied_force = 0
+
+block = Block()
+spring = Spring()
+
+
+# Initialize lists to store data for plotting
+t_values = []
+pos_values = []
+vel_values = []
+accel_values = []
+force_net_values = []
+spring_force_values = []
+del_l_values = []
+
+
+del_t = 0.1
+t = 0
+while t < 10:
+    spring.pos1 = block.statespace.pos
+    spring.force = spring.get_force()
+
+    block.net_force = applied_force + spring.force
+    block.statespace.accel =  block.net_force / block.mass
+    
+    block.statespace.vel += block.statespace.accel * del_t
+    block.statespace.pos += block.statespace.vel  * del_t
+    
+    # Store data for plotting
+    t_values.append(t)
+    pos_values.append(block.statespace.pos)
+    vel_values.append(block.statespace.vel)
+    accel_values.append(block.statespace.accel)
+    force_net_values.append(block.net_force)
+    spring_force_values.append(spring.force)
+    del_l_values.append(spring.del_l)
+
+    t += del_t
+
+print(max(pos_values))
+
+# Plot the data
+plt.figure(figsize=(12, 8))
+
+plt.subplot(3, 1, 1)
+plt.plot(t_values, pos_values)
+plt.plot(t_values, del_l_values)
+plt.title('Position vs Time')
+plt.legend(['Position', 'Delta Length'])
+plt.xlabel('Time (s)')
+plt.ylabel('Position')
+
+plt.subplot(3, 1, 2)
+plt.plot(t_values, vel_values)
+plt.title('Velocity vs Time')
+plt.xlabel('Time (s)')
+plt.ylabel('Velocity')
+
+plt.subplot(3, 1, 3)
+plt.plot(t_values, accel_values)
+plt.plot(t_values, force_net_values)
+plt.plot(t_values, spring_force_values)
+plt.legend(['Acceleration', 'Net Force', 'Spring Force'])
+plt.title('Acceleration, Net Force, and Spring Force vs Time')
+plt.xlabel('Time (s)')
+plt.ylabel('Value')
+
+plt.tight_layout()
+plt.show()

code/deprecated/Generalized_1d Mass_Spring_Damper_old.py

@@ -0,0 +1,436 @@
+import math
+import numpy as np
+from typing import List
+import matplotlib.pyplot as plt
+
+ZERO_VECTOR = np.array([0.0, 0.0, 0.0])
+
+
+
+class StateSpace():
+    def __init__(self, pos=np.copy(ZERO_VECTOR), vel=np.copy(ZERO_VECTOR), accel=np.copy(ZERO_VECTOR), force=np.copy(ZERO_VECTOR)):
+        self.pos = pos
+        self.vel = vel
+        self.accel = accel
+
+        self.force = force
+
+    def get_pos(self):
+        return self.pos
+    def get_vel(self):
+        return self.vel
+    def get_accel(self):
+        return self.accel
+    def get_force(self):
+        return self.force
+    
+    def set_pos(self, new_pos):
+        self.pos = new_pos
+    def set_vel(self, new_vel):
+        self.vel = new_vel
+    def set_accel(self, new_accel):
+        self.accel = new_accel
+    def set_force(self, new_force):
+        self.force = new_force
+
+    def integrate(self, delta_t):
+        self.vel = self.vel + self.accel * delta_t
+        self.pos = self.pos + self.vel * delta_t
+
+    def print(self):
+        print(f"Pos: {self.pos}, Vel: {self.vel}, Accel: {self.accel}")
+
+
+class Joint():
+    def __init__(self, pos=np.copy(ZERO_VECTOR), fixed: bool=False):
+        self.statespace = StateSpace(pos, ZERO_VECTOR, ZERO_VECTOR, ZERO_VECTOR)
+
+        self.connected_springs: List[Spring] = []
+        self.connected_masses: List[Mass] = []
+        self.connected_rigid_bodies: List[RigidBody] = []
+        self.external_forces: List[List[float, float, float]] = []
+
+        self.fixed = fixed
+
+        self.parent_joints: List[Joint] = []
+        self.child_joints: List[Joint] = []
+    
+    def get_pos(self):
+        return self.statespace.get_pos()
+    def get_vel(self):
+        return self.statespace.get_vel()
+    def get_accel(self):
+        return self.statespace.get_accel()
+    def get_force(self):
+        return self.statespace.get_force()
+    
+    def get_connected_springs(self):
+        return self.connected_springs
+    def get_connected_masses(self):
+        return self.connected_masses
+    def get_connected_rigid_bodies(self):
+        return self.connected_rigid_bodies
+    
+    def is_fixed(self):
+        return self.fixed
+    
+    def add_spring(self, new_spring):
+        self.connected_springs.append(new_spring)
+
+    def add_mass(self, new_mass):
+        self.connected_masses.append(new_mass)
+
+    def add_rigid_body(self, new_rigid_body):
+        self.connected_rigid_bodies.append(new_rigid_body)
+    
+    def add_extenal_force(self, new_force = ZERO_VECTOR):
+        self.external_forces.append(new_force)
+
+    def calc_net_spring_force(self):
+        net_spring_force = 0
+        for spring in self.connected_springs:
+            spring_force = spring.calc_spring_force() # This is the force the spring exerts on the joint ASSUMING self is parent_joint of the spring
+            net_spring_force += spring_force
+        return net_spring_force
+
+    
+    def calc_net_external_force(self):
+        net_external_force = np.copy(ZERO_VECTOR)
+        for force in self.external_forces:
+            net_external_force += force
+        return net_external_force
+    
+    def calc_net_force(self):
+        net_external_force = self.calc_net_external_force()
+        net_spring_force = self.calc_net_spring_force()
+        self.statespace.force = net_external_force + net_spring_force
+        return self.statespace.force
+    
+    def integrate_accel_vel(self, delta_t: float=0.1):
+        self.statespace.integrate(delta_t)
+
+    def set_accel(self, accel):
+        self.statespace.set_accel(accel)
+
+    def find_parent_joints(self):
+        for spring in self.connected_springs:
+            if spring.child_joint == self:
+                if spring.parent_joint not in self.parent_joints:
+                    self.parent_joints.append(spring.parent_joint)
+
+        for mass in self.connected_masses:
+            if mass.child_joint == self:
+                if mass.parent_joint not in self.parent_joints:
+                    self.parent_joints.append(mass.parent_joint)
+
+    def find_child_joints(self):
+        for spring in self.connected_springs:
+            if spring.parent_joint == self:
+                if spring.child_joint not in self.child_joints:
+                    self.child_joints.append(spring.child_joint)
+
+        for mass in self.connected_masses:
+            if mass.parent_joint == self:
+                if mass.child_joint not in self.child_joints:
+                    self.child_joints.append(mass.child_joint)
+
+class Spring():
+    def __init__(self, parent_joint: Joint, child_joint: Joint, zero_length: float=0, stiffness: float=0):
+        self.parent_joint = parent_joint
+        self.child_joint = child_joint
+        
+        self.zero_length = zero_length
+        self.stiffness = stiffness
+
+        self.vector = self.calc_r_vector()
+        self.length = self.calc_length()
+        self.unit_vector = self.calc_r_unit_vector()
+        self.spring_force = self.calc_spring_force()
+
+        self.parent_joint.add_spring(self)
+        self.child_joint.add_spring(self)
+    
+    def calc_r_vector(self):
+        self.vector = self.child_joint.get_pos() - self.parent_joint.get_pos()
+        return self.vector
+
+    def calc_length(self):
+        self.vector = self.calc_r_vector()
+        return np.linalg.norm(self.vector)
+        
+    def calc_r_unit_vector(self):
+        self.vector = self.calc_r_vector()
+        self.length = self.calc_length()
+        self.unit_vector = self.vector / self.length
+        return self.unit_vector
+    
+    def calc_spring_force(self):
+        '''Positive force is in tension. Aka, the spring PULLS on the other object'''
+        self.length = self.calc_length()
+        del_length = self.length - self.zero_length
+        spring_force = del_length * self.stiffness
+
+        self.r_unit_vector = self.calc_r_unit_vector()
+        self.spring_force = spring_force * self.r_unit_vector
+        return self.spring_force
+        
+
+class Mass():
+    '''
+    A mass is a point mass located in the center of 2 joints.
+    It cannot exert a force
+    '''
+    def __init__(self, parent_joint: Joint, child_joint: Joint, mass: float=0):
+        self.parent_joint = parent_joint
+        self.child_joint = child_joint
+
+        self.mass = mass
+
+        self.parent_joint.add_mass(self)
+        self.child_joint.add_mass(self)
+
+
+        self.statespace = StateSpace((child_joint.get_pos() + parent_joint.get_pos()) / 2.0,
+                                     (child_joint.get_vel() + parent_joint.get_vel()) / 2.0,
+                                     (child_joint.get_accel() + parent_joint.get_accel()) / 2.0,
+                                     (child_joint.get_force() + parent_joint.get_force()) / 2.0)
+
+    def set_accel(self, accel):
+        self.statespace.set_accel(accel)
+
+    def integrate_accel_vel(self, delta_t: float=0.1):
+        self.statespace.integrate(delta_t)
+
+
+
+class Simulation():
+    def __init__(self, parent_joint: Joint, duration: float, delta_t: float):
+        self.parent_joint = parent_joint
+        self.duration = duration
+        self.delta_t = delta_t
+
+        self.joints: List[Joint] = []
+        self.masses: List[Mass] = []
+        self.springs: List[Spring] = []
+        self.rigid_bodies: List[RigidBody] = []
+
+    def get_all_nodes_and_edges(self):
+        '''
+        Do a BFS to get all of the joints (nodes) and springs/masses (edges) in the system
+        '''
+        queue: List[Joint] = []
+
+        queue.append(self.parent_joint)
+
+        while queue:
+            node: Joint
+            node = queue.pop(0)
+            if node not in self.joints:
+                self.joints.append(node)
+            
+            connected_springs = node.get_connected_springs()
+            connected_masses = node.get_connected_masses()
+            connected_rigid_bodies = node.get_connected_rigid_bodies()
+            
+            for spring in connected_springs:
+                if spring not in self.springs:
+                    self.springs.append(spring)
+
+                if spring.child_joint not in self.joints and spring.child_joint not in queue:
+                    queue.append(spring.child_joint)
+
+            for mass in connected_masses:
+                if mass not in self.masses:
+                    self.masses.append(mass)
+                if mass.child_joint not in self.joints and mass.child_joint not in queue:
+                    queue.append(mass.child_joint)
+            
+            for rigid_body in connected_rigid_bodies:
+                if rigid_body not in self.rigid_bodies:
+                    self.rigid_bodies.append(rigid_body)
+                if rigid_body.child_joint not in self.joints and rigid_body.child_joint not in queue:
+                    queue.append(rigid_body.child_joint)
+
+    def print_components(self):
+        print("Joints:")
+        count = 0
+        for joint in self.joints:
+            print(f"Accel: {joint.get_accel()}")
+            print(f"Vel: {joint.get_vel()}")
+            print(f"Pos: {joint.get_pos()}")
+            print()
+        
+        print("Masses:")
+        count = 0
+        for mass in self.masses:
+            print(f"Accel: {mass.statespace.get_accel()}")
+            print(f"Vel: {mass.statespace.get_vel()}")
+            print(f"Pos: {mass.statespace.get_pos()}")
+            print()
+        
+        print("Springs:")
+        count = 0
+        for spring in self.springs:
+            print(f"Spring Force: {spring.spring_force}")
+            print(f"Spring Length: {spring.length}")
+            print()
+        
+        print("Rigid Bodies:")
+        count = 0
+        for rigid_body in self.rigid_bodies:
+            print(f"Transfer Force: {rigid_body.force}")
+            print()
+
+    def calc_force_at_every_joint(self):
+        '''
+        At every joint, calculate the net force at each joint (this accounts for springs and external forces).
+        '''
+        for joint in self.joints:
+            joint.calc_net_force()
+
+    def calc_rigid_body_force(self):
+        for body in self.rigid_bodies:
+            body.force = 0
+
+    def calc_accel_at_every_mass(self):
+        '''
+        Using the sum of the forces at the 2 joints on each mass, we calc the accel of the mass
+        '''
+        for mass in self.masses:
+            net_force = mass.parent_joint.get_force() + mass.child_joint.get_force()
+            accel = net_force / mass.mass
+            mass.set_accel(accel)
+
+    def assign_joint_accel(self):
+        '''
+        If the joint is fixed, accel = 0
+        '''
+        net_joint_accel = np.copy(ZERO_VECTOR)
+        for joint in self.joints:
+            if joint.is_fixed() == True:
+                continue
+
+            for mass in joint.get_connected_masses():
+                net_joint_accel += mass.statespace.get_accel()
+            joint.set_accel(net_joint_accel)
+
+    def integrate_timestep(self):
+        self.calc_force_at_every_joint()
+        self.calc_accel_at_every_mass()
+
+        self.assign_joint_accel()
+
+        for joint in self.joints:
+            joint.integrate_accel_vel(self.delta_t)
+
+        for mass in self.masses:
+            mass.integrate_accel_vel(self.delta_t)
+
+
+
+    def run(self):
+        mass_pos_values = []
+        time_values = []
+        t = 0
+        while t < self.duration:
+            self.integrate_timestep()
+            self.print_components()
+            
+            mass_pos_values.append(self.masses[0].statespace.get_pos())
+            time_values.append(t)
+
+            print("*"*100)
+            t += self.delta_t
+
+        plt.close()
+        # Plot the data
+        plt.figure(figsize=(12, 8))
+
+        plt.plot(time_values, mass_pos_values)
+        plt.title('Position vs Time')
+        plt.xlabel('Time (s)')
+        plt.ylabel('Position')
+        plt.show()
+
+
+class BFSSimulation():
+    def __init__(self, parent_joint: Joint, duration: float, delta_t: float):
+        self.parent_joint = parent_joint
+        self.duration = duration
+        self.delta_t = delta_t
+
+        self.joints: List[Joint] = []
+
+        self.get_joints_bfs()
+        self.get_parent_joints_for_every_joint()
+
+
+    def get_joints_bfs(self):
+        queue: List[Joint] = []
+
+        queue.append(self.parent_joint)
+        visited_springs: List[Spring] = []
+        visited_masses: List[Spring] = []
+
+        while queue:
+            node: Joint
+            node = queue.pop(0)
+            if node not in self.joints:
+                self.joints.append(node)
+
+            connected_springs = node.get_connected_springs()
+            connected_masses = node.get_connected_masses()
+            
+            for spring in connected_springs:
+                if spring not in visited_springs:
+                    visited_springs.append(spring)
+
+                if spring.child_joint not in self.joints and spring.child_joint not in queue:
+                    queue.append(spring.child_joint)
+
+            for mass in connected_masses:
+                if mass not in visited_masses:
+                    visited_masses.append(mass)
+                if mass.child_joint not in self.joints and mass.child_joint not in queue:
+                    queue.append(mass.child_joint)
+
+    def get_parent_joints_for_every_joint(self):
+        for joint in self.joints:
+            joint.find_parent_joints()
+
+    def get_child_joints_for_every_joint(self):
+        for joint in self.joints:
+            joint.find_child_joints()
+            
+    def integrate_joints(self):
+        for joint in self.joints:
+            joint.calc_net_force()
+
+def main():
+    joint_left_mass1 = Joint(np.array([0,0,0]), fixed=False)
+    joint_mass1_spring1 = Joint(np.array([0,0,0]), fixed=False)
+
+    joint_spring1_rigidbody1 = Joint(np.array([10,0,0]), fixed=False)
+    joint_rigidbody1_spring2 = Joint(np.array([10,0,0]), fixed=False)
+
+    joint_right_spring2 = Joint(np.array([20,0,0]), fixed=True)
+
+    mass1 = Mass(joint_left_mass1, joint_mass1_spring1, 5)
+    spring1 = Spring(joint_mass1_spring1, joint_spring1_rigidbody1, 10, 10)
+    #rigidbody1 = RigidBody(joint_spring1_rigidbody1, joint_rigidbody1_spring2, 0.0)
+    spring2 = Spring(joint_rigidbody1_spring2, joint_right_spring2, 10, 100)
+
+    joint_left_mass1.add_extenal_force(np.array([5, 0, 0]))
+
+    simulation = BFSSimulation(joint_left_mass1, 1, 0.1)
+
+
+    
+
+    simulation.get_all_nodes_and_edges()
+
+    simulation.run()
+
+if __name__ == "__main__":
+    main()
+    

code/deprecated/is_picklable.py

@@ -0,0 +1,30 @@
+import pickle
+from PhysicsElements import StateSpace, ZERO_VECTOR, Joint, Spring
+import numpy as np
+from Object_Sharing import SharedJointList
+
+def is_picklable(obj):
+    try:
+        pickle.dumps(obj)
+        return True
+    except pickle.PicklingError:
+        return False
+
+from multiprocessing.managers import BaseManager
+
+def func(x,y):
+    return 10
+
+if __name__ == '__main__':
+    BaseManager.register('Joint', Joint)
+    BaseManager.register('Spring', Spring)
+
+    manager = BaseManager()
+    manager.start()
+
+    north_wall_joint:Joint = manager.Joint(np.array([10,0,0]), mass=0.001, fixed=True, name="North Wall Joint")
+    main_mass:Joint = manager.Joint(np.array([0,0,0]), mass=3, fixed=False, name="Blue Mass")
+    middle_mass:Joint = manager.Joint(np.array([5,0,0]), mass=5, fixed=False, name="White Mass")
+    spring1 = manager.Spring(parent_joint=middle_mass, child_joint=north_wall_joint, unstretched_length=7.5, stiffness_func=func, name="Spring 1")
+
+    print(is_picklable(spring1))

code/deprecated/live_plotter_test.py

@@ -0,0 +1,111 @@
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+import random
+from multiprocessing import Process, Manager
+
+class LivePlot:
+    def __init__(self, mult=1, max_data_points=100):
+        fig, (ax1, ax2, ax3) = plt.subplots(3, 1, figsize=(8, 6))
+        self.fig = fig
+        self.ax1 = ax1
+        self.ax2 = ax2
+        self.ax3 = ax3
+
+        self.mult = mult
+
+        self.ax1.set_ylabel('Plot 1')
+        self.ax2.set_ylabel('Plot 2')
+        self.ax3.set_ylabel('Plot 3')
+        self.ax3.set_xlabel('Time')
+
+        self.max_data_points = max_data_points
+        self.t = 0
+        self.x_data = []
+        self.y_data1 = []
+        self.y_data1_2 = []
+        self.y_data2 = []
+        self.y_data3 = []
+
+        # Initialize lines (empty initially)
+        self.line1, = self.ax1.plot([], [], label='Plot 1')
+        self.line1_2, = self.ax1.plot([], [], label='Plot 1.2')
+        self.line2, = self.ax2.plot([], [], label='Plot 2')
+        self.line3, = self.ax3.plot([], [], label='Plot 3')
+
+        self.ax1.legend()
+        self.ax2.legend()
+        self.ax3.legend()
+
+    def generate_random_data(self):
+        return random.randint(1, 100)
+
+    def update_data_external(self):
+        # Simulate external updates (replace with your actual data update mechanism)
+        new_x = self.t
+        self.t += 1
+        new_y1 = self.generate_random_data() * self.mult
+        new_y2 = self.generate_random_data() * self.mult
+        new_y3 = self.generate_random_data() * self.mult
+
+        self.x_data.append(new_x)
+        self.y_data1.append(new_y1)
+        self.y_data1_2.append(new_y1 * -1)  # Example modification of data
+        self.y_data2.append(new_y2)
+        self.y_data3.append(new_y3)
+
+        # Keep only the last `max_data_points` data points
+        self.x_data = self.x_data[-self.max_data_points:]
+        self.y_data1 = self.y_data1[-self.max_data_points:]
+        self.y_data1_2 = self.y_data1_2[-self.max_data_points:]
+        self.y_data2 = self.y_data2[-self.max_data_points:]
+        self.y_data3 = self.y_data3[-self.max_data_points:]
+
+    def update_plot(self, i):
+        self.update_data_external()
+
+        # Update plot data
+        self.line1.set_data(self.x_data, self.y_data1)
+        self.line1_2.set_data(self.x_data, self.y_data1_2)
+        self.line2.set_data(self.x_data, self.y_data2)
+        self.line3.set_data(self.x_data, self.y_data3)
+
+        # Adjust plot limits (x-axis)
+        if len(self.x_data) > 1:
+            self.ax1.set_xlim(self.x_data[0], self.x_data[-1])
+            self.ax2.set_xlim(self.x_data[0], self.x_data[-1])
+            self.ax3.set_xlim(self.x_data[0], self.x_data[-1])
+
+        # Adjust plot limits (y-axis) - optional
+        self.ax1.relim()
+        self.ax1.autoscale_view()
+        self.ax2.relim()
+        self.ax2.autoscale_view()
+        self.ax3.relim()
+        self.ax3.autoscale_view()
+
+        #return self.line1, self.line2, self.line3
+
+    def animate(self):
+        anim = animation.FuncAnimation(self.fig, self.update_plot, frames=1000, interval=10, blit=False)
+        plt.show()
+        #anim.save(filename="test.mp4")
+
+# Function to run animation in a separate process
+def run_animation(fig, ax1, ax2, ax3):
+    #live_plot = LivePlot(fig, ax1, ax2, ax3)
+    #live_plot.animate()
+    pass
+
+# Example usage with multiprocessing
+if __name__ == '__main__':
+    plot1 = LivePlot(mult=1)
+    plot2 = LivePlot(mult=5)
+
+    process1 = Process(target=plot1.animate)
+    process2 = Process(target=plot2.animate)
+
+    process1.start()
+    process2.start()
+
+    process1.join()
+    process2.join()

code/deprecated/mcc_usb1024_test.py

@@ -0,0 +1,59 @@
+from mcculw import ul
+from mcculw.enums import DigitalIODirection
+from mcculw.ul import ULError
+
+# Define the board number
+BOARD_NUM = 0
+
+# Define the ports and the direction of data flow
+PORT_A = 10
+PORT_B = 11
+PORT_C_LOW = 12
+PORT_C_HIGH = 13
+
+# Configure ports as input or output
+def configure_ports():
+    try:
+        ul.d_config_port(BOARD_NUM, PORT_A, DigitalIODirection.OUT)
+        ul.d_config_port(BOARD_NUM, PORT_B, DigitalIODirection.IN)
+        ul.d_config_port(BOARD_NUM, PORT_C_LOW, DigitalIODirection.OUT)
+        ul.d_config_port(BOARD_NUM, PORT_C_HIGH, DigitalIODirection.IN)
+        print("Ports configured successfully.")
+    except ULError as e:
+        print(f"Error configuring ports: {e}")
+
+# Write data to a port
+def write_data(port, data):
+    try:
+        ul.d_out(BOARD_NUM, port, data)
+        print(f"Data {data} written to port {port} successfully.")
+    except ULError as e:
+        print(f"Error writing data to port {port}: {e}")
+
+# Read data from a port
+def read_data(port):
+    try:
+        data = ul.d_in(BOARD_NUM, port)
+        print(f"Data read from port {port}: {data}")
+        return data
+    except ULError as e:
+        print(f"Error reading data from port {port}: {e}")
+        return None
+
+def main():
+    # Configure the ports
+    configure_ports()
+    
+    # Write some data to PORT_A and PORT_C_LOW
+    write_data(PORT_A, 0xFF)  # Write 0xFF (255) to PORT_A
+    write_data(PORT_C_LOW, 0xAA)  # Write 0xAA (170) to PORT_C_LOW
+    
+    # Read data from PORT_B and PORT_C_HIGH
+    data_b = read_data(PORT_B)
+    data_c_high = read_data(PORT_C_HIGH)
+    
+    # Perform additional processing as needed
+    # For example, you might want to perform some logic based on the input data
+    
+if __name__ == "__main__":
+    main()

code/deprecated/pstats_reader.py

@@ -0,0 +1,8 @@
+import cProfile
+import pstats
+
+# Load the profiling data from the file
+stats = pstats.Stats('profile_results.txt')
+
+# Print the top 10 functions by cumulative time
+stats.sort_stats('cumulative').print_stats(25)

code/deprecated/pyqtgraph_test.py

@@ -0,0 +1,78 @@
+import pyqtgraph as pg
+from pyqtgraph.Qt import QtWidgets, QtCore
+import numpy as np
+
+class RealTimePlot:
+    def __init__(self, update_interval=1, max_display_time=10, num_subplots=2, line_styles=None):
+        # Create the application
+        self.app = QtWidgets.QApplication([])
+
+        # Create a plot window
+        self.win = pg.GraphicsLayoutWidget(show=True, title="Real-Time Plot")
+        self.win.resize(1000, 600)
+
+        # Add plots
+        self.plots = []
+        self.curves = []
+        self.line_styles = line_styles if line_styles is not None else ['line'] * num_subplots
+        for i in range(num_subplots):
+            plot = self.win.addPlot(title=f"Subplot {i+1}")
+            plot.setLabel('left', f'Subplot {i+1} Y-Axis')
+            plot.addLegend()  # Add a legend to each plot
+            self.plots.append(plot)
+
+            if self.line_styles[i] == 'step':
+                curve = plot.plot(pen='y', name=f'Subplot {i+1} Data', stepMode=True)
+            else:
+                curve = plot.plot(pen='y', name=f'Subplot {i+1} Data')
+            
+            self.curves.append(curve)
+            if i < num_subplots - 1:
+                self.win.nextRow()
+
+        # Data buffers
+        self.xdata = [np.empty(0) for _ in range(num_subplots)]
+        self.ydata = [np.empty(0) for _ in range(num_subplots)]
+
+        # Parameters
+        self.update_interval = update_interval
+        self.max_display_time = max_display_time
+        self.timer = QtCore.QTimer()
+        self.timer.timeout.connect(self.update_live_window)
+        self.timer.start(update_interval)
+
+    def update_live_window(self):
+        for i in range(len(self.plots)):
+            # Generate new data
+            t = self.xdata[i][-1] + self.update_interval / 1000.0 if len(self.xdata[i]) > 0 else 0
+            y = np.sin(2 * np.pi * t + i)  # Different phase for each subplot
+
+            # Append new data to buffers
+            self.xdata[i] = np.append(self.xdata[i], t)
+            self.ydata[i] = np.append(self.ydata[i], y)
+
+            # Remove old data to keep the buffer size within max_display_time
+            if t > self.max_display_time:
+                self.xdata[i] = self.xdata[i][self.xdata[i] > t - self.max_display_time]
+                self.ydata[i] = self.ydata[i][-len(self.xdata[i]):]
+
+            # Ensure correct lengths for step mode
+            if self.line_styles[i] == 'step':
+                xdata_step = np.append(self.xdata[i], self.xdata[i][-1] + self.update_interval / 1000.0)
+                self.curves[i].setData(xdata_step, self.ydata[i])
+            else:
+                self.curves[i].setData(self.xdata[i], self.ydata[i])
+
+    def run(self):
+        # Start the Qt event loop
+        QtWidgets.QApplication.instance().exec_()
+
+# Parameters
+update_interval = 100  # milliseconds
+max_display_time = 10  # seconds
+num_subplots = 2  # number of subplots
+line_styles = ['line', 'step']  # specify 'line' or 'step' for each subplot
+
+# Create and run the real-time plot
+rt_plot = RealTimePlot(update_interval, max_display_time, num_subplots, line_styles)
+rt_plot.run()

code/deprecated/shared_memory.py

@@ -0,0 +1,99 @@
+import multiprocessing
+from multiprocessing.managers import BaseManager
+import numpy as np
+import time
+import random
+    
+from PhysicsElements import Joint, Spring, SpringWithMass, Actuator, StateSpace
+
+class CustomManager(BaseManager):
+    # nothing
+    pass
+
+
+def worker_process(shared_state_space):
+    while True:
+        print("Worker Process:")
+        shared_state_space.print()
+        time.sleep(0.1)
+
+def worker_process2(shared_joint: Joint):
+    while True:
+        print("Worker Process:")
+        statespace = shared_joint.get_state_space()
+        print(statespace.print())
+        time.sleep(0.1)
+
+def worker_process3(shared_joint_list: list):
+    while True:
+        print("Worker Process:")
+        statespace:StateSpace = shared_joint_list.at(0).get_state_space()
+        statespace.print()
+        time.sleep(0.1)
+
+def statespace_changer(shared_joint_list: list):
+    while True:
+        rand_int = random.randint(0, 10)
+        shared_joint_list.at(0).set_state_space_pos(np.array([rand_int,0,0]))
+        print("Changed pos")
+        time.sleep(1)
+
+class SharedJointList():
+    def __init__(self):
+        self.list: list = []
+
+
+    def append(self, new_joint: Joint):
+        self.list.append(new_joint)
+
+    def at(self, index: int):
+        return self.list[index]
+    
+
+
+
+if __name__ == "__main__":
+    # Create a multiprocessing manager
+    # CustomManager.register('StateSpace', StateSpace)
+    # CustomManager.register('Joint', Joint)
+    # CustomManager.register('SharedJointList', SharedJointList)
+    #BaseManager.register('StateSpace', StateSpace)
+    BaseManager.register('Joint', Joint)
+    BaseManager.register('SharedJointList', SharedJointList)
+
+    with BaseManager() as manager:
+        #shared_statespace = manager.StateSpace()
+
+        shared_joint:Joint = manager.Joint(pos=np.array([0,0,-4]), mass=0.001, fixed=True, name="North Actuator Joint")
+        
+        joint_list:SharedJointList = manager.SharedJointList()
+
+        rand_joint = manager.Joint(pos=np.array([0,2,15]), 
+                                mass=0.001, 
+                                fixed=True, 
+                                name="Random Joint")
+        joint_list.append(rand_joint)
+
+
+        #proc = multiprocessing.Process(target=worker_process, args=(shared_statespace,))
+        #proc = multiprocessing.Process(target=worker_process2, args=(shared_joint,))
+        proc = multiprocessing.Process(target=worker_process3, args=(joint_list,))
+        proc.start()
+
+        changer_proc = multiprocessing.Process(target=statespace_changer, args=(joint_list,))
+        changer_proc.start()
+        time.sleep(2)
+
+        #shared_statespace.set_pos(np.array([-8,-4,-2]))
+        #shared_joint.set_state_space_pos((np.array([-8,-4,-2])))
+        
+        
+        
+        
+        while True:
+            print("Main: ", end="")
+            joint_list.at(0).get_state_space().print()
+            time.sleep(1)
+
+        proc.join()
+        changer_proc.join()

code/deprecated/tests/.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "python.REPL.enableREPLSmartSend": false
+}

code/setup/code/.vscode/launch.json

@@ -0,0 +1,21 @@
+{
+    // Use IntelliSense to learn about possible attributes.
+    // Hover to view descriptions of existing attributes.
+    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+    "version": "0.2.0",
+    "configurations": [
+        {
+            "name": "Python Debugger: Current File",
+            "type": "debugpy",
+            "request": "launch",
+            "program": "${file}",
+            "console": "integratedTerminal",
+            "env": {
+                "PYDEVD_DISABLE_FILE_VALIDATION": "1"
+            },
+            "args": [
+                "-Xfrozen_modules=off"
+            ]
+        }
+    ]
+}

code/setup/code/.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "python.REPL.enableREPLSmartSend": false
+}

code/setup/code/Controller_Input_Elements.py

@@ -0,0 +1,197 @@
+import numpy as np
+
+from Physics_Elements import SharedRigidActuator
+
+from typing import TYPE_CHECKING
+if TYPE_CHECKING:
+    from Object_Sharing import SharedFloat
+    from Physics_Elements import StateSpace, Spring
+
+class RigidActuatorController():
+    def __init__(self, controller_settings: dict):
+        self.controller_settings = controller_settings
+
+        self.attached_sim_target: SharedRigidActuator = None
+
+        self.shared_attributes: SharedRigidActuatorController = None # For plotting if desired
+
+        self.parse_settings()
+
+        self.input_voltage: float = 0.0
+        self.digital_command: int = 0 # 0 means hold position, 1 means control using velocity
+
+        self.controlled_length: float = 0.0
+        self.controlled_vel_scalar: float = 0.0
+        self.controlled_pos = np.array([0.0, 0.0, 0.0])
+        self.controlled_vel = np.array([0.0, 0.0, 0.0])
+
+    def parse_settings(self):
+        self.name = self.controller_settings["name"]
+        self.input_channel = self.controller_settings["analog_input_channel"]
+        self.digital_channel = self.controller_settings["digital_input_channel"]
+        self.max_length = self.controller_settings["max_length"]
+        self.min_length = self.controller_settings["min_length"]
+        self.neutral_length = self.controller_settings.get("neutral_length", 0)
+        self.controls_pos_vel_accel = self.controller_settings["controls_pos/vel/accel"]
+
+    def calc_neutral_pos(self):
+        delta_pos_from_child = -1 * self.neutral_length * self.r_unit_vector
+        self.neutral_pos = self.attached_sim_target.get_child_statespace().get_pos() + delta_pos_from_child
+
+    def get_input_channel(self) -> int:
+        return self.input_channel
+    def get_digital_channel(self) -> int:
+        return self.digital_channel
+
+    def attach_sim_target(self, target: 'SharedRigidActuator'):
+        if not isinstance(target._getvalue(), SharedRigidActuator):
+            raise TypeError(f"target for RigidActuatorController {self.name} is not a SharedRigidActuator")
+        
+        self.attached_sim_target = target
+        self.r_unit_vector = self.get_r_unit_vector()
+        self.calc_neutral_pos()
+        self.controlled_pos = self.neutral_pos
+        self.attached_sim_target.set_parent_statespace_pos(self.controlled_pos)
+
+    def attach_shared_attributes(self, shared_attributes: 'SharedRigidActuatorController'):
+        if not isinstance(shared_attributes._getvalue(), SharedRigidActuatorController):
+            raise TypeError(f"shared_attributes for RigidActuatorController {self.name} is not a SharedRigidActuatorController")
+        self.shared_attributes = shared_attributes
+        
+
+    def get_r_unit_vector(self):
+        return self.attached_sim_target.get_r_unit_vector()
+
+    def set_input_voltage(self, voltage: float):
+        '''After reading a voltage, set it to this controller element'''
+        self.input_voltage = voltage
+
+    def set_digital_command(self, command: int):
+        self.digital_command = command
+
+    def set_controlled_attribute(self):
+        if self.controls_pos_vel_accel == 0: # Control pos
+            self.controlled_length = (self.input_voltage - self.controller_settings["neutral_voltage"]) * self.controller_settings["units_per_volt"] + self.neutral_length
+            self.set_controlled_pos(self.controlled_length)
+        elif self.controls_pos_vel_accel == 1: # Controls vel
+            self.set_controlled_vel()
+        elif self.controls_pos_vel_accel == 2: # Controls accel
+            self.set_controlled_accel()
+
+    def set_controlled_vel(self):
+        # Check if the controlled vel would put us outside our max or min displacement
+            # if it is, set controlled vel to 0 and set our controlled pos manually
+        current_length = self.attached_sim_target.get_length()
+        self.controlled_vel_scalar = (self.input_voltage - self.controller_settings["neutral_voltage"]) * self.controller_settings["units_per_volt"]
+        if current_length > self.max_length:
+            self.set_controlled_pos(self.max_length)
+            if self.controlled_vel_scalar > 0:
+                self.controlled_vel_scalar = 0
+        elif current_length < self.min_length:
+            self.set_controlled_pos(self.min_length)
+            if self.controlled_vel_scalar < 0:
+                self.controlled_vel_scalar = 0
+        self.r_unit_vector = self.get_r_unit_vector()
+        controlled_vel = -1*self.r_unit_vector * self.controlled_vel_scalar # -1 * r vector becaus r vector describes child - parent
+        self.controlled_vel = controlled_vel
+
+    def set_controlled_accel(self): 
+        pass
+
+    def set_controlled_pos(self, length:float):
+        self.r_unit_vector = self.get_r_unit_vector()
+        if length > self.max_length:
+            self.controlled_length = self.max_length
+        elif length < self.min_length:
+            self.controlled_length = self.min_length
+        delta_pos_from_child = self.controlled_length * -1*self.r_unit_vector # -1 * r vector becaus r vector describes child - parent
+        child_pos = self.attached_sim_target.get_child_statespace().get_pos()
+        self.controlled_pos = delta_pos_from_child + child_pos
+
+    def update_shared_attributes(self):
+        if self.shared_attributes is not None:
+            self.shared_attributes.set_digital_command(self.digital_command)
+            self.shared_attributes.set_input_voltage(self.input_voltage)
+            
+            self.shared_attributes.set_controlled_pos(self.controlled_pos)
+            self.shared_attributes.set_controlled_vel(self.controlled_vel)
+            self.shared_attributes.set_controlled_length(self.controlled_length)
+            self.shared_attributes.set_controlled_vel_scalar(self.controlled_vel_scalar)
+        
+
+    def update_sim_target(self):
+        '''Update the sim target with the values we input from the controller'''
+        if self.controls_pos_vel_accel == 0: # Control pos
+            if self.digital_command == 1: # We are actively controlling the actuator
+                self.attached_sim_target.set_parent_statespace_pos(self.controlled_pos)
+        elif self.controls_pos_vel_accel == 1: # Controls vel
+            if self.digital_command == 0: # We are NOT actively controlling the actuator
+                self.attached_sim_target.set_parent_statespace_vel([0,0,0])
+            else: # We are actively controlling the vel
+                self.attached_sim_target.set_parent_statespace_vel(self.controlled_vel)
+        elif self.controls_pos_vel_accel == 2: # Controls accel
+            pass
+        
+
+
+class SharedRigidActuatorController():
+    def __init__(self):
+        self.input_voltage: float = 0.0
+        self.digital_command: int = 0
+
+        self.controlled_length: float = 0.0
+        self.controlled_vel_scalar: float = 0.0
+        self.controlled_pos = np.array([0.0, 0.0, 0.0])
+        self.controlled_vel = np.array([0.0, 0.0, 0.0])
+
+        self.connected_to_plotter: bool = False
+        self.connected_to_visualization: bool = False
+        self.connected_to_sensor: bool = False
+        self.connected_to_controller: bool = False
+
+    def set_input_voltage(self, voltage: float):
+        self.input_voltage = voltage
+    def set_digital_command(self, command: int):
+        self.digital_command = command
+    def set_controlled_pos(self, new_pos):
+        self.controlled_pos = new_pos
+    def set_controlled_vel(self, new_vel):
+        self.controlled_vel = new_vel
+    def set_controlled_length(self, disp: float) -> None:
+        self.controlled_length = disp
+    def set_controlled_vel_scalar(self, vel_scalar: float) -> None:
+        self.controlled_vel_scalar = vel_scalar
+
+    def set_connected_to_plotter(self, state: bool) -> None:
+        self.connected_to_plotter = state
+    def set_connected_to_visualization(self, state: bool) -> None:
+        self.connected_to_visualization = state
+    def set_connected_to_sensor(self, state: bool) -> None:
+        self.connected_to_sensor = state
+    def set_connected_to_controller(self, state: bool) -> None:
+        self.connected_to_controller = state
+    
+
+    def get_connected_to_plotter(self) -> bool:
+        return self.connected_to_plotter
+    def get_connected_to_visualization(self) -> bool:
+        return self.connected_to_visualization
+    def get_connected_to_sensor(self) -> bool:
+        return self.connected_to_sensor
+    def get_connected_to_controller(self) -> bool:
+        return self.connected_to_controller
+
+    def get_controlled_pos(self):
+        return self.controlled_pos
+    def get_controlled_vel(self):
+        return self.controlled_vel
+    def get_controlled_length(self) -> float:
+        return self.controlled_length
+    def get_controlled_vel_scalar(self) -> float:
+        return self.controlled_vel_scalar
+
+
+    def get_input_voltage(self) -> float:
+        return self.input_voltage
+    def get_digital_command(self) -> int:
+        return self.digital_command

code/setup/code/HITL_Controller.py

@@ -0,0 +1,147 @@
+import time
+from mcculw.ul import ignore_instacal
+
+import threading
+
+from Physics_Elements import Joint, Spring, StateSpace
+from MCC_Interface import MCC3114, MCC202
+
+from typing import List
+from typing import TYPE_CHECKING
+if TYPE_CHECKING:
+    from Sensor_Elements import LoadCell, SharedLoadCellJoint, LoadCellSpring, SharedLoadCellSpring, DisplacementSensor
+    from Controller_Input_Elements import RigidActuatorController
+
+class HITLController():
+    def __init__(self, stop_event, settings: dict):
+        self.stop_event = stop_event
+        self.settings = settings
+
+        self.parse_settings()
+        
+        self.load_cells: List[LoadCell] = []
+        #self.displacement_actuators: List[DisplacementSensor] = []
+        self.displacement_sensors: List[DisplacementSensor] = []
+
+        self.rigid_actuator_controllers: List[RigidActuatorController] = []
+
+    def parse_settings(self):
+        '''Parse the settings to get the settings for the MCC devices'''
+        self.pull_from_sim_period = self.settings["pull_from_sim_period"] / 1000.0
+        self.updated_plotting_shared_attributes_period = self.settings["plotting_update_period"] / 1000.0
+
+    def attach_load_cell(self, load_cell: 'LoadCell'):
+        self.load_cells.append(load_cell)
+
+    def attach_displacement_sensor(self, sensor: 'DisplacementSensor'):
+        self.displacement_sensors.append(sensor)
+
+    def attach_rigid_actuator_controller(self, controller: 'RigidActuatorController'):
+        self.rigid_actuator_controllers.append(controller)
+
+    def update_from_sim(self):
+        # Updates the local sensors based on the sim values
+        for lc in self.load_cells:
+            lc.pull_from_sim()
+        # for rigid_actuator in self.displacement_actuators:
+        #     rigid_actuator.pull_from_sim()
+        for sensor in self.displacement_sensors:
+            sensor.pull_from_sim()
+
+    def update_from_sim_thread(self):
+        while self.spare_stop_event.is_set() == False:
+            self.update_from_sim()
+            time.sleep(self.pull_from_sim_period / 1000)
+
+    def update_internal_sensor_attributes(self):
+        '''Updates internal attributes. This should be updated as fast as possible because it generates
+        sensor noise.'''
+        for lc in self.load_cells:
+            lc.update_internal_attributes()
+        # for rigid_actuator in self.displacement_actuators:
+        #     rigid_actuator.update_internal_attributes()
+        for sensor in self.displacement_sensors:
+            sensor.update_internal_attributes()
+    
+    def update_plotting_shared_attributes(self):
+        for lc in self.load_cells:
+            lc.update_shared_attributes()
+        for rigid_actuator_controller in self.rigid_actuator_controllers:
+            rigid_actuator_controller.update_shared_attributes()
+        # for rigid_actuator in self.displacement_actuators:
+        #     rigid_actuator.update_shared_attributes()
+        for sensor in self.displacement_sensors:
+            sensor.update_shared_attributes()
+
+    def update_plotting_thread(self):
+        while self.spare_stop_event.is_set() == False:
+            self.update_plotting_shared_attributes()
+            time.sleep(self.updated_plotting_shared_attributes_period / 1000)
+
+    def update_mcc3114(self):
+        for lc in self.load_cells:
+            channel = lc.sensor_settings["output_channel"]
+            voltage = lc.get_noisy_voltage_scalar()
+            self.mcc3114.voltage_write(channel, voltage)
+        for sensor in self.displacement_sensors:
+            channel = sensor.sensor_settings["output_channel"]
+            voltage = sensor.get_noisy_voltage_scalar()
+            self.mcc3114.voltage_write(channel, voltage)
+
+    def read_from_mcc202(self):
+        for rigid_actuator_controller in self.rigid_actuator_controllers:
+            input_channel:int = rigid_actuator_controller.get_input_channel()
+            voltage = self.mcc202.voltage_read(channel=input_channel)
+            digital_channel:int = rigid_actuator_controller.get_digital_channel()
+            digital_command = self.mcc202.digital_read(channel=digital_channel)
+            
+            rigid_actuator_controller.set_digital_command(digital_command)
+            rigid_actuator_controller.set_input_voltage(voltage)
+            rigid_actuator_controller.set_controlled_attribute()
+
+    def update_sim_targets(self):
+        for rigid_actuator_controller in self.rigid_actuator_controllers:
+            rigid_actuator_controller.update_sim_target()
+
+    def controller_loop(self):
+        while self.stop_event.is_set() == False:
+            self.loop_time = time.time()
+
+            # Update the internal attributes of the sensors
+            self.update_internal_sensor_attributes()
+
+            # Update MCC3114 (analog output)
+            self.update_mcc3114()
+
+            # Get readings from the MCC202
+            self.read_from_mcc202()
+
+            # Update the shared actuators for the sim to respond to
+            self.update_sim_targets()
+            
+
+    def start_mcc(self):
+        ignore_instacal() # ONLY CALL ONCE
+        self.mcc3114 = MCC3114()
+        self.mcc202 = MCC202()
+
+    def run_process(self):
+        self.start_mcc()
+
+        # Make the threads for interacting with shared elements
+        self.spare_stop_event = threading.Event()
+        self.spare_stop_event.clear()
+
+        updating_plotting_elements_thread = threading.Thread(target=self.update_plotting_thread)
+        update_from_sim_thread = threading.Thread(target=self.update_from_sim_thread)
+
+        updating_plotting_elements_thread.start()
+        update_from_sim_thread.start()
+        
+        self.controller_loop()
+
+        self.spare_stop_event.set()
+
+        updating_plotting_elements_thread.join()
+        update_from_sim_thread.join()
+

code/setup/code/MCC_Interface.py

@@ -0,0 +1,200 @@
+from mcculw import ul as mcculw
+from mcculw.enums import (AiChanType, BoardInfo, Status, InterfaceType,
+                          FunctionType, InfoType, ScanOptions,
+                          ULRange, AnalogInputMode, DigitalPortType, DigitalIODirection)
+
+import time
+
+
+class MCC3114():
+    def __init__(self):
+        self.range = ULRange.BIP10VOLTS
+        self.board_num = self.setup_device()
+        if self.board_num == -1:
+            print("Could not setup MCC3114")
+
+        self.config_outputs()
+
+        self.current_voltage_outputs: list[float] = [0.0]*16
+        
+        self.set_all_to_zero()
+
+    def __del__(self):
+        self.set_all_to_zero()
+
+    def setup_device(self) -> int:
+        '''Sets up the device without Instacal configuration files'''
+        board_num = 0
+        board_index = 0
+        find_device = "USB-3114"
+        board_num = -1
+        dev_list = mcculw.get_daq_device_inventory(InterfaceType.USB)
+        if len(dev_list) > 0:
+            for device in dev_list:
+                if str(device) == find_device:
+                    board_num = board_index
+                    mcculw.create_daq_device(board_num, device)
+                board_index = board_index + 1
+            if board_num == -1:
+                print(f"Device {find_device} not found")
+                return board_num
+        else:
+            print("No devices detected")
+            return board_num
+        return board_num
+    
+    def config_outputs(self):
+        if self.board_num == -1:
+            return
+        for ch in range(16):
+            mcculw.set_config(
+                InfoType.BOARDINFO, self.board_num, ch,
+                BoardInfo.DACRANGE, self.range
+            )
+
+
+    def set_all_to_zero(self):
+        '''Make the voltage outputs be 0.0 V when the program exits or we delete the object'''
+        if self.board_num == -1:
+            return
+        for i in range(16):
+            self.voltage_write(channel=i, voltage=0.0)
+    
+    def voltage_write(self, channel:int, voltage: float):
+        if self.board_num == -1:
+            return
+        if channel < 0 or channel > 15:
+            print(f"Channel: {channel} is out of range for the MCC USB-3114. Valid range is [0, 15]")
+            return
+        if voltage < -10:
+            print(f"Voltage: {voltage} is out of range for the MCC USB-3114 in this configuration. Valid range is [-10, 10] V")
+            voltage = -10
+        if voltage > 10:
+            print(f"Voltage: {voltage} is out of range for the MCC USB-3114 in this configuration. Valid range is [-10, 10] V")
+            voltage = 10
+        try:
+            mcculw.v_out(self.board_num, channel, self.range, voltage)
+            self.current_voltage_outputs[channel] = voltage
+        except Exception as exc:
+            print(f"Exception occurred doing voltage write to MCC USB-3114: {exc}")
+
+
+
+
+class MCC202():
+    def __init__(self):
+        self.range = ULRange.BIP10VOLTS
+        self.digital_type = DigitalPortType.AUXPORT
+        self.board_num = self.setup_device()
+
+        if self.board_num == -1:
+            print("Could not setup MCC202")
+
+        self.current_analog_inputs: list[float] = [0.0] * 8
+        self.current_digital_inputs: list[int] = [0] * 8
+
+    def __del__(self):
+        pass  # No specific cleanup required for inputs
+
+    def setup_device(self) -> int:
+        '''Sets up the device without Instacal configuration files'''
+        board_num = 0
+        board_index = 0
+        find_device = "USB-202"
+        board_num = -1
+        dev_list = mcculw.get_daq_device_inventory(InterfaceType.USB)
+        if len(dev_list) > 0:
+            for device in dev_list:
+                if str(device) == find_device:
+                    board_num = board_index
+                    mcculw.create_daq_device(board_num, device)
+                board_index = board_index + 1
+            if board_num == -1:
+                print(f"Device {find_device} not found")
+                return board_num
+        else:
+            print("No devices detected")
+            return board_num
+        return board_num
+    
+    def config_inputs(self):
+        if self.board_num == -1:
+            return
+        for ch in range(8):
+            mcculw.set_config(
+                InfoType.BOARDINFO, self.board_num, ch,
+                BoardInfo.RANGE, self.range
+            )
+        for ch in range(8):
+            mcculw.d_config_port(self.board_num, self.digital_type, DigitalIODirection.IN)
+
+    def print_all_channels(self):
+        for i in range(8):
+            print(f"Analog channel {i} reads {self.current_analog_inputs[i]} Volts")
+        for i in range(8):
+            print(f"Digital channel {i} reads {self.current_digital_inputs[i]}")
+
+    def voltage_read(self, channel: int) -> float:
+        if self.board_num == -1:
+            return 0.0
+        if channel < 0 or channel > 7:
+            print(f"Channel: {channel} is out of range for the MCC USB-202. Valid range is [0, 7]")
+            return 0.0
+        try:
+            voltage = mcculw.v_in(self.board_num, channel, self.range)
+            self.current_analog_inputs[channel] = voltage
+            return voltage
+        except Exception as exc:
+            print(f"Exception occurred doing voltage read from MCC USB-202: {exc}")
+            return 0.0
+    
+    def read_all_analog_channels(self):
+        '''Read all analog input channels and update the current_analog_inputs list'''
+        for i in range(8):
+            self.voltage_read(i)
+        
+    def digital_read(self, channel: int) -> int:
+        if self.board_num == -1:
+            return -1
+        if channel < 0 or channel > 7:
+            print(f"Digital channel: {channel} is out of range for the MCC USB-202. Valid range is [0, 7]")
+            return -1
+        try:
+            state = mcculw.d_bit_in(self.board_num, self.digital_type, channel)
+            self.current_digital_inputs[channel] = state
+            return state
+        except Exception as exc:
+            print(f"Exception occurred doing digital read from MCC USB-202: {exc}")
+            return -1
+    
+    def read_all_digital_channels(self):
+        '''Read all digital input channels and update the current_digital_inputs list'''
+        for i in range(8):
+            self.digital_read(i)    
+    
+
+    def read_all_channels(self):
+        for i in range(8):
+            self.voltage_read(i)
+            self.digital_read(i)
+
+
+def main():
+    mcculw.ignore_instacal() # ONLY CALL ONCE
+    #mcc3114 = MCC3114()
+
+    mcc202 = MCC202()
+
+    while True:
+        mcc202.read_all_channels()
+        mcc202.print_all_channels()
+        time.sleep(3)
+
+
+    #mcc3114.voltage_write(1, 5)
+    #mcc3114.voltage_write(7, 5)
+    #time.sleep(100)
+    
+
+if __name__ == "__main__":
+    main()

code/setup/code/Object_Sharing.py

@@ -0,0 +1,9 @@
+class SharedFloat:
+    def __init__(self, initial_value:float=0.0):
+        self.value = initial_value
+
+    def get(self):
+        return self.value
+
+    def set(self, new_value):
+        self.value = new_value

code/setup/code/Physics_Elements.py

@@ -0,0 +1,711 @@
+from typing import List, Callable
+import numpy as np
+
+
+from typing import TYPE_CHECKING
+if TYPE_CHECKING:
+    from Physics_Elements import StateSpace, Joint, Spring, SharedRigidActuator, SharedSpring
+
+ZERO_VECTOR = np.array([0.0, 0.0, 0.0])
+
+
+'''
+TODO
+1. Change spring_force to just force
+    Include getters and setters, lest important for attributes
+2. Add Object_Sharing objects to this file
+'''
+
+
+def stiffness_function_const_then_rigid(length, unstretched_length):
+    percent_length = 0.9 # For the first bit of length in compression, it follors k*x
+    k = 1000
+
+    if length < (1-percent_length)* unstretched_length:
+        # It is really compressed, so make k = k *10
+        k = k + k*10/(1 + np.exp(-10*(unstretched_length - length - percent_length*unstretched_length)))
+    return k
+
+def damping_force_x(mass: 'Joint', t):
+    x_vel = mass.statespace.get_vel()[0]
+    a = 1
+    b = 0.01
+    return x_vel*a*-1 + x_vel**2 * b*-1
+
+def damping_force_y(mass: 'Joint', t):
+    y_vel = mass.statespace.get_vel()[1]
+    a = 1
+    b = 0.01
+    return y_vel*a*-1 + y_vel**2 * b*-1
+
+def damping_force_z(mass: 'Joint', t):
+    z_vel = mass.statespace.get_vel()[2]
+    a = 1
+    b = 0.01
+    return z_vel*a*-1 + z_vel**2 * b*-1
+
+'''
+TODO
+'''
+class StateSpace():
+    def __init__(self, pos=np.copy(ZERO_VECTOR), vel=np.copy(ZERO_VECTOR), accel=np.copy(ZERO_VECTOR), force=np.copy(ZERO_VECTOR), integration_method="simple"):
+        self.pos = pos
+        self.vel = vel
+        self.accel = accel
+
+        self.force = force
+
+        self.integration_method = self.parse_integration_method(integration_method)
+
+        self.max_saved = 2
+        self.previous_positions: list[list[float, float, float]] = []
+        self.previous_vel: list[list[float, float, float]] = []
+        self.previous_accel: list[list[float, float, float]] = []
+
+        self.verlet_constants = [
+            [],
+            [],
+            [-1, 2, 1],
+            [0, -1, 2, 1],
+            [1/11, -4/11, -6/11, 20/11, 12/11],
+        ]
+
+    def parse_integration_method(self, method_str):
+        if method_str == "simple":
+            return 1
+        elif method_str == "verlet":
+            return 2
+        elif method_str == "adams-bashforth":
+            return 3
+
+
+    def integrate(self, delta_t):
+        '''
+        NOTE: This is run after accel has been updated. 
+        '''
+        if self.integration_method == 1:
+            self.vel = self.vel + self.accel * delta_t
+            self.pos = self.pos + self.vel * delta_t
+        elif self.integration_method == 2:
+            # Verlet integration
+            self.save_last()
+            if delta_t >= 0.005:
+                self.vel = self.vel + self.accel * delta_t
+                self.pos = self.pos + self.vel * delta_t
+            else:
+                self.vel = self.vel + self.accel * delta_t
+                self.verlet_integration(delta_t)
+        elif self.integration_method == 3: # Adams-Bashforth
+            self.save_last()
+            if delta_t >= 0.005:
+                self.vel = self.vel + self.accel * delta_t
+                self.pos = self.pos + self.vel * delta_t
+            else:
+                self.adams_bashforth_integration(delta_t)
+
+    def print(self):
+        print(f"Pos: {self.pos}, Vel: {self.vel}, Accel: {self.accel}")
+
+    def adams_bashforth_integration(self, delta_t):
+        '''
+        Testing.
+        2 points seems to damp the system. 3 does better at maintaining. Above 3 it is worse
+        '''
+        num_prev_accel = len(self.previous_accel)
+        match num_prev_accel:
+            case 0 | 1:
+                self.vel = self.vel + self.accel * delta_t
+            case 2:
+                self.vel = self.vel + delta_t * (1.5 * self.previous_accel[-1] - 0.5 * self.previous_accel[-2])
+            case 3:
+                self.vel = self.vel + delta_t * (23/12 * self.previous_accel[-1] - 4/3 * self.previous_accel[-2] + 5/12 * self.previous_accel[-3])
+            case 4:
+                self.vel = self.vel + delta_t * (55/24 * self.previous_accel[-1] - 59/24 * self.previous_accel[-2] + 37/24 * self.previous_accel[-3] - 9/24 * self.previous_accel[-4])
+            case 5:
+                self.vel = self.vel + delta_t * ((1901 * self.previous_accel[-1] - 2774 * self.previous_accel[-2] + 2616 * self.previous_accel[-3] - 1274 * self.previous_accel[-4] + 251 * self.previous_accel[-5]) / 720)
+            case _:
+                self.vel = self.vel + self.accel * delta_t
+        self.pos = self.pos + self.vel * delta_t
+
+    def verlet_integration(self, delta_t):
+        num_prev_positions = len(self.previous_positions)
+        match num_prev_positions:
+            case 0 | 1:
+                self.pos = self.pos + self.vel * delta_t
+            case 2:
+                new_pos = np.array([0.0, 0.0, 0.0])
+                for i in range(2):
+                    new_pos += self.verlet_constants[2][i] * self.previous_positions[i]
+                new_pos += self.verlet_constants[2][-1] * delta_t * self.accel * delta_t
+                self.pos = new_pos
+            case 3:
+                new_pos = np.array([0.0, 0.0, 0.0])
+                for i in range(3):
+                    new_pos += self.verlet_constants[3][i] * self.previous_positions[i]
+                new_pos += self.verlet_constants[3][-1] * delta_t * self.accel * delta_t
+                self.pos = new_pos
+            case 4:
+                new_pos = np.array([0.0, 0.0, 0.0])
+                for i in range(4):
+                    new_pos += self.verlet_constants[4][i] * self.previous_positions[i]
+                new_pos += self.verlet_constants[4][-1] * delta_t * self.accel * delta_t
+                self.pos = new_pos
+            case _:
+                self.pos = self.pos + self.vel * delta_t
+
+    def save_last(self):
+        self.previous_positions.append(np.copy(self.pos))
+        self.previous_positions = self.previous_positions[-1*self.max_saved:]
+        self.previous_vel.append(np.copy(self.vel))
+        self.previous_vel = self.previous_vel[-1*self.max_saved:]
+        self.previous_accel.append(np.copy(self.accel))
+        self.previous_accel = self.previous_accel[-1*self.max_saved:]
+
+    def save_last_statespace(self):
+        if self.last_statespace is None:
+            self.last_statespace = StateSpace()
+        self.last_statespace.pos = np.copy(self.pos)
+        self.last_statespace.vel = np.copy(self.vel)
+        self.last_statespace.accel = np.copy(self.accel)
+        
+    def set_pos(self, new_pos):
+        self.pos = new_pos
+    def set_vel(self, new_vel):
+        self.vel = new_vel
+    def set_accel(self, new_accel):
+        self.accel = new_accel
+    def set_force(self, new_force):
+        self.force = new_force
+    
+    def get_pos(self):
+        return self.pos
+    def get_vel(self):
+        return self.vel
+    def get_accel(self):
+        return self.accel
+    def get_force(self):
+        return self.force
+
+class Joint():
+    def __init__(self, pos=np.copy(ZERO_VECTOR), mass: float=0.1, fixed: bool=False, name: str="Joint", integration_method:str="simple"):
+        self.name = name
+        self.statespace = StateSpace(pos, ZERO_VECTOR, ZERO_VECTOR, ZERO_VECTOR, integration_method=integration_method)
+
+        self.connected_springs: List[Spring] = []
+        self.constant_external_forces: List[List[float, float, float]] = []
+        
+        self.connected_actuators: List[RigidActuator] = []
+
+        self.variable_external_forces = [[], [], []]
+
+        self.fixed = fixed
+        self.connected_to_rigid_actuator = False
+        self.mass = mass
+
+        self.parent_joints: List[Joint] = []
+        self.child_joints: List[Joint] = []
+
+        self.damping = False
+
+        self.sharing_attributes = False
+        self.shared_attributes = None
+
+    def is_same(self, other_joint: 'Joint'):
+        if self.name == other_joint.get_name():
+            return True
+        return False
+
+    def get_statespace(self):
+        return self.statespace
+
+    def get_pos(self):
+        return self.statespace.get_pos()
+    
+    def get_vel(self):
+        return self.statespace.get_vel()
+    
+    def get_accel(self):
+        return self.statespace.get_accel()
+
+    def set_state_space_pos(self, new_pos):
+        self.statespace.set_pos(new_pos)
+
+    def set_state_space_vel(self, new_vel):
+        self.statespace.set_vel(new_vel)
+    
+    def set_connected_to_rigid_actuator(self, state):
+        self.connected_to_rigid_actuator = state
+
+    def get_connected_springs(self):
+        return self.connected_springs
+    
+    def get_connected_actuators(self):
+        return self.connected_actuators
+    
+    def get_name(self):
+        return self.name
+
+    def add_spring(self, new_spring):
+        self.connected_springs.append(new_spring)
+
+    def add_actuator(self, new_actuator):
+        self.connected_actuators.append(new_actuator)
+    
+    def add_constant_force(self, new_force):
+        self.constant_external_forces.append(new_force)
+
+    def integrate_statespace(self, delta_t):
+        self.statespace.integrate(delta_t)
+
+    def add_gravity(self):
+        gravity_force = np.array([
+            0,
+            -9.81 * self.mass,
+            0
+        ])
+        self.add_constant_force(gravity_force)
+
+    def add_variable_force(self, force_vect: list):
+        if force_vect[0] is not None:
+            self.variable_external_forces[0].append(force_vect[0])
+        if force_vect[1] is not None:
+            self.variable_external_forces[1].append(force_vect[1])
+        if force_vect[2] is not None:
+            self.variable_external_forces[2].append(force_vect[2])
+
+    def calc_net_spring_force(self):
+        net_spring_force = 0
+        for spring in self.get_connected_springs():
+            if self.is_same(spring.get_parent_joint()):
+                spring_force = spring.get_spring_force() # This is the force the spring exerts on the joint ASSUMING self is parent_joint of the spring
+            else: 
+                spring_force = -1*spring.get_spring_force()
+            net_spring_force += spring_force
+        return net_spring_force
+
+    def calc_net_constant_external_force(self):
+        net_external_force = np.copy(ZERO_VECTOR)
+        for force in self.constant_external_forces:
+            net_external_force += force    
+        return net_external_force
+    
+    def calc_net_variable_external_force(self, t):
+        net_variable_external_force = np.array([0.0, 0.0, 0.0])
+        for i in range(3):
+            for func in self.variable_external_forces[i]:
+                force = func(t)
+                net_variable_external_force[i] += force
+        return net_variable_external_force
+    
+    def calc_net_force(self, t):
+        net_force = np.copy(ZERO_VECTOR)
+        if len(self.constant_external_forces) != 0:
+            net_force += self.calc_net_constant_external_force()
+        if len(self.variable_external_forces) != 0:
+            net_force += self.calc_net_variable_external_force(t)
+        
+        net_spring_force = self.calc_net_spring_force()
+        
+        if self.damping == True:
+            net_force += self.calc_damping(net_spring_force)
+
+        net_force += net_spring_force
+
+        self.statespace.set_force(net_force)
+        return self.statespace.get_force()
+    
+    def add_damping(self, vel_ratio=0.25, mom_ratio=None):
+        self.damping = True
+        if mom_ratio is not None:
+            self.damping_vel_ratio = self.mass * mom_ratio
+        else:
+            self.damping_vel_ratio = vel_ratio
+
+    def calc_damping(self, spring_force):
+        vel_damping = self.damping_vel_ratio * (self.statespace.get_vel()) * -1
+
+        return vel_damping
+
+    def calc_accel(self):
+        if self.fixed == False and self.connected_to_rigid_actuator == False:
+            self.statespace.set_accel(self.statespace.get_force() / self.mass)
+    
+    def is_in_list(self, joints: list['Joint']):
+        for j in joints:
+            if self.is_same(j):
+                return True
+        return False
+
+    def find_parent_joints(self):
+        for spring in self.connected_springs:
+            if self.is_same(spring.get_child_joint()):
+                parent_joint = spring.get_parent_joint()
+                if not parent_joint.is_in_list(self.parent_joints):
+                    self.parent_joints.append(parent_joint)
+
+    def find_child_joints(self):
+        for spring in self.connected_springs:
+            if self.is_same(spring.get_parent_joint()):
+                child_joint = spring.get_child_joint()
+                if not child_joint.is_in_list(self.child_joints):
+                    self.child_joints.append(child_joint)
+
+    def print_attributes(self):
+        print(f"Joint: {self.name}")
+        print(f"Force: {self.statespace.get_force()}")
+        print(f"Accel: {self.statespace.get_accel()}")
+        print(f"Vel: {self.statespace.get_vel()}")
+        print(f"Pos: {self.statespace.get_pos()}")
+
+    def attach_shared_attributes(self, shared_attributes: 'SharedJoint'):
+        if not isinstance(shared_attributes._getvalue(), SharedJoint):
+            raise TypeError(f"shared_attributes for Joint {self.name} is not a SharedJoint")
+        self.sharing_attributes = True
+        self.shared_attributes = shared_attributes
+
+        self.shared_attributes.set_name(f"{self.name}")
+
+        self.update_shared_attributes()
+
+    def update_shared_attributes(self):
+        if self.shared_attributes is not None:
+            self.shared_attributes.set_statespace(self.statespace)
+
+class Spring():
+    def __init__(self, parent_joint: 'Joint', child_joint: 'Joint', unstretched_length: float=0, constant_stiffness: float = None, stiffness_func: Callable[[float, float], float]=None, name: str="Spring"):
+        self.name = name
+
+        self.parent_joint = parent_joint
+        self.child_joint = child_joint
+        
+        self.unstretched_length = unstretched_length
+        self.stiffness_func = stiffness_func
+        self.constant_stiffness = constant_stiffness
+        self.current_stiffness = 0.0
+
+        self.vector = self.calc_r_vector()
+        self.length = self.calc_length()
+        self.unit_vector: list[float] = self.calc_r_unit_vector()
+        self.spring_force_scalar: float = 0.0
+        self.spring_force: list[float] = self.calc_spring_force()
+        
+
+        self.parent_joint.add_spring(self)
+        self.child_joint.add_spring(self)
+
+        self.sharing_attributes = False
+        self.shared_attributes: 'SharedSpring' = None
+
+    def get_name(self):
+        return self.name
+
+    def get_parent_joint(self):
+        return self.parent_joint
+    
+    def get_child_joint(self):
+        return self.child_joint
+    
+    def get_spring_force(self):
+        return self.spring_force
+    
+    def calc_r_vector(self):
+        return self.child_joint.get_statespace().get_pos() - self.parent_joint.get_statespace().get_pos()
+
+    def calc_length(self):
+        return ((self.vector[0])**2 + (self.vector[1])**2 + (self.vector[2])**2) ** 0.5
+        
+    def calc_r_unit_vector(self):
+        return self.vector / self.length
+    
+    def calc_stiffness(self):
+        if self.stiffness_func is not None:
+            return self.stiffness_func(self.length, self.unstretched_length)
+        else:
+            return self.constant_stiffness
+    
+    def calc_spring_force(self):
+        '''Positive force is in tension. Aka, the spring PULLS on the other object'''
+        self.length = self.calc_length()
+        del_length = self.length - self.unstretched_length
+        self.current_stiffness = self.calc_stiffness()
+        self.spring_force_scalar = del_length * self.current_stiffness
+
+        self.vector = self.calc_r_vector()
+        self.r_unit_vector = self.calc_r_unit_vector()
+        self.spring_force = self.spring_force_scalar * self.r_unit_vector
+        return self.spring_force
+    
+    def print_attributes(self):
+        print(f"Spring: {self.name}")
+        print(f"Length: {self.length}")
+        print(f"Spring Force: {self.spring_force}")
+    
+    def get_pos(self):
+        return self.parent_statespace.get_pos()
+    def get_axis_vector(self):
+        return self.child_statespace.get_pos() - self.parent_statespace.get_pos()
+
+    def attach_shared_attributes(self, shared_attributes: 'SharedSpring'):
+        if not isinstance(shared_attributes._getvalue(), SharedSpring):
+            raise TypeError(f"shared_attributes for Spring {self.name} is not a SharedSpring")
+        self.sharing_attributes = True
+        self.shared_attributes = shared_attributes
+
+        self.update_shared_attributes()
+    
+    def update_shared_attributes(self):
+        if self.shared_attributes is not None:
+            self.shared_attributes.set_child_statespace(self.child_joint.get_statespace())
+            self.shared_attributes.set_parent_statespace(self.parent_joint.get_statespace())
+            self.shared_attributes.set_spring_force(self.spring_force)
+            self.shared_attributes.set_spring_force_scalar(self.spring_force_scalar)
+            self.shared_attributes.set_length(self.length)
+            self.shared_attributes.set_unstretched_length(self.unstretched_length)
+            self.shared_attributes.set_stiffness(self.current_stiffness)
+
+    def is_same(self, other_spring: 'Spring'):
+        if self.name == other_spring.get_name():
+            return True
+        return False
+
+
+    def is_in_list(self, springs: list['Spring']):
+        for j in springs:
+            if self.is_same(j):
+                return True
+        return False
+
+'''
+It connects 2 joints. 
+1 joint is completely fixed. Cannot move. Ever. It will be referred to as "grounded".
+The other joint is contrained to the grounded joint. The actuator controls the vector displacement <x,y,z> from the grounded joint
+'''
+class RigidActuator():
+    def __init__(self, parent_joint: 'Joint', grounded_joint: 'Joint', name: str = "Rigid Actuator", max_length:float=1, min_length:float=0.1, control_code=0):
+        self.name = name
+
+        self.parent_joint = parent_joint
+        self.grounded_joint = grounded_joint # Same as child joint
+
+        self.parent_joint.add_actuator(self)
+        self.parent_joint.set_connected_to_rigid_actuator(True)
+        self.grounded_joint.add_actuator(self)
+        self.grounded_joint.set_connected_to_rigid_actuator(True)
+
+        self.max_length = max_length
+        self.min_length = min_length
+        self.control_code = control_code # 0 for pos, 1 for vel, 2 for accel
+
+        self.calc_r_unit_vector()
+
+        self.shared_attributes: 'SharedRigidActuator' = None
+
+    def get_name(self):
+        return self.name
+
+    def get_parent_joint(self):
+        return self.parent_joint
+    
+    def get_child_joint(self):
+        return self.grounded_joint
+    
+    def is_same(self, other: 'RigidActuator'):
+        if self.name == other.get_name():
+            return True
+        return False
+
+    def is_in_list(self, actuators: list['RigidActuator']):
+        for j in actuators:
+            if self.is_same(j):
+                return True
+        return False
+    
+    def calc_r_vector(self):
+        return self.grounded_joint.get_statespace().get_pos() - self.parent_joint.get_statespace().get_pos()
+    
+    def calc_length(self):
+        return ((self.vector[0])**2 + (self.vector[1])**2 + (self.vector[2])**2) ** 0.5
+        
+    def calc_r_unit_vector(self):
+        self.vector = self.calc_r_vector()
+        self.length = self.calc_length()
+        return self.vector / self.length
+    
+    def get_pos(self):
+        return self.parent_joint.get_statespace().get_pos()
+    
+    def get_axis_vector(self):
+        return self.grounded_joint.get_statespace().get_pos() - self.parent_joint.get_statespace().get_pos()
+    
+    def get_r_unit_vector(self):
+        return self.calc_r_unit_vector()
+
+    def update_shared_attributes(self):
+        # These 2 lines are used to make sure we set the internal properties correctly
+        self.vector = self.calc_r_vector()
+        self.length = self.calc_length()
+        # END
+        if self.shared_attributes is not None:
+            self.shared_attributes.set_child_statespace(self.grounded_joint.get_statespace())
+            self.shared_attributes.set_parent_statespace(self.parent_joint.get_statespace())
+            self.shared_attributes.set_length(self.length)
+
+    def attach_shared_attributes(self, shared_attributes: 'SharedRigidActuator'):
+        if not isinstance(shared_attributes._getvalue(), SharedRigidActuator):
+            raise TypeError(f"shared_attributes for RigidActuator {self.name} is not a SharedRigidActuator")
+        self.sharing_attributes = True
+        self.shared_attributes = shared_attributes
+
+        self.update_shared_attributes()
+
+    def set_parent_pos(self, new_pos):
+        '''Set the position of the parent joint's statestpace'''
+        self.parent_joint.set_state_space_pos(new_pos)
+
+    def update_from_controller(self):
+        if self.control_code == 0: # Controller controls pos
+            self.set_pos_to_controlled_pos()
+        elif self.control_code == 1: # Controller controls vel
+            self.set_vel_to_controlled_vel()
+        elif self.control_code == 2: # Controls controls accel
+            pass
+
+    def set_vel_to_controlled_vel(self):
+        controlled_vel = self.shared_attributes.get_vel()
+        self.parent_joint.set_state_space_vel(controlled_vel)
+
+    def set_pos_to_controlled_pos(self):
+        controlled_pos = self.shared_attributes.get_pos()
+        self.parent_joint.set_state_space_pos(controlled_pos)
+
+
+class SharedPhysicsElement():
+    def __init__(self) -> None:
+        self.name: str = None
+        self.statespace: StateSpace = None
+
+        self.connected_to_plotter: bool = False
+        self.connected_to_visualization: bool = False
+        self.connected_to_sensor: bool = False
+        self.connected_to_controller: bool = False
+    
+    def set_name(self, name:str) -> None:
+        self.name = name
+    def set_statespace(self, statespace:StateSpace) -> None:
+        self.statespace = statespace
+    def set_connected_to_plotter(self, state: bool) -> None:
+        self.connected_to_plotter = state
+    def set_connected_to_visualization(self, state: bool) -> None:
+        self.connected_to_visualization = state
+    def set_connected_to_sensor(self, state: bool) -> None:
+        self.connected_to_sensor = state
+    def set_connected_to_controller(self, state: bool) -> None:
+        self.connected_to_controller = state
+
+    def get_name(self) -> str:
+        return self.name
+    def get_statespace(self) -> StateSpace:
+        return self.statespace
+    def get_pos(self) -> List[float]:
+        return self.statespace.get_pos()
+    def get_vel(self) -> List[float]:
+        return self.statespace.get_vel()
+    def get_accel(self) -> List[float]:
+        return self.statespace.get_accel()
+    def get_force(self) -> List[float]:
+        return self.statespace.get_force()
+    def get_connected_to_plotter(self) -> bool:
+        return self.connected_to_plotter
+    def get_connected_to_visualization(self) -> bool:
+        return self.connected_to_visualization
+    def get_connected_to_sensor(self) -> bool:
+        return self.connected_to_sensor
+    def get_connected_to_controller(self) -> bool:
+        return self.connected_to_controller
+    
+class SharedJoint(SharedPhysicsElement):
+    def _init__(self) -> None:
+        super().__init__()
+
+class SharedSpring(SharedPhysicsElement):
+    '''self.statespace is the statespace of the parent'''
+    def _init__(self) -> None:
+        super().__init__()
+        self.child_statespace: StateSpace = None
+
+        self.unstretched_length: float = None
+        self.length: float = None
+        self.stiffness: float = None
+
+        self.spring_force: float = None
+
+    def set_parent_statespace(self, statespace:StateSpace) -> None:
+        self.statespace = statespace
+    def set_child_statespace(self, statespace:StateSpace) -> None:
+        self.child_statespace = statespace
+    def set_unstretched_length(self, unstretched_length: float) -> None:
+        self.unstretched_length = unstretched_length
+    def set_length(self, length: float) -> None:
+        self.length = length
+    def set_stiffness(self, stiffness: float) -> None:
+        self.stiffness = stiffness
+    def set_spring_force(self, set_spring_force: float) -> None:
+        self.spring_force = set_spring_force
+    def set_spring_force_scalar(self, scalar_force: float) -> None:
+        self.spring_force_scalar = scalar_force
+    
+    def get_parent_statespace(self) -> StateSpace:
+        return self.statespace
+    def get_child_statespace(self) -> StateSpace:
+        return self.child_statespace
+    def get_unstretched_length(self) -> float:
+        return self.unstretched_length
+    def get_length(self) -> float:
+        return self.length
+    def get_stiffness(self) -> float:
+        return self.stiffness
+    def get_spring_force(self) -> list[float]:
+        return self.spring_force
+    def get_spring_force_scalar(self) -> float:
+        return self.spring_force_scalar
+    def get_axis_vector(self) -> List[float]:
+        return self.child_statespace.get_pos() - self.statespace.get_pos()
+
+
+class SharedRigidActuator(SharedPhysicsElement):
+    '''self.statespace is the statespace of the parent'''
+    def _init__(self) -> None:
+        super().__init__()
+        self.child_statespace: StateSpace = None
+
+        self.length: float = None
+
+    def set_parent_statespace(self, statespace:StateSpace) -> None:
+        self.statespace = statespace
+    def set_parent_statespace_pos(self, new):
+        self.statespace.set_pos(new)
+    def set_parent_statespace_vel(self, new):
+        self.statespace.set_vel(new)
+    def set_child_statespace(self, statespace:StateSpace) -> None:
+        self.child_statespace = statespace
+    def set_length(self, length: float) -> None:
+        self.length = length
+    
+    def get_parent_statespace(self) -> StateSpace:
+        return self.statespace
+    def get_child_statespace(self) -> StateSpace:
+        return self.child_statespace
+    def get_length(self) -> float:
+        return self.length
+    def get_axis_vector(self) -> List[float]:
+        return self.child_statespace.get_pos() - self.statespace.get_pos()
+    def get_r_unit_vector(self):
+        return self.calc_r_unit_vector()
+    def calc_r_vector(self):
+        return self.child_statespace.get_pos() - self.statespace.get_pos()
+    def calc_length(self):
+        return ((self.vector[0])**2 + (self.vector[1])**2 + (self.vector[2])**2) ** 0.5  
+    def calc_r_unit_vector(self):
+        self.vector = self.calc_r_vector()
+        self.length = self.calc_length()
+        return self.vector / self.length

code/setup/code/Sensor_Elements.py

@@ -0,0 +1,451 @@
+import numpy as np
+from math import log, copysign
+
+from Physics_Elements import SharedRigidActuator, SharedJoint, SharedSpring
+
+from typing import TYPE_CHECKING
+if TYPE_CHECKING:
+    from Object_Sharing import SharedFloat
+    from Physics_Elements import StateSpace, Spring
+
+
+
+'''
+TODO
+1.
+2. Better noise generation
+    Non-linearity, hysteresis, background noise, AC noise
+'''
+
+class LoadCell():
+    '''Provides net force feedback.'''
+    def __init__(self, sensor_settings: dict={}):
+        self.sensor_settings = sensor_settings
+
+        self.attached_sim_source = None
+
+        self.shared_attributes: SharedLoadCell = None # For plotting if desired
+
+        self.true_force_vector = np.array([0.0, 0.0, 0.0]) # Current real force on the joint
+        self.true_force_scalar: float = 0.0
+        self.last_true_force_scalars: list[float] = []           # Stores the last true force for hysteresis reasons
+        self.true_voltage_scalar:float = 0.0
+
+        self.noisy_force_vector = np.array([0.0, 0.0, 0.0])
+        self.noisy_force_scalar: float = 0.0
+        self.noisy_voltage_scalar: float = 0.0
+
+        self.parse_settings()
+
+        self.calc_noise_parameters()
+
+    def parse_settings(self):
+        self.name = self.sensor_settings["name"]
+        self.scale = self.sensor_settings["mV/V"] / 1000.0
+        self.excitation = self.sensor_settings["excitation"]
+        self.full_scale_force = self.sensor_settings["full_scale_force"]
+        self.output_channel = self.sensor_settings["output_channel"]
+
+        self.noise_settings = self.sensor_settings.get("noise_settings", dict())
+
+        self.full_scale_voltage = self.scale * self.excitation
+
+    def calc_noise_parameters(self):
+        # Calculate static error
+        self.static_error_stdev = self.full_scale_voltage * self.noise_settings.get("static_error_band", 0) / 100
+
+        # Non-linearity
+        self.nonlinearity_exponent = self.calc_nonlinearity_exponent()
+
+        # Hysteresis
+        self.hysteresis = self.noise_settings.get("hysteresis", 0) / 100
+
+        # Repeatability
+        self.repeatability_offset = np.random.normal(0, self.full_scale_voltage * self.noise_settings.get("repeatability", 0) / 100)
+
+        # Thermal applied at a given true voltage
+        self.thermal_error = self.noise_settings.get("thermal_error", 0) / 100 * self.noise_settings.get("temperature_offset", 0)
+
+
+    def calc_nonlinearity_exponent(self):
+        voltage = 10
+        if self.full_scale_voltage != 1:
+            self.full_scale_voltage
+        error = self.noise_settings.get("non-linearity", 0) / 100
+        b = log(1 + error, voltage)
+        return b
+
+    def attach_sim_source(self, sim_source):
+        self.attached_sim_source = sim_source
+
+    def attach_shared_attributes(self, shared_attributes):
+        self.shared_attributes = shared_attributes
+
+    def update_shared_attributes(self):
+        if self.shared_attributes is not None:
+            self.shared_attributes.set_true_force_vector(self.true_force_vector)
+            self.shared_attributes.set_true_force_scalar(self.true_force_scalar)
+            self.shared_attributes.set_true_voltage_scalar(self.true_voltage_scalar)
+
+            self.shared_attributes.set_noisy_voltage_scalar(self.noisy_voltage_scalar)
+
+    def update_internal_attributes(self):
+        self.calc_sensor_force_scalar()
+        self.calc_true_voltage_scalar()
+
+        # Calc noise
+        self.calc_noisy_voltage_scalar()
+
+    def pull_from_sim(self):
+        '''Gets the real net force from the shared attribute updated by the sim'''
+        # Must be overridden
+        pass
+    
+    def calc_sensor_force_scalar(self):
+        self.last_true_force_scalars.append(self.true_force_scalar)
+        self.last_true_force_scalars = self.last_true_force_scalars[-5:]
+        self.true_force_scalar = ((self.true_force_vector[0])**2 + (self.true_force_vector[1])**2 + (self.true_force_vector[2])**2)**0.5
+
+    def get_sensor_force_scalar(self):
+        return self.true_force_scalar
+
+    def get_sensor_force_vector(self):
+        return self.true_force_vector
+    
+    def get_true_voltage_scalar(self) -> float:
+        return self.true_voltage_scalar
+    
+    def get_noisy_voltage_scalar(self) -> float:
+        return self.noisy_voltage_scalar
+    
+    def calc_true_voltage_scalar(self):
+        force_fraction = self.true_force_scalar / self.full_scale_force
+        full_force_voltage = self.excitation * self.scale
+        self.true_voltage_scalar = force_fraction * full_force_voltage
+
+    def calc_noisy_voltage_scalar(self):
+        # Calculate static error
+        static_error = np.random.normal(0, self.static_error_stdev)
+        # Non-linearity
+        if self.true_voltage_scalar > 1E-5:
+            nonlinearity_multiplier = self.true_voltage_scalar**(self.nonlinearity_exponent)
+        else:
+            nonlinearity_multiplier = 1
+        # Hysteresis
+        average_last = np.average(self.last_true_force_scalars)
+        #hysteresis_error = (self.true_force_scalar - average_last) / self.true_force_scalar * self.hysteresis * self.full_scale_voltage
+        hysteresis_error = copysign(1, self.true_force_scalar - average_last) * self.hysteresis * self.full_scale_voltage
+        # Repeatability
+        # self.repeatability_offset
+
+        # Thermal
+        thermal_error = self.thermal_error * self.true_voltage_scalar
+
+        self.noisy_voltage_scalar = self.true_voltage_scalar*nonlinearity_multiplier + static_error + hysteresis_error + self.repeatability_offset + thermal_error
+
+class LoadCellJoint(LoadCell):
+    def __init__(self, sensor_settings: dict={}):
+        super().__init__(sensor_settings=sensor_settings)
+        self.attached_sim_source: SharedJoint = None
+    
+    def attach_sim_source(self, sim_source: 'SharedJoint'):
+        if not isinstance(sim_source._getvalue(), SharedJoint):
+            raise TypeError(f"sim_source for LoadCellJoint {self.name} is not a SharedJoint")
+        self.attached_sim_source = sim_source
+
+    def pull_from_sim(self):
+        self.true_force_vector = self.attached_sim_source.get_force()
+
+class LoadCellSpring(LoadCell):
+    '''Provides feedback on the force along the axis of a spring.'''
+    def __init__(self, sensor_settings: dict={}):
+        super().__init__(sensor_settings=sensor_settings)
+        self.attached_sim_source: SharedSpring = None
+
+        self.unstretched_length: float = 0.0
+        self.true_length: float = 0.0
+
+    def attach_sim_source(self, sim_source: 'SharedSpring'):
+        if not isinstance(sim_source._getvalue(), SharedSpring):
+            raise TypeError(f"sim_source for LoadCellSpring {self.name} is not a SharedSpring")
+        self.attached_sim_source = sim_source
+        self.unstretched_length = sim_source.get_unstretched_length()
+
+    def pull_from_sim(self):
+        self.true_force_vector = self.attached_sim_source.get_spring_force()
+        self.calc_sensor_force_scalar()
+        self.true_length = self.attached_sim_source.get_length()
+
+    def calc_sensor_force_scalar(self):
+        self.last_true_force_scalars.append(self.true_force_scalar)
+        self.last_true_force_scalars = self.last_true_force_scalars[-5:]
+        self.true_force_scalar = ((self.true_force_vector[0])**2 + (self.true_force_vector[1])**2 + (self.true_force_vector[2])**2)**0.5
+
+        if self.true_length >= self.unstretched_length: # Tension is positive
+            self.true_force_scalar *= 1
+        else:
+            self.true_force_scalar*= -1
+
+    def calc_true_voltage_scalar(self):
+        '''Include the possibility to be negative if in tension'''
+        force_fraction = self.true_force_scalar / self.full_scale_force
+        full_force_voltage = self.excitation * self.scale
+        self.true_voltage_scalar = force_fraction * full_force_voltage
+
+
+
+class SharedLoadCell():
+    def __init__(self):
+        self.true_force_vector = np.array([0.0, 0.0, 0.0]) # Current real force on the joint
+        self.true_force_scalar: float = 0.0
+        self.true_voltage_scalar: float = 0.0
+
+        self.noisy_force_vector = np.array([0.0, 0.0, 0.0]) # Current real force on the joint
+        self.noisy_force_scalar: float = 0.0
+        self.noisy_voltage_scalar: float = 0.0
+
+        self.connected_to_plotter: bool = False
+        self.connected_to_visualization: bool = False
+        self.connected_to_sensor: bool = False
+        self.connected_to_controller: bool = False
+
+    def set_true_force_vector(self, true_force_vector):
+        self.true_force_vector = true_force_vector
+    
+    def get_true_force_vector(self) -> float:
+        return self.true_force_vector
+    
+    def set_true_force_scalar(self, true_force_scalar):
+        self.true_force_scalar = true_force_scalar
+    
+    def get_true_force_scalar(self) -> float:
+        return self.true_force_scalar
+    
+    def set_true_voltage_scalar(self, true_voltage_scalar):
+        self.true_voltage_scalar = true_voltage_scalar
+    
+    def get_true_voltage_scalar(self) -> float:
+        return self.true_voltage_scalar
+    
+    def set_noisy_voltage_scalar(self, noisy_voltage_scalar):
+        self.noisy_voltage_scalar = noisy_voltage_scalar
+    
+    def get_noisy_voltage_scalar(self) -> float:
+        return self.noisy_voltage_scalar
+    
+    def set_connected_to_plotter(self, state: bool) -> None:
+        self.connected_to_plotter = state
+    def set_connected_to_visualization(self, state: bool) -> None:
+        self.connected_to_visualization = state
+    def set_connected_to_sensor(self, state: bool) -> None:
+        self.connected_to_sensor = state
+    def set_connected_to_controller(self, state: bool) -> None:
+        self.connected_to_controller = state
+
+    def get_connected_to_plotter(self) -> bool:
+        return self.connected_to_plotter
+    def get_connected_to_visualization(self) -> bool:
+        return self.connected_to_visualization
+    def get_connected_to_sensor(self) -> bool:
+        return self.connected_to_sensor
+    def get_connected_to_controller(self) -> bool:
+        return self.connected_to_controller
+
+class SharedLoadCellJoint(SharedLoadCell):
+    def __init__(self):
+        super().__init__()
+
+class SharedLoadCellSpring(SharedLoadCell):
+    def __init__(self):
+        super().__init__()
+
+
+
+class DisplacementSensor():
+    '''Measures the displacement between 2 joints.
+    The output is the non-negative scalar distance between parent and child'''
+    def __init__(self, parent_joint: 'SharedJoint', child_joint: 'SharedJoint', sensor_settings: dict={}):
+        self.sensor_settings = sensor_settings
+        self.parse_settings()
+        self.calc_noise_parameters()
+
+        if not isinstance(parent_joint._getvalue(), SharedJoint):
+            raise TypeError(f"parent_joint for DisplacementSensor {self.name} is not a SharedJoint")
+        if not isinstance(child_joint._getvalue(), SharedJoint):
+            raise TypeError(f"child_joint for DisplacementSensor {self.name} is not a SharedJoint")
+
+        parent_joint.set_connected_to_sensor(True)
+        child_joint.set_connected_to_sensor(True)
+
+        
+        self.sim_source_parent:SharedJoint = parent_joint
+        self.sim_source_child:SharedJoint = child_joint
+        
+    
+        self.shared_attributes: SharedDisplacementSensor = None # For plotting if desired
+
+        self.length: float = 0.0                                # distance from child to parent
+        self.true_disp_scalar: float = 0.0                      # length - neutral_length
+        self.last_true_disp_scalars: list[float] = []           # Stores the last true displacements for hysteresis reasons
+        self.calc_sensor_true_disp()
+
+        self.true_voltage: float = 0.0
+        self.noisy_voltage: float = 0.0
+
+    def parse_settings(self):
+        self.name = self.sensor_settings["name"]
+        self.scale = self.sensor_settings["volts_per_meter"]
+        self.output_channel = self.sensor_settings["output_channel"]
+        self.neutral_length = self.sensor_settings["neutral_length"]
+        self.neutral_voltage = self.sensor_settings.get("neutral_voltage", 0)
+
+        self.noise_settings = self.sensor_settings.get("noise_settings", dict())
+
+        self.full_scale_voltage = self.scale * (self.sensor_settings["max_length"] - self.neutral_length)
+
+    def calc_noise_parameters(self):
+        # Calculate static error
+        self.static_error_stdev = self.full_scale_voltage * self.noise_settings.get("static_error_band", 0) / 100
+
+        # Non-linearity
+        self.nonlinearity_exponent = self.calc_nonlinearity_exponent()
+
+        # Hysteresis
+        self.hysteresis = self.noise_settings.get("hysteresis", 0) / 100
+
+        # Repeatability
+        self.repeatability_offset = np.random.normal(0, self.full_scale_voltage * self.noise_settings.get("repeatability", 0) / 100)
+
+        # Thermal applied at a given true voltage
+        self.thermal_error = self.noise_settings.get("thermal_error", 0) / 100 * self.noise_settings.get("temperature_offset", 0)
+    
+    def calc_nonlinearity_exponent(self):
+        voltage = 10
+        if self.full_scale_voltage != 1:
+            self.full_scale_voltage
+        error = self.noise_settings.get("non-linearity", 0) / 100
+        b = log(1 + error, voltage)
+        return b
+    
+    def attach_shared_attributes(self, shared_attributes):
+        if not isinstance(shared_attributes._getvalue(), SharedDisplacementSensor):
+            raise TypeError(f"shared_attributes for DisplacementSensor {self.name} is not a SharedDisplacementSensor")
+        self.shared_attributes = shared_attributes
+        self.shared_attributes.set_neutral_length(self.neutral_length)
+
+    def pull_from_sim(self):
+        '''Gets the real displacement from the shared attribute updated by the sim'''
+        parent_pos = self.sim_source_parent.get_pos()
+        child_pos = self.sim_source_child.get_pos()
+        
+        axis_vector = child_pos - parent_pos
+
+        self.length = ((axis_vector[0])**2 + (axis_vector[1])**2 + (axis_vector[2])**2)**0.5
+
+    def calc_sensor_true_disp(self):
+        self.last_true_disp_scalars.append(self.true_disp_scalar)
+        self.last_true_disp_scalars = self.last_true_disp_scalars[-5:]
+        self.true_disp_scalar = self.length - self.neutral_length
+
+    def calc_true_voltage(self):
+        self.true_voltage = self.true_disp_scalar * self.scale
+
+    def calc_noisy_voltage(self):
+        # Calculate static error
+        static_error = np.random.normal(0, self.static_error_stdev)
+        # Non-linearity
+        if self.true_voltage > 1E-5:
+            nonlinearity_multiplier = self.true_voltage**(self.nonlinearity_exponent)
+        else:
+            nonlinearity_multiplier = 1
+        # Hysteresis
+        average_last = np.average(self.last_true_disp_scalars)
+        #hysteresis_error = (self.true_force_scalar - average_last) / self.true_force_scalar * self.hysteresis * self.full_scale_voltage
+        hysteresis_error = copysign(1, self.true_disp_scalar - average_last) * self.hysteresis * self.full_scale_voltage
+        # Repeatability
+        # self.repeatability_offset
+
+        # Thermal
+        thermal_error = self.thermal_error * self.true_voltage
+
+        self.noisy_voltage = self.true_voltage*nonlinearity_multiplier + static_error + hysteresis_error + self.repeatability_offset + thermal_error
+
+    def update_shared_attributes(self):
+        if self.shared_attributes is not None:
+            self.shared_attributes.set_current_length(self.length)
+            self.shared_attributes.set_true_voltage(self.true_voltage)
+            self.shared_attributes.set_noisy_voltage(self.noisy_voltage)
+            self.shared_attributes.set_displacement(self.true_disp_scalar)
+
+    def update_internal_attributes(self):
+        self.calc_sensor_true_disp()
+        self.calc_true_voltage()
+        self.calc_noisy_voltage()
+
+    def get_true_voltage_scalar(self) -> float:
+        return self.true_voltage
+    
+    def get_noisy_voltage_scalar(self) -> float:
+        return self.noisy_voltage
+
+class SharedDisplacementSensor():
+    def __init__(self):
+        self.neutral_length: float = 0.0
+        self.length: float = 0.0
+        self.displacement: float = 0.0
+
+        self.true_voltage: float = 0.0
+        self.noisy_voltage: float = 0.0
+
+        self.connected_to_plotter: bool = False
+        self.connected_to_visualization: bool = False
+        self.connected_to_sensor: bool = False
+        self.connected_to_controller: bool = False
+    
+    def set_neutral_length(self, new:float):
+        self.neutral_length = new
+
+    def set_current_length(self, new:float):
+        self.length = new
+    
+    def set_displacement(self, new:float):
+        self.displacement = new
+
+    def set_true_voltage(self, new:float):
+        self.true_voltage = new
+
+    def set_noisy_voltage(self, new:float):
+        self.noisy_voltage = new
+
+    def set_connected_to_plotter(self, state: bool) -> None:
+        self.connected_to_plotter = state
+    def set_connected_to_visualization(self, state: bool) -> None:
+        self.connected_to_visualization = state
+    def set_connected_to_sensor(self, state: bool) -> None:
+        self.connected_to_sensor = state
+    def set_connected_to_controller(self, state: bool) -> None:
+        self.connected_to_controller = state
+
+    def get_connected_to_plotter(self) -> bool:
+        return self.connected_to_plotter
+    def get_connected_to_visualization(self) -> bool:
+        return self.connected_to_visualization
+    def get_connected_to_sensor(self) -> bool:
+        return self.connected_to_sensor
+    def get_connected_to_controller(self) -> bool:
+        return self.connected_to_controller
+    
+    def get_neutral_length(self) -> float:
+        return self.neutral_length
+    
+    def get_displacement(self) -> float:
+        return self.displacement
+
+    def get_current_length(self) -> float:
+        return self.length
+    
+    def get_true_voltage(self) -> float:
+        return self.true_voltage
+
+    def get_noisy_voltage(self) -> float:
+        return self.noisy_voltage

code/setup/code/Simulation.py

@@ -0,0 +1,236 @@
+import time
+import threading
+import numpy as np
+
+from typing import List
+from Visualization import Visualization
+from Physics_Elements import Joint, Spring, RigidActuator
+
+class BFSSimulation():
+    def __init__(self, parent_joint: Joint, settings:dict):
+        if not isinstance(parent_joint, Joint):
+            raise TypeError(f"parent_joint for BFSSimulation is not a Joint")
+        
+        self.parent_joint = parent_joint
+        self.duration = settings["duration"]
+        self.delta_t = settings["delta_t"]
+        self.plotting_update_period = settings["plotting_update_period"]
+        self.sensor_update_period = settings["sensor_update_period"]
+        self.controller_pull_period = settings["controller_pull_period"]
+
+        self.joints: List[Joint] = []
+        self.springs: List[Spring] = []
+        self.actuators: List[RigidActuator] = []
+        self.rigid_actuators: List[RigidActuator] = []
+
+        self.get_joints_bfs()
+        #self.get_parent_joints_for_every_joint()
+
+        # Plotting or viz elements are updated slower than sensor
+        self.plotting_or_viz_only_shared = []
+        self.sensor_shared = []
+        self.controller_shared = []
+        self.sort_shared_attributes_into_frequencies()
+
+        self.vis:Visualization = None
+
+        self.attached_shared_time:float = None
+
+        self.sim_time:float = 0.0
+
+    def is_in_joints(self, joint: Joint):
+        for j in self.joints:
+            if joint.is_same(j):
+                return True
+        return False
+    
+    def is_in_queue(self, joint: Joint, queue):
+        for j in queue:
+            if joint.is_same(j):
+                return True
+        return False
+
+    def get_joints_bfs(self):
+        queue: List[Joint] = []
+
+        queue.append(self.parent_joint)
+
+        while queue:
+            parent_joint: Joint
+            parent_joint = queue.pop(0)
+            if parent_joint is None:
+                continue
+            if not parent_joint.is_in_list(self.joints):
+                self.joints.append(parent_joint)
+
+            connected_springs = parent_joint.get_connected_springs()
+            
+            for spring in connected_springs:
+                if not spring.is_in_list(self.springs):
+                    self.springs.append(spring)
+                child_joint = spring.get_child_joint()
+                if not self.is_in_joints(child_joint) and not self.is_in_queue(child_joint, queue):
+                    queue.append(child_joint)
+            
+            connected_actuators = parent_joint.get_connected_actuators()
+            actuator: RigidActuator = None
+            for actuator in connected_actuators:
+                if actuator not in self.actuators:
+                    self.rigid_actuators.append(actuator)
+                    if actuator.get_child_joint() not in self.joints and actuator.get_child_joint() not in queue:
+                        queue.append(actuator.get_child_joint())
+
+
+    def get_parent_joints_for_every_joint(self):
+        for joint in self.joints:
+            joint.find_parent_joints()
+
+    def get_child_joints_for_every_joint(self):
+        for joint in self.joints:
+            joint.find_child_joints()
+
+    def update_actuators_from_controller(self):
+        for rigid_actuator in self.rigid_actuators:
+            rigid_actuator.update_from_controller()
+    
+    def pull_actuator_positions_thread(self):
+        while self.spare_stop_event.is_set() == False:
+            self.update_actuators_from_controller()
+            time.sleep(self.controller_pull_period)
+
+    def update_sensor_attributes(self):
+        '''Updates the joints, springs, and actuators that are attached to sensors'''
+        for obj in self.sensor_shared:
+            obj.update_shared_attributes()
+
+    def update_sensor_attributes_thread(self):
+        while self.spare_stop_event.is_set() == False:
+            self.update_sensor_attributes()
+            time.sleep(self.sensor_update_period)
+
+    def update_plotting_or_viz_only_attributes(self):
+        '''Updates the joints, springs, and actuators that are only attached for plotting or visualization'''
+        for obj in self.plotting_or_viz_only_shared:
+            obj.update_shared_attributes()
+
+    def update_plotting_or_viz_only_attributes_thread(self):
+        while self.spare_stop_event.is_set() == False:
+            self.update_plotting_or_viz_only_attributes()
+            time.sleep(self.plotting_update_period)
+
+    def sort_shared_attributes_into_frequencies(self):
+        '''For each shared attribute connected to anything, put them in lists so we know how fast we need to update them.'''
+        for joint in self.joints:
+            shared = joint.shared_attributes
+            if shared is None:
+                continue
+            plot = shared.get_connected_to_plotter()
+            vis = shared.get_connected_to_visualization()
+            sens = shared.get_connected_to_sensor()
+            contr = shared.get_connected_to_controller()
+
+            if sens:
+                self.sensor_shared.append(joint)
+            elif plot or vis:
+                self.plotting_or_viz_only_shared.append(joint)
+            
+            if contr:
+                self.controller_shared.append(joint)
+        
+        for spring in self.springs:
+            shared = spring.shared_attributes
+            if shared is None:
+                continue
+            plot = shared.get_connected_to_plotter()
+            vis = shared.get_connected_to_visualization()
+            sens = shared.get_connected_to_sensor()
+            contr = shared.get_connected_to_controller()
+
+            if sens:
+                self.sensor_shared.append(spring)
+            elif plot or vis:
+                self.plotting_or_viz_only_shared.append(spring)
+            
+            if contr:
+                self.controller_shared.append(spring)
+
+        for actuator in self.rigid_actuators:
+            shared = actuator.shared_attributes
+            if shared is None:
+                continue
+            plot = shared.get_connected_to_plotter()
+            vis = shared.get_connected_to_visualization()
+            sens = shared.get_connected_to_sensor()
+            contr = shared.get_connected_to_controller()
+
+            if sens:
+                self.sensor_shared.append(actuator)
+            elif plot or vis:
+                self.plotting_or_viz_only_shared.append(actuator)
+            
+            if contr:
+                self.controller_shared.append(actuator)
+
+    def attach_shared_time(self, attached_shared_time):
+        self.attached_shared_time = attached_shared_time
+
+    def run_process(self):
+
+        time_values = []
+
+        self.sim_time = 0
+        #time_last = time.perf_counter()
+        time_last = time.time()
+        count = 0
+
+        self.spare_stop_event = threading.Event()
+        self.spare_stop_event.clear()
+        pull_actuator_thread = threading.Thread(target=self.pull_actuator_positions_thread)
+        pull_actuator_thread.start()
+        update_plotting_or_viz_only_thread = threading.Thread(target=self.update_plotting_or_viz_only_attributes_thread)
+        update_plotting_or_viz_only_thread.start()
+        update_sensor_thread = threading.Thread(target=self.update_sensor_attributes_thread)
+        update_sensor_thread.start()
+
+        while self.sim_time < self.duration:
+            count += 1
+            if self.delta_t is None:
+                #new_time = time.perf_counter()
+                new_time = time.time()
+                delta_time = new_time - time_last
+                time_last = new_time
+            
+            for spring in self.springs:
+                spring.calc_spring_force()
+            
+            joint: Joint
+            for joint in self.joints:
+                if joint.fixed == True:
+                    continue
+                # Get joint forces
+                joint.calc_net_force(self.sim_time)
+                # Get joint accels.
+                joint.calc_accel()
+                # Integrate joint vel and pos
+                if self.delta_t is None:
+                    joint.integrate_statespace(delta_time)
+                else:
+                    joint.integrate_statespace(self.delta_t)
+
+            
+            time_values.append(self.sim_time)
+
+            if self.delta_t is None:
+                self.sim_time += delta_time
+            else:
+                self.sim_time += self.delta_t
+
+            # Update the shared sim time as fast as possible
+            self.attached_shared_time.set(self.sim_time)
+        
+        self.spare_stop_event.set()
+        pull_actuator_thread.join()
+        update_plotting_or_viz_only_thread.join()
+        update_sensor_thread.join()
+
+        print(f"Average delta_t = {self.sim_time / count}")

code/setup/code/Visualization.py

@@ -0,0 +1,594 @@
+import vpython
+import time
+from copy import deepcopy
+
+import multiprocessing
+import threading
+
+import pyqtgraph as pg
+from pyqtgraph.Qt import QtWidgets, QtCore
+
+
+from Physics_Elements import Joint, Spring, SharedRigidActuator, SharedSpring, SharedJoint
+from Sensor_Elements import SharedLoadCellJoint, SharedLoadCellSpring, SharedDisplacementSensor
+from Controller_Input_Elements import SharedRigidActuatorController
+
+from typing import TYPE_CHECKING
+if TYPE_CHECKING:
+    from Object_Sharing import SharedFloat
+
+
+'''
+TODO
+2. Make this in a separate thread
+'''
+class Visualization():
+    def __init__(self, stop_event, scene_settings:dict):
+        self.stop_event = stop_event
+        self.scene_settings = scene_settings
+
+        self.attached_attributes = []
+        self.shared_attributes_object_settings = []
+
+        self.vpython_objects = []
+
+    def draw_scene(self):
+        vpython.scene = vpython.canvas(width=self.scene_settings["canvas_width"], height=self.scene_settings["canvas_height"])
+        side = self.scene_settings["cube_size"]
+        thk = self.scene_settings["wall_thickness"]
+
+        wall_length = side + 2*thk
+
+        
+
+        wall_bottom = vpython.box(pos=vpython.vector(0, -thk/2, 0), size=vpython.vector(wall_length, thk, wall_length), color=vpython.color.gray(0.9))
+
+        wall_left = vpython.box(pos=vpython.vector(-(side + thk)/2, side/2, 0), size=vpython.vector(thk, side, side), color=vpython.color.gray(0.7))
+        wall_right = vpython.box(pos=vpython.vector((side + thk)/2, side/2, 0), size=vpython.vector(thk, side, side), color=vpython.color.gray(0.7))
+        
+        wall_back = vpython.box(pos=vpython.vector(0, (side)/2, -(side+ thk)/2), size=vpython.vector(wall_length, side, thk), color=vpython.color.gray(0.7))
+
+    def attach_shared_attributes(self, shared_attributes, object_settings):
+        self.attached_attributes.append(shared_attributes)
+        self.shared_attributes_object_settings.append(object_settings)
+
+        shared_attributes.set_connected_to_visualization(True)
+
+    def generate_vpython_objects(self):
+        for i in range(len(self.attached_attributes)):
+            object_settings = self.shared_attributes_object_settings[i]
+            
+            if object_settings["type"] == "sphere":
+                shared_attribute:Joint = self.attached_attributes[i]
+                object_pos = deepcopy(shared_attribute.get_pos())
+                object_pos = vpython.vector(object_pos[0], object_pos[1], object_pos[2])
+
+                object_color = self.return_vpython_color(object_settings["color"])
+
+                self.vpython_objects.append(vpython.sphere(pos=object_pos, radius=object_settings["radius"], color=object_color))
+            elif object_settings["type"] == "helix":
+                shared_attribute:Spring = self.attached_attributes[i]
+                object_pos = deepcopy(shared_attribute.get_pos())
+                object_pos = vpython.vector(object_pos[0], object_pos[1], object_pos[2])
+
+                object_axis = deepcopy(shared_attribute.get_axis_vector())
+                object_axis = vpython.vector(object_axis[0], object_axis[1], object_axis[2])
+
+                object_color = self.return_vpython_color(object_settings["color"])
+
+                self.vpython_objects.append(vpython.helix(pos=object_pos, axis=object_axis, radius=object_settings["radius"], thickness=object_settings["thickness"], color=object_color))
+            elif object_settings["type"] == "cylinder":
+                shared_attribute:Spring = self.attached_attributes[i]
+                object_pos = deepcopy(shared_attribute.get_pos())
+                object_pos = vpython.vector(object_pos[0], object_pos[1], object_pos[2])
+
+                object_axis = deepcopy(shared_attribute.get_axis_vector())
+                object_axis = vpython.vector(object_axis[0], object_axis[1], object_axis[2])
+
+                object_color = self.return_vpython_color(object_settings["color"])
+
+                self.vpython_objects.append(vpython.cylinder(pos=object_pos, axis=object_axis, radius=object_settings["radius"], color=object_color))
+        
+    def return_vpython_color(self, color_str):
+        if color_str == "blue":
+            return vpython.color.blue
+        elif color_str == "black":
+            return vpython.color.black
+        elif color_str == "red":
+            return vpython.color.red
+        elif color_str == "green":
+            return vpython.color.green
+        elif color_str == "white":
+            return vpython.color.white
+
+    def update_scene(self):
+        for i in range(len(self.vpython_objects)):
+            element = self.vpython_objects[i]
+            shared_attributes = self.attached_attributes[i]
+            if isinstance(element, vpython.sphere):
+                updated_pos = shared_attributes.get_pos()
+                element.pos = deepcopy(vpython.vector(*(updated_pos)))
+            elif isinstance(element, vpython.helix):
+                updated_pos = shared_attributes.get_pos()
+                element.pos = deepcopy(vpython.vector(*(updated_pos)))
+                updated_axis = shared_attributes.get_axis_vector()
+                element.axis = deepcopy(vpython.vector(*(updated_axis)))
+            elif isinstance(element, vpython.cylinder):
+                updated_pos = shared_attributes.get_pos()
+                element.pos = deepcopy(vpython.vector(*(updated_pos)))
+                updated_axis = shared_attributes.get_axis_vector()
+                element.axis = deepcopy(vpython.vector(*(updated_axis)))
+
+    def run_process(self):
+        # Draw scene
+        self.draw_scene()
+        
+        # Generate vpython objects from settings
+        self.generate_vpython_objects()
+
+        
+
+        while self.stop_event.is_set() == False:
+            # Update pos and axes of all vpython objects
+            self.update_scene()
+
+
+
+class Plotter():
+    def __init__(self, plot_settings: dict, stop_event: multiprocessing.Event):
+        self.plot_settings = plot_settings
+        self.title = plot_settings["title"]
+        self.window_length = self.plot_settings["window_length"]
+        self.update_interval = self.plot_settings["update_interval"]
+        self.pull_interval = self.plot_settings["pull_interval"]
+        self.stop_event: multiprocessing.Event = stop_event
+
+        self.attached_attributes = []
+        self.shared_attributes_plot_settings = []
+
+        self.pull_data_stop_event: threading.Event = None
+
+        self.shared_x: SharedFloat = None
+
+        self.plot_setup = {}
+
+    def attach_shared_attributes(self, shared_attributes, plot_settings):
+        self.attached_attributes.append(shared_attributes)
+        self.shared_attributes_plot_settings.append(plot_settings)
+
+        shared_attributes.set_connected_to_plotter(True)
+
+    def attach_shared_x(self, shared_x):
+        self.shared_x = shared_x
+    
+    def pull_data_thread(self):
+        while self.pull_data_stop_event.is_set() == False:
+            self.pull_data()
+            time.sleep(self.pull_interval / 1000)
+
+    def pull_data(self):
+        self.raw_xdata.append(self.shared_x.get())
+
+        for i in range(len(self.shared_attributes_plot_settings)):
+            foo = self.attached_attributes[i]._getvalue()
+            for key, value in self.shared_attributes_plot_settings[i].items():
+                if isinstance(foo, SharedLoadCellJoint):
+                    bar: SharedLoadCellJoint = self.attached_attributes[i]
+                    if key == "force":
+                        force_vector = bar.get_true_force_vector()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel: str = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[2])
+                    elif key == "true_voltage":
+                        voltage_scalar = bar.get_true_voltage_scalar()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(voltage_scalar)
+                    elif key == "noisy_voltage":
+                        voltage_scalar = bar.get_noisy_voltage_scalar()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(voltage_scalar)
+                elif isinstance(foo, SharedLoadCellSpring):
+                    bar: SharedLoadCellSpring = self.attached_attributes[i]
+                    if key == "force":
+                        force_vector = bar.get_true_force_vector()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[2])
+                            elif subkey == "scalar":
+                                force_scalar = bar.get_true_force_scalar()
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_scalar)
+                    elif key == "true_voltage":
+                        voltage_scalar = bar.get_true_voltage_scalar()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(voltage_scalar)
+                    elif key == "noisy_voltage":
+                        voltage_scalar = bar.get_noisy_voltage_scalar()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(voltage_scalar)
+                elif isinstance(foo, SharedRigidActuator):
+                    bar: SharedRigidActuator = self.attached_attributes[i]
+                    if key == "pos":
+                        pos_vector = bar.get_pos()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(pos_vector[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(pos_vector[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(pos_vector[2])
+                    if key == "vel":
+                        pos_vector = self.attached_attributes[i].get_vel()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(pos_vector[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(pos_vector[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(pos_vector[2])
+                elif isinstance(foo, SharedRigidActuatorController):
+                    bar: SharedRigidActuatorController = self.attached_attributes[i]
+                    if key == "input_voltage":
+                        input_voltage = bar.get_input_voltage()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(input_voltage)
+                    elif key == "digital_input":
+                        digital_input = bar.get_digital_command()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(digital_input)
+                    elif key == "pos":
+                        controlled_vel = bar.get_controlled_pos()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(controlled_vel[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(controlled_vel[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(controlled_vel[2])
+                    elif key == "vel":
+                        controlled_vel = bar.get_controlled_vel()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(controlled_vel[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(controlled_vel[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(controlled_vel[2])
+                            if subkey == "scalar":
+                                vel_scalar = bar.get_controlled_vel_scalar()
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(vel_scalar)
+                    elif key == "disp":
+                        disp_scalar = bar.get_controlled_length()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(disp_scalar)
+                elif isinstance(foo, SharedDisplacementSensor):
+                    bar: SharedDisplacementSensor = self.attached_attributes[i]
+                    if key == "voltage":
+                        for subkey, settings in value.items():
+                            if subkey == "noisy":
+                                voltage_scalar = bar.get_noisy_voltage()
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(voltage_scalar)
+                            elif subkey == "true":
+                                voltage_scalar = bar.get_true_voltage()
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(voltage_scalar)
+                    if key == "disp":
+                        disp_scalar = bar.get_displacement()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(disp_scalar)
+                    elif key == "length":
+                        length_scalar = bar.get_current_length()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(length_scalar)
+                elif isinstance(foo, SharedSpring):
+                    bar: SharedSpring = self.attached_attributes[i]
+                    if key == "force":
+                        force_vector = bar.get_spring_force()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[2])
+                            elif subkey == "scalar":
+                                force_scalar = bar.get_spring_force_scalar()
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_scalar)
+                    elif key == "length":
+                        length = bar.get_length()
+                        for subkey, settings in value.items():
+                            if subkey == "scalar":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(length)
+                elif isinstance(foo, SharedJoint):
+                    bar: SharedJoint = self.attached_attributes[i]
+                    if key == "accel":
+                        accel_vector = bar.get_accel()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(accel_vector[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(accel_vector[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(accel_vector[2])
+                    elif key == "vel":
+                        vel_vector = bar.get_vel()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(vel_vector[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(vel_vector[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(vel_vector[2])
+                    elif key == "pos":
+                        pos_vector = bar.get_pos()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(pos_vector[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(pos_vector[1])
+                            if subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(pos_vector[2])
+                    elif key == "force":
+                        force_vector = bar.get_spring_force()
+                        for subkey, settings in value.items():
+                            if subkey == "x":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[0])
+                            elif subkey == "y":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[1])
+                            elif subkey == "z":
+                                ylabel = settings["ylabel"]
+                                (subplot, line) = self.raw_data_map[ylabel]
+                                self.raw_ydata[subplot][line].append(force_vector[2])
+
+                            
+    def update_live_window(self):
+        if self.stop_event.is_set():
+            self.timer.stop()
+            self.pull_data_stop_event.set()
+            return
+        
+        final_time = self.raw_xdata[-1]
+        target_time = final_time - self.window_length
+        closest_index = min(range(len(self.raw_xdata)), key=lambda i: abs(self.raw_xdata[i] - target_time))
+        self.window_xdata = self.raw_xdata[closest_index:]
+
+        # Attach the data to the appropriate sublines in the subplots
+        for i in range(self.num_plots):  
+            for j in range(len(self.subplot_keys[i])):
+                window_ydata = self.raw_ydata[i][j][closest_index:]
+                if self.plot_settings["step_or_plot"] == "plot":
+                    if len(self.window_xdata) == len(window_ydata):
+                        self.subplot_curves[i][j].setData(self.window_xdata, window_ydata)
+                    elif len(self.window_xdata) > len(window_ydata):
+                        self.subplot_curves[i][j].setData(self.window_xdata[:-1], window_ydata)
+                elif self.plot_settings["step_or_plot"] == "step":
+                    if len(self.window_xdata) == len(window_ydata):
+                        self.subplot_curves[i][j].setData(self.window_xdata, window_ydata[:-1])
+                    elif len(self.window_xdata) > len(window_ydata):
+                        self.subplot_curves[i][j].setData(self.window_xdata[:-1], window_ydata[:-1])
+
+    def generate_plots(self):
+        # Create the application
+        self.app = QtWidgets.QApplication([])
+
+        # Create a plot window
+        self.win = pg.GraphicsLayoutWidget(show=True, title=self.title)
+        self.win.resize(1000, 600)
+        self.win.setWindowTitle(self.plot_settings["title"])
+
+        self.num_plots = self.plot_settings["num_subplots"]
+
+        self.plots = []
+
+        for i in range(self.num_plots):
+            plot = self.win.addPlot(title=self.plot_settings["subplots"][i]["ylabel"])
+            plot.setLabel('left', self.plot_settings["subplots"][i]["ylabel"])
+            plot.addLegend()
+            self.plots.append(plot)
+
+            # Move us to the next row for the subplot
+            if i < self.num_plots - 1:
+                self.win.nextRow()
+        
+    def generate_curves(self):
+        self.subplot_keys = [[] for _ in range(self.num_plots)] # list for each subplot
+        self.subplot_curves = [[] for _ in range(self.num_plots)] # list for each subplot
+        self.raw_xdata = []
+        self.raw_ydata = [[] for _ in range(self.num_plots)] # list for each subplot
+        self.raw_data_map = {}
+        for setting in self.shared_attributes_plot_settings:
+            '''
+            Each setting looks like {
+                "accel": {
+                    "x": {
+                        "subplot": 1,
+                        "ylabel": "Main Mass x-Accel"
+                    },
+                    "y": {
+                        "subplot": 1,
+                        "ylabel": "Main Mass y-Accel"
+                    }
+                },
+                "vel": {
+                    "x": {
+                        "subplot": 2,
+                        "ylabel": "Main Mass x-Vel"
+                    }
+                },
+                "pos": {
+                    "x": {
+                        "subplot": 3,
+                        "ylabel": "Main Mass x-Vel"
+                    }
+                }
+            }
+            '''
+            for key, value in setting.items():
+                ''' key would be like "accel"
+                    value would be like {
+                    "x": {
+                        "subplot": 1,
+                        "ylabel": "Main Mass x-Accel"
+                    },
+                    "y": {
+                        "subplot": 1,
+                        "ylabel": "Main Mass y-Accel"
+                    }
+                }
+                '''
+                for line, line_settings in value.items():
+                    ''' line would be like "x"
+                    line_settings would be like {
+                        "subplot": 1,
+                        "ylabel": "Main Mass x-Accel",
+                        "color": "w"
+                    }
+                    '''
+                    subplot = line_settings["subplot"]
+                    label = line_settings["ylabel"]
+                    color = line_settings.get("color", "w")  # Default to white if no color is specified
+                    self.subplot_keys[subplot].append(label)
+                    if self.plot_settings["step_or_plot"] == "plot":
+                        self.subplot_curves[subplot].append(self.plots[subplot].plot(name=label, pen=pg.mkPen(color=color)))
+                    elif self.plot_settings["step_or_plot"] == "step":
+                        self.subplot_curves[subplot].append(self.plots[subplot].plot(name=label, stepMode=True, pen=pg.mkPen(color=color)))
+
+                    self.raw_data_map[label] = (subplot, len(self.raw_ydata[subplot]))
+                    self.raw_ydata[subplot].append([])
+
+        self.window_xdata = []
+        self.window_ydata = [[] for _ in range(self.num_plots)] # list for each subplot
+
+    def show_plot(self):
+        pass
+
+    def run_process(self):
+        # Generate the figure and subplots
+        self.generate_plots()
+
+        # Generate the curves
+        self.generate_curves()
+
+        self.pull_data_stop_event = threading.Event()
+        self.pull_data_stop_event.clear()
+        pull_data_thread = threading.Thread(target=self.pull_data_thread)
+        pull_data_thread.start()
+
+        self.timer = QtCore.QTimer()
+        self.timer.timeout.connect(self.update_live_window)
+        self.timer.start(self.update_interval)
+
+        # Start the Qt event loop
+        QtWidgets.QApplication.instance().exec_()
+
+        pull_data_thread.join()
+        

code/setup/code/_example_1d_mass_spring_oscillator.py

@@ -0,0 +1,268 @@
+import numpy as np
+import time
+import multiprocessing
+from multiprocessing.managers import BaseManager
+
+
+from Visualization import Visualization, Plotter
+from Simulation import BFSSimulation
+from Physics_Elements import Joint, Spring, SharedJoint, SharedSpring
+from Object_Sharing import SharedFloat
+
+
+
+
+
+def main():
+    '''Setting up the BaseManager so data can be shared between processes'''
+    base_manager = BaseManager()
+
+    BaseManager.register('SharedFloat', SharedFloat)
+    BaseManager.register('SharedJoint', SharedJoint)
+    BaseManager.register('SharedSpring', SharedSpring)
+
+    base_manager.start()
+
+
+
+
+
+
+    '''Creating instances for plotter and visualizer'''
+    # Setup the plotter and visualization processes
+    stop_event = multiprocessing.Event()
+    stop_event.clear()
+
+    # Create a synchronized time between the processes so the plotter and physics sim are in sync
+    shared_sim_time:SharedFloat = base_manager.SharedFloat()
+    shared_sim_time.set(0.0) # Set the initial time = 0.0 seconds
+
+
+    # Create a real-time plotter for physics plotting. 1 for the mass and the other for the spring
+    mass_plotter = Plotter(
+        stop_event = stop_event,
+        plot_settings = {
+            "title": "Joint Plotter",
+            "xlabel": "Time (s)",
+            "num_subplots": 3,
+            "step_or_plot": "plot",
+            "subplots": [
+                {
+                    "ylabel": "Position (m)"
+                },
+                {
+                    "ylabel": "Velocity (m/s)"
+                },
+                {
+                    "ylabel": "Accel (m/s/s)"
+                }
+            ],
+            "window_length": 100,               # Keep 100 seconds visible
+            "pull_interval": 10,                # Pull data every 10 millisecond
+            "update_interval": 100              # Update the graph every 100 milliseconds
+        }
+    )
+    mass_plotter.attach_shared_x(shared_sim_time)
+
+    spring_plotter = Plotter(
+        stop_event = stop_event,
+        plot_settings = {
+            "title": "Spring Plotter",
+            "xlabel": "Time (s)",
+            "num_subplots": 2,
+            "step_or_plot": "plot",
+            "subplots": [
+                {
+                    "ylabel": "Length (m)"
+                },
+                {
+                    "ylabel": "Force (N)"
+                }
+            ],
+            "window_length": 100,               # Keep 100 seconds visible
+            "pull_interval": 10,                # Pull data every 10 milliseconds
+            "update_interval": 100              # Update the graph every 100 milliseconds
+        }
+    )
+    spring_plotter.attach_shared_x(shared_sim_time)
+    
+
+    # Create a 3D visualization for the entire model
+    visualizer = Visualization(
+        stop_event = stop_event,
+        scene_settings = {
+            "canvas_width": 1600,
+            "canvas_height": 1000,
+            "wall_thickness": 0.1,
+            "cube_size": 20
+        }
+    )
+
+
+
+
+
+
+    '''Setting up physics simulation'''
+    # Create the physics elements
+    main_mass = Joint(
+        pos = np.array([0, 5, 0]),
+        mass = 5,
+        fixed = False,
+        name = "Main mass",
+        integration_method = "adams-bashforth"
+    )
+
+    # Before we create a spring, we need to create the Joint on the wall for the Spring to mount to
+    wall_joint = Joint(
+        pos = np.array([-10, 5, 0]),
+        mass = 1,                           # Does not matter because it is fixed
+        fixed = True,                       # We do not want it to ever move
+        name = "Wall Joint"
+    )
+
+
+    spring = Spring(
+        parent_joint = main_mass,
+        child_joint = wall_joint,
+        unstretched_length = 13,
+        constant_stiffness = 100,
+        name = "Spring"
+    )
+
+
+
+
+
+
+    '''Adding items to be plotted'''
+    # Since we want to plot the physics of the mass and spring, we need shared attributes for them
+    shared_main_mass: SharedJoint = base_manager.SharedJoint()
+    main_mass.attach_shared_attributes(shared_main_mass)
+
+    shared_spring: SharedSpring = base_manager.SharedSpring()
+    spring.attach_shared_attributes(shared_spring)
+
+    # Attach the shared elements to the plotter
+    mass_plotter.attach_shared_attributes(
+        shared_attributes = shared_main_mass,
+        plot_settings = {
+            "pos": {
+                "x": {
+                    "subplot": 0,
+                    "ylabel": "Main Poss x-Pos",
+                    "color": "r"
+                }
+            },
+            "vel": {
+                "x": {
+                    "subplot": 1,
+                    "ylabel": "Main Poss x-Vel",
+                    "color": "g"
+                }
+            },
+            "accel": {
+                "x": {
+                    "subplot": 2,
+                    "ylabel": "Main Poss x-Accel",
+                    "color": "b"
+                }
+            }
+        }
+    )
+
+    spring_plotter.attach_shared_attributes(
+        shared_attributes = shared_spring,
+        plot_settings = {
+            "length": {
+                "scalar": {
+                    "subplot": 0,
+                    "ylabel": "Spring Length",
+                    "color": "r"
+                }
+            },
+            "force": {
+                "x": {
+                    "subplot": 1,
+                    "ylabel": "Spring x-Force",
+                    "color": "g"
+                }
+            }
+        }
+    )
+
+
+
+
+
+
+
+    '''Adding items to be visualized'''
+    # We aready have shared_main_mass and shared_spring from the plotting, so there is no need to make more shared elements
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_main_mass,
+        object_settings = {
+            "type": "sphere",
+            "color": "red",
+            "radius": 0.5,
+        }
+    )
+    
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_spring,
+        object_settings = {
+            "type": "helix",
+            "color": "white",
+            "radius": 0.5,
+            "thickness": 0.1
+        }
+    )
+
+
+
+
+    
+
+    '''Create the physics simulation'''
+    simulation = BFSSimulation(
+        parent_joint = main_mass,           # Because spring is a child of main_mass, it will be reached by BFS
+        settings = {
+            "duration": 1000,               # Run the sim for 1000 seconds
+            "delta_t": None,                # Run in real-time
+            "plotting_update_period": 0.01, # Update the plottable elements every 0.01 seconds
+            "sensor_update_period": 0.01,      # Update the sensor elements every second (because we have none, this doesn't matter)
+            "controller_pull_period": 0.01     # Update the controller elements every seconds (again, we have none)
+        }
+    )
+    simulation.attach_shared_time(shared_sim_time)
+    
+
+
+
+
+    '''Setup the processes and run them'''
+    mass_plotter_process = multiprocessing.Process(target=mass_plotter.run_process)
+    spring_plotter_process = multiprocessing.Process(target=spring_plotter.run_process)
+    visualization_process = multiprocessing.Process(target=visualizer.run_process)
+    simulation_process = multiprocessing.Process(target=simulation.run_process)
+
+    mass_plotter_process.start()
+    spring_plotter_process.start()
+    visualization_process.start()
+
+    time.sleep(5)                           # Give the plotters and visualizer time to startup before the physics sim runs.
+
+    # Join the process
+    simulation_process.start()
+    simulation_process.join()               # This blocks until the simulation finishes
+
+
+    mass_plotter_process.join()
+    spring_plotter_process.join()
+    visualization_process.join()
+
+    # Close the manager
+    base_manager.shutdown()
+
+if __name__ == "__main__":
+    main()

code/setup/code/_example_1d_mass_spring_oscillator_with_actuator.py

@@ -0,0 +1,430 @@
+import numpy as np
+import time
+import multiprocessing
+from multiprocessing.managers import BaseManager
+
+
+from Visualization import Visualization, Plotter
+from Simulation import BFSSimulation
+from Physics_Elements import Joint, Spring, RigidActuator, SharedJoint, SharedSpring, SharedRigidActuator
+
+from HITL_Controller import HITLController
+from Controller_Input_Elements import RigidActuatorController, SharedRigidActuatorController
+from Sensor_Elements import DisplacementSensor, LoadCellSpring, SharedDisplacementSensor, SharedLoadCellSpring
+
+from Object_Sharing import SharedFloat
+
+
+
+
+
+def main():
+    '''Setting up the BaseManager so data can be shared between processes'''
+    base_manager = BaseManager()
+
+    BaseManager.register('SharedFloat', SharedFloat)
+    BaseManager.register('SharedJoint', SharedJoint)
+    BaseManager.register('SharedSpring', SharedSpring)
+
+    BaseManager.register('SharedRigidActuator', SharedRigidActuator)
+
+    BaseManager.register('SharedRigidActuatorController', SharedRigidActuatorController)
+
+    BaseManager.register('SharedDisplacementSensor', SharedDisplacementSensor)
+    BaseManager.register('SharedLoadCellSpring', SharedLoadCellSpring)
+
+    base_manager.start()
+
+
+
+
+
+
+    '''Creating instances for plotter and visualizer'''
+    # Setup the plotter and visualization processes
+    stop_event = multiprocessing.Event()
+    stop_event.clear()
+
+    # Create a synchronized time between the processes so the plotter and physics sim are in sync
+    shared_sim_time:SharedFloat = base_manager.SharedFloat()
+    shared_sim_time.set(0.0) # Set the initial time = 0.0 seconds
+
+
+    # Create a real-time plotter for physics plotting.
+    physics_plotter = Plotter(
+        stop_event = stop_event,
+        plot_settings = {
+            "title": "Physics Plotter",
+            "xlabel": "Time (s)",
+            "num_subplots": 4,
+            "step_or_plot": "plot",
+            "subplots": [
+                {
+                    "ylabel": "Position (m)"
+                },
+                {
+                    "ylabel": "Velocity (m/s)"
+                },
+                {
+                    "ylabel": "Accel (m/s/s)"
+                },
+                {
+                    "ylabel": "Force (N)"
+                },
+            ],
+            "window_length": 10,               # Keep 100 seconds visible
+            "pull_interval": 10,                # Pull data every 10 millisecond
+            "update_interval": 100              # Update the graph every 100 milliseconds
+        }
+    )
+    physics_plotter.attach_shared_x(shared_sim_time)
+
+    # Create a real-time plotter for the controller input plotting.
+    controller_plotter = Plotter(
+        stop_event = stop_event,
+        plot_settings = {
+            "title": "Controller Plotter",
+            "xlabel": "Time (s)",
+            "num_subplots": 3,
+            "step_or_plot": "plot",
+            "subplots": [
+                {
+                    "ylabel": "Controller Input (V)",
+                },
+                {
+                    "ylabel": "Digital Input"
+                },
+                {
+                    "ylabel": "Controlled Pos (m)",
+                },
+            ],
+            "window_length": 10,               # Keep 100 seconds visible
+            "pull_interval": 10,                # Pull data every 10 millisecond
+            "update_interval": 100              # Update the graph every 100 milliseconds
+        }
+    )
+    controller_plotter.attach_shared_x(shared_sim_time)
+
+    # Create a real-time plotter for the sensor output plotting
+    sensor_plotter = Plotter(stop_event=stop_event, plot_settings={
+        "title": "Sensor Plotter",
+        "xlabel": "Time (s)",
+        "num_subplots": 2,
+        "step_or_plot": "plot",
+        "subplots": [
+            {
+                "ylabel": "Actuator Length (m)",
+            },
+            {
+                "ylabel": "Sensor Output (V)",
+            },
+        ],
+        "window_length": 10, # Max data points to keep on the plot
+        "pull_interval": 10, # in milliseconds
+        "update_interval": 100, # In milliseconds.
+        })
+    sensor_plotter.attach_shared_x(shared_sim_time)
+
+    # Create a 3D visualization for the entire model
+    visualizer = Visualization(
+        stop_event = stop_event,
+        scene_settings = {
+            "canvas_width": 1600,
+            "canvas_height": 1000,
+            "wall_thickness": 0.1,
+            "cube_size": 20
+        }
+    )
+
+
+    '''Creating an instance for the HITLController'''
+    hitl_controller = HITLController(
+        stop_event = stop_event,
+        settings = {
+            "pull_from_sim_period": 5,
+            "plotting_update_period": 10
+        }
+    )
+
+
+
+    '''Setting up physics simulation'''
+    # Create the physics elements
+    main_mass = Joint(
+        pos = np.array([0, 5, 0]),
+        mass = 5,
+        fixed = False,
+        name = "Main mass",
+        integration_method = "adams-bashforth"
+    )
+
+    # Since we want to plot the physics of the mass we need shared attributes for it
+    shared_main_mass: SharedJoint = base_manager.SharedJoint()
+    main_mass.attach_shared_attributes(shared_main_mass)
+    physics_plotter.attach_shared_attributes(
+        shared_attributes = shared_main_mass,
+        plot_settings = {
+            "pos": {
+                "x": {
+                    "subplot": 0,
+                    "ylabel": "Main Poss x-Pos",
+                    "color": "r"
+                }
+            },
+            "vel": {
+                "x": {
+                    "subplot": 1,
+                    "ylabel": "Main Poss x-Vel",
+                    "color": "g"
+                }
+            },
+            "accel": {
+                "x": {
+                    "subplot": 2,
+                    "ylabel": "Main Poss x-Accel",
+                    "color": "b"
+                }
+            }
+        }
+    )
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_main_mass,
+        object_settings = {
+            "type": "sphere",
+            "color": "red",
+            "radius": 0.5,
+        }
+    )
+    
+    # Before we create a spring, we need to create the Joint on the wall for the Spring to mount to
+    north_wall_joint = Joint(
+        pos = np.array([10, 5, 0]),
+        mass = 1,                           # Does not matter because it is fixed
+        fixed = True,                       # We do not want it to ever move
+        name = "North Wall Joint"
+    )
+
+    north_spring = Spring(
+        parent_joint = main_mass,
+        child_joint = north_wall_joint,
+        unstretched_length = 13,
+        constant_stiffness = 100,
+        name = "North Spring"
+    )
+    shared_north_spring: SharedSpring = base_manager.SharedSpring()
+    north_spring.attach_shared_attributes(shared_north_spring)
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_north_spring,
+        object_settings = {
+            "type": "helix",
+            "color": "white",
+            "radius": 0.5,
+            "thickness": 0.1
+        }
+    )
+
+    # Setting up the actuator. This requires the "grounded_joint" to fix to. We also want a spring in between the actuator and main_mass
+    actuator_joint = Joint(
+        pos = np.array([-5, 5, 0]),
+        mass = 1,                           # Does not matter because it is fixed to an actuator
+        fixed = False,                      # We do not want it to ever move
+        name = "Actuator Joint"
+    )
+    
+    actuator_spring = Spring(
+        parent_joint = main_mass,
+        child_joint = actuator_joint,
+        unstretched_length = 5,
+        constant_stiffness = 150,
+        name = "Actuator Spring"
+    )
+    shared_actuator_spring: SharedSpring = base_manager.SharedSpring()
+    actuator_spring.attach_shared_attributes(shared_actuator_spring)
+    physics_plotter.attach_shared_attributes(
+        shared_attributes = shared_actuator_spring,
+        plot_settings = {
+            "force": {
+                "scalar": {
+                    "subplot": 3,
+                    "ylabel": "Actuator Spring Force",
+                    "color": "r"
+                }
+            },
+        }
+    )
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_actuator_spring,
+        object_settings = {
+            "type": "helix",
+            "color": "white",
+            "radius": 0.5,
+            "thickness": 0.1
+        }
+    )
+
+    south_wall_joint = Joint(
+        pos = np.array([-10, 5, 0]),
+        mass = 1,                           # Does not matter because it is fixed
+        fixed = True,                       # We do not want it to ever move
+        name = "South Wall Joint"
+    )
+
+    rigid_actuator = RigidActuator(
+        parent_joint = actuator_joint,
+        grounded_joint = south_wall_joint,
+        name = "Rigid Actuator",
+        max_length = 8,
+        min_length = 2,
+        control_code = 0                    # Position Control
+    )
+    shared_rigid_actuator: SharedRigidActuator = base_manager.SharedRigidActuator()
+    rigid_actuator.attach_shared_attributes(shared_rigid_actuator)
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_rigid_actuator,
+        object_settings = {
+            "type": "cylinder",
+            "color": "black",
+            "radius": 0.5,
+        }
+    )
+
+    # We have an actuator, but it currently does nothing without the RigidActuatorController
+        # We will add the Controller and a DisplacementSensor for something in the real-world to control with
+    rigid_actuator_controller = RigidActuatorController(
+        controller_settings = {
+            "name": "Rigid Actuator Controller",
+            "analog_input_channel": 0,
+            "digital_input_channel": 0,
+            "units_per_volt": 2,                    # 2 meters per volt
+            "neutral_length": 5,
+            "neutral_voltage": 2.5,                 # At 2.5V, go to neutral_length of 5
+            "controls_pos/vel/accel": 0,            # Controls position
+            "max_length": 8,
+            "min_length": 2
+        }
+    )
+    rigid_actuator_controller.attach_sim_target(shared_rigid_actuator)
+    hitl_controller.attach_rigid_actuator_controller(rigid_actuator_controller)
+
+    shared_rigid_actuator_controller: SharedRigidActuatorController = base_manager.SharedRigidActuatorController()
+    rigid_actuator_controller.attach_shared_attributes(shared_rigid_actuator_controller)
+    controller_plotter.attach_shared_attributes(
+        shared_attributes = shared_rigid_actuator_controller,
+        plot_settings = {
+            "input_voltage": {
+                "scalar": {
+                    "subplot": 0,
+                    "ylabel": f"Actuator Controller Voltage",
+                    "color": "b"
+                }
+            },
+            "digital_input": {
+                "scalar": {
+                    "subplot": 1,
+                    "ylabel": f"Actuator Controller CTRL Input",
+                    "color": "b"
+                }
+            },
+            "disp": {
+                "scalar": {
+                    "subplot": 2,
+                    "ylabel": f"Actuator Controller Displacement",
+                    "color": "b"
+                }
+            }
+        }
+    )
+
+    # For the displacement sensor, we need 2 shared joints. 1 is the grounded joint of the actuator. the other is the parent
+    shared_south_wall_joint: SharedJoint = base_manager.SharedJoint()
+    south_wall_joint.attach_shared_attributes(shared_south_wall_joint)
+    shared_actuator_joint: SharedJoint = base_manager.SharedJoint()
+    actuator_joint.attach_shared_attributes(shared_actuator_joint)
+    rigid_actuator_displacement_sensor = DisplacementSensor(
+        parent_joint = shared_actuator_joint,
+        child_joint = shared_south_wall_joint,
+        sensor_settings = {
+            "name": f"Actuator Displacement Sensor",
+            "output_channel": 0,
+            "volts_per_meter": 1,
+            "neutral_length": 0,            
+            "neutral_voltage": 0,           # Voltage at neutral length
+            "max_length": 8,
+        }
+    )
+    shared_rigid_actuator_displacement_sensor: SharedDisplacementSensor = base_manager.SharedDisplacementSensor()
+    rigid_actuator_displacement_sensor.attach_shared_attributes(shared_rigid_actuator_displacement_sensor)
+    sensor_plotter.attach_shared_attributes(shared_attributes=shared_rigid_actuator_displacement_sensor,
+        plot_settings = {
+            "length": {
+                    "scalar": {
+                        "subplot": 0,
+                        "ylabel": f"Actuator Displacement Length",
+                        "color": "b"
+                    }
+            },
+            "voltage": {
+                "true": {               # true or noisy because we didn't add noise
+                    "subplot": 1,
+                    "ylabel": f"Actuator Displacement Voltage",
+                    "color": "b"
+                }
+            },
+        }
+    )
+    hitl_controller.attach_displacement_sensor(rigid_actuator_displacement_sensor)
+
+    
+
+    '''Create the physics simulation'''
+    simulation = BFSSimulation(
+        parent_joint = main_mass,           # Because spring is a child of main_mass, it will be reached by BFS
+        settings = {
+            "duration": 1000,               # Run the sim for 1000 seconds
+            "delta_t": None,                # Run in real-time
+            "plotting_update_period": 0.01, # Update the plottable elements every 0.01 seconds
+            "sensor_update_period": 0.01,      # Update the sensor elements every second (because we have none, this doesn't matter)
+            "controller_pull_period": 0.01     # Update the controller elements every seconds (again, we have none)
+        }
+    )
+    simulation.attach_shared_time(shared_sim_time)
+    
+
+
+
+
+    '''Setup the processes and run them'''
+    physics_plotter_process = multiprocessing.Process(target=physics_plotter.run_process)
+    controller_plotter_process = multiprocessing.Process(target=controller_plotter.run_process)
+    sensor_plotter_process = multiprocessing.Process(target=sensor_plotter.run_process)
+    visualization_process = multiprocessing.Process(target=visualizer.run_process)
+    simulation_process = multiprocessing.Process(target=simulation.run_process)
+
+    hitl_controller_process = multiprocessing.Process(target=hitl_controller.run_process)
+    
+    physics_plotter_process.start()
+    controller_plotter_process.start()
+    sensor_plotter_process.start()
+    visualization_process.start()
+
+    hitl_controller_process.start()
+
+    time.sleep(5)                           # Give the plotters and visualizer time to startup before the physics sim runs.
+
+    # Join the process
+    simulation_process.start()
+    simulation_process.join()               # This blocks until the simulation finishes
+
+
+    physics_plotter_process.join()
+    controller_plotter_process.join()
+    sensor_plotter_process.join()
+    visualization_process.join()
+
+    hitl_controller_process.join()
+
+    # Close the manager
+    base_manager.shutdown()
+
+if __name__ == "__main__":
+    main()

code/setup/code/_example_2d_mass_spring_oscillator_with_actuators.py

@@ -0,0 +1,649 @@
+import numpy as np
+import time
+import multiprocessing
+from multiprocessing.managers import BaseManager
+
+
+from Visualization import Visualization, Plotter
+from Simulation import BFSSimulation
+from Physics_Elements import Joint, Spring, RigidActuator, SharedJoint, SharedSpring, SharedRigidActuator
+
+from HITL_Controller import HITLController
+from Controller_Input_Elements import RigidActuatorController, SharedRigidActuatorController
+from Sensor_Elements import DisplacementSensor, LoadCellSpring, SharedDisplacementSensor, SharedLoadCellSpring
+
+from Object_Sharing import SharedFloat
+
+
+
+
+
+def main():
+    '''Setting up the BaseManager so data can be shared between processes'''
+    base_manager = BaseManager()
+
+    BaseManager.register('SharedFloat', SharedFloat)
+    BaseManager.register('SharedJoint', SharedJoint)
+    BaseManager.register('SharedSpring', SharedSpring)
+
+    BaseManager.register('SharedRigidActuator', SharedRigidActuator)
+
+    BaseManager.register('SharedRigidActuatorController', SharedRigidActuatorController)
+
+    BaseManager.register('SharedDisplacementSensor', SharedDisplacementSensor)
+    BaseManager.register('SharedLoadCellSpring', SharedLoadCellSpring)
+
+    base_manager.start()
+
+
+
+
+
+
+    '''Creating instances for plotter and visualizer'''
+    # Setup the plotter and visualization processes
+    stop_event = multiprocessing.Event()
+    stop_event.clear()
+
+    # Create a synchronized time between the processes so the plotter and physics sim are in sync
+    shared_sim_time:SharedFloat = base_manager.SharedFloat()
+    shared_sim_time.set(0.0) # Set the initial time = 0.0 seconds
+
+
+    # Create a real-time plotter for physics plotting.
+    physics_plotter = Plotter(
+        stop_event = stop_event,
+        plot_settings = {
+            "title": "Physics Plotter",
+            "xlabel": "Time (s)",
+            "num_subplots": 4,
+            "step_or_plot": "plot",
+            "subplots": [
+                {
+                    "ylabel": "Position (m)"
+                },
+                {
+                    "ylabel": "Velocity (m/s)"
+                },
+                {
+                    "ylabel": "Accel (m/s/s)"
+                },
+                {
+                    "ylabel": "Force (N)"
+                },
+            ],
+            "window_length": 10,               # Keep 100 seconds visible
+            "pull_interval": 10,                # Pull data every 10 millisecond
+            "update_interval": 100              # Update the graph every 100 milliseconds
+        }
+    )
+    physics_plotter.attach_shared_x(shared_sim_time)
+
+    # Create a real-time plotter for the controller input plotting.
+    controller_plotter = Plotter(
+        stop_event = stop_event,
+        plot_settings = {
+            "title": "Controller Plotter",
+            "xlabel": "Time (s)",
+            "num_subplots": 3,
+            "step_or_plot": "plot",
+            "subplots": [
+                {
+                    "ylabel": "Controller Input (V)",
+                },
+                {
+                    "ylabel": "Digital Input"
+                },
+                {
+                    "ylabel": "Controlled Pos (m)",
+                },
+            ],
+            "window_length": 10,               # Keep 100 seconds visible
+            "pull_interval": 10,                # Pull data every 10 millisecond
+            "update_interval": 100              # Update the graph every 100 milliseconds
+        }
+    )
+    controller_plotter.attach_shared_x(shared_sim_time)
+
+    # Create a real-time plotter for the sensor output plotting
+    sensor_plotter = Plotter(stop_event=stop_event, plot_settings={
+        "title": "Sensor Plotter",
+        "xlabel": "Time (s)",
+        "num_subplots": 3,
+        "step_or_plot": "plot",
+        "subplots": [
+            {
+                "ylabel": "Actuator Length (m)",
+            },
+            {
+                "ylabel": "Load Cell Force (N)",
+            },
+            {
+                "ylabel": "Sensor Output (V)",
+            },
+        ],
+        "window_length": 10, # Max data points to keep on the plot
+        "pull_interval": 10, # in milliseconds
+        "update_interval": 100, # In milliseconds.
+        })
+    sensor_plotter.attach_shared_x(shared_sim_time)
+
+    # Create a 3D visualization for the entire model
+    visualizer = Visualization(
+        stop_event = stop_event,
+        scene_settings = {
+            "canvas_width": 1600,
+            "canvas_height": 1000,
+            "wall_thickness": 0.1,
+            "cube_size": 20
+        }
+    )
+
+
+    '''Creating an instance for the HITLController'''
+    hitl_controller = HITLController(
+        stop_event = stop_event,
+        settings = {
+            "pull_from_sim_period": 5,
+            "plotting_update_period": 10
+        }
+    )
+
+
+
+    '''Setting up physics simulation'''
+    # Create the physics elements
+    main_mass = Joint(
+        pos = np.array([0, 10, 0]),
+        mass = 5,
+        fixed = False,
+        name = "Main mass",
+        integration_method = "adams-bashforth"
+    )
+    main_mass.add_damping(mom_ratio=0.0)
+
+    # Since we want to plot the physics of the mass we need shared attributes for it
+    shared_main_mass: SharedJoint = base_manager.SharedJoint()
+    main_mass.attach_shared_attributes(shared_main_mass)
+    physics_plotter.attach_shared_attributes(
+        shared_attributes = shared_main_mass,
+        plot_settings = {
+            "pos": {
+                "x": {
+                    "subplot": 0,
+                    "ylabel": "Main Poss x-Pos",
+                    "color": "r"
+                }
+            },
+            "vel": {
+                "x": {
+                    "subplot": 1,
+                    "ylabel": "Main Poss x-Vel",
+                    "color": "g"
+                }
+            },
+            "accel": {
+                "x": {
+                    "subplot": 2,
+                    "ylabel": "Main Poss x-Accel",
+                    "color": "b"
+                }
+            }
+        }
+    )
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_main_mass,
+        object_settings = {
+            "type": "sphere",
+            "color": "red",
+            "radius": 0.5,
+        }
+    )
+    
+    # Before we create a spring, we need to create the Joint on the wall for the Spring to mount to
+    north_wall_joint = Joint(
+        pos = np.array([10, 15, 0]),
+        mass = 1,                           # Does not matter because it is fixed
+        fixed = True,                       # We do not want it to ever move
+        name = "North Wall Joint"
+    )
+
+    north_spring = Spring(
+        parent_joint = main_mass,
+        child_joint = north_wall_joint,
+        unstretched_length = 13,
+        constant_stiffness = 100,
+        name = "North Spring"
+    )
+    shared_north_spring: SharedSpring = base_manager.SharedSpring()
+    north_spring.attach_shared_attributes(shared_north_spring)
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_north_spring,
+        object_settings = {
+            "type": "helix",
+            "color": "white",
+            "radius": 0.5,
+            "thickness": 0.1
+        }
+    )
+
+    west_wall_joint = Joint(
+        pos = np.array([0, 15, -10]),
+        mass = 1,                           # Does not matter because it is fixed
+        fixed = True,                       # We do not want it to ever move
+        name = "West Wall Joint"
+    )
+
+    west_spring = Spring(
+        parent_joint = main_mass,
+        child_joint = west_wall_joint,
+        unstretched_length = 9,
+        constant_stiffness = 100,
+        name = "West Spring"
+    )
+    shared_west_spring: SharedSpring = base_manager.SharedSpring()
+    west_spring.attach_shared_attributes(shared_west_spring)
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_west_spring,
+        object_settings = {
+            "type": "helix",
+            "color": "white",
+            "radius": 0.5,
+            "thickness": 0.1
+        }
+    )
+
+    # Setting up the actuator. This requires the "grounded_joint" to fix to. We also want a spring in between the actuator and main_mass
+    actuator_joint = Joint(
+        pos = np.array([-5, 10, 0]),
+        mass = 1,                           # Does not matter because it is fixed to an actuator
+        fixed = False,                      # We do not want it to ever move
+        name = "Actuator Joint"
+    )
+    
+    actuator_spring = Spring(
+        parent_joint = main_mass,
+        child_joint = actuator_joint,
+        unstretched_length = 7,
+        constant_stiffness = 150,
+        name = "Actuator Spring"
+    )
+    shared_actuator_spring: SharedSpring = base_manager.SharedSpring()
+    actuator_spring.attach_shared_attributes(shared_actuator_spring)
+    physics_plotter.attach_shared_attributes(
+        shared_attributes = shared_actuator_spring,
+        plot_settings = {
+            "force": {
+                "scalar": {
+                    "subplot": 3,
+                    "ylabel": "Actuator Spring Force",
+                    "color": "b"
+                }
+            },
+        }
+    )
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_actuator_spring,
+        object_settings = {
+            "type": "helix",
+            "color": "white",
+            "radius": 0.5,
+            "thickness": 0.1
+        }
+    )
+
+    south_wall_joint = Joint(
+        pos = np.array([-10, 10, 0]),
+        mass = 1,                           # Does not matter because it is fixed
+        fixed = True,                       # We do not want it to ever move
+        name = "South Wall Joint"
+    )
+
+    rigid_actuator = RigidActuator(
+        parent_joint = actuator_joint,
+        grounded_joint = south_wall_joint,
+        name = "Rigid Actuator",
+        max_length = 8,
+        min_length = 2,
+        control_code = 0                    # Position Control
+    )
+    shared_rigid_actuator: SharedRigidActuator = base_manager.SharedRigidActuator()
+    rigid_actuator.attach_shared_attributes(shared_rigid_actuator)
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_rigid_actuator,
+        object_settings = {
+            "type": "cylinder",
+            "color": "black",
+            "radius": 0.5,
+        }
+    )
+
+    # We have an actuator, but it currently does nothing without the RigidActuatorController
+        # We will add the Controller and a DisplacementSensor for something in the real-world to control with
+    rigid_actuator_controller = RigidActuatorController(
+        controller_settings = {
+            "name": "Rigid Actuator Controller",
+            "analog_input_channel": 0,
+            "digital_input_channel": 0,
+            "units_per_volt": 2,                    # 2 meters per volt
+            "neutral_length": 5,
+            "neutral_voltage": 2.5,                 # At 2.5V, go to neutral_length of 5
+            "controls_pos/vel/accel": 0,            # Controls position
+            "max_length": 8,
+            "min_length": 2
+        }
+    )
+    rigid_actuator_controller.attach_sim_target(shared_rigid_actuator)
+    hitl_controller.attach_rigid_actuator_controller(rigid_actuator_controller)
+
+    shared_rigid_actuator_controller: SharedRigidActuatorController = base_manager.SharedRigidActuatorController()
+    rigid_actuator_controller.attach_shared_attributes(shared_rigid_actuator_controller)
+    controller_plotter.attach_shared_attributes(
+        shared_attributes = shared_rigid_actuator_controller,
+        plot_settings = {
+            "input_voltage": {
+                "scalar": {
+                    "subplot": 0,
+                    "ylabel": f"Actuator Controller Voltage",
+                    "color": "r"
+                }
+            },
+            "digital_input": {
+                "scalar": {
+                    "subplot": 1,
+                    "ylabel": f"Actuator Controller CTRL Input",
+                    "color": "r"
+                }
+            },
+            "disp": {
+                "scalar": {
+                    "subplot": 2,
+                    "ylabel": f"Actuator Controller Displacement",
+                    "color": "r"
+                }
+            }
+        }
+    )
+
+    # For the displacement sensor, we need 2 shared joints. 1 is the grounded joint of the actuator. the other is the parent
+    shared_south_wall_joint: SharedJoint = base_manager.SharedJoint()
+    south_wall_joint.attach_shared_attributes(shared_south_wall_joint)
+    shared_actuator_joint: SharedJoint = base_manager.SharedJoint()
+    actuator_joint.attach_shared_attributes(shared_actuator_joint)
+    rigid_actuator_displacement_sensor = DisplacementSensor(
+        parent_joint = shared_actuator_joint,
+        child_joint = shared_south_wall_joint,
+        sensor_settings = {
+            "name": f"Actuator Displacement Sensor",
+            "output_channel": 0,
+            "volts_per_meter": 1,
+            "neutral_length": 0,            
+            "neutral_voltage": 0,           # Voltage at neutral length
+            "max_length": 8,
+        }
+    )
+    shared_rigid_actuator_displacement_sensor: SharedDisplacementSensor = base_manager.SharedDisplacementSensor()
+    rigid_actuator_displacement_sensor.attach_shared_attributes(shared_rigid_actuator_displacement_sensor)
+    sensor_plotter.attach_shared_attributes(shared_attributes=shared_rigid_actuator_displacement_sensor,
+        plot_settings = {
+            "length": {
+                    "scalar": {
+                        "subplot": 0,
+                        "ylabel": f"Actuator Displacement Length",
+                        "color": "r"
+                    }
+            },
+            "voltage": {
+                "true": {               # true or noisy because we didn't add noise
+                    "subplot": 2,
+                    "ylabel": f"Actuator Displacement Voltage",
+                    "color": "r"
+                }
+            },
+        }
+    )
+    hitl_controller.attach_displacement_sensor(rigid_actuator_displacement_sensor)
+
+
+    '''Second actuator'''
+
+    bottom_actuator_joint = Joint(
+        pos = np.array([0, 5, 0]),
+        mass = 1,                           # Does not matter because it is fixed to an actuator
+        fixed = False,                      # We do not want it to ever move
+        name = "Bottom Actuator Joint"
+    )
+    
+    bottom_actuator_spring = Spring(
+        parent_joint = main_mass,
+        child_joint = bottom_actuator_joint,
+        unstretched_length = 7.5,
+        constant_stiffness = 1000,
+        name = "Bottom Actuator Spring"
+    )
+    shared_bottom_actuator_spring: SharedSpring = base_manager.SharedSpring()
+    bottom_actuator_spring.attach_shared_attributes(shared_bottom_actuator_spring)
+    physics_plotter.attach_shared_attributes(
+        shared_attributes = shared_bottom_actuator_spring,
+        plot_settings = {
+            "force": {
+                "scalar": {
+                    "subplot": 3,
+                    "ylabel": "Bottom Actuator Spring Force",
+                    "color": "r"
+                }
+            },
+        }
+    )
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_bottom_actuator_spring,
+        object_settings = {
+            "type": "helix",
+            "color": "white",
+            "radius": 0.5,
+            "thickness": 0.1
+        }
+    )
+
+
+    # LoadCell on the bottom spring
+    bottom_spring_loadcell = LoadCellSpring(sensor_settings = {
+        "name": "Bottom Spring LC",
+        "mV/V": 1000,
+        "excitation": 10,
+        "full_scale_force": 5000,      # What is the max force on the load cell
+        "output_channel": 8          # The output channel that the load cell outputs through the MCC3114. Must be between [0, 15]
+        }
+    )
+    bottom_spring_loadcell.attach_sim_source(shared_bottom_actuator_spring)                   # Point the LoadCellSpring to pull physics values from the SharedSpring
+    shared_bottom_spring_loadcell: SharedLoadCellSpring = base_manager.SharedLoadCellSpring()
+    bottom_spring_loadcell.attach_shared_attributes(shared_bottom_spring_loadcell)
+    hitl_controller.attach_load_cell(bottom_spring_loadcell)
+
+    sensor_plotter.attach_shared_attributes(shared_attributes = shared_bottom_spring_loadcell,
+        plot_settings = {
+            "force": {
+            "scalar": {
+                "subplot": 1,     # Must be in range [0, num_subplots-1]
+                "ylabel": "Bottom Spring LC-Force",
+                "color": "g"        # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w"
+            },
+        },
+        "noisy_voltage": {
+            "scalar": {
+                "subplot": 2,     # Must be in range [0, num_subplots-1]
+                "ylabel": "Bottom Spring LC-Voltage",
+                "color": "g"        # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w"
+            }
+        }
+        }    
+    )
+    
+
+
+    bottom_wall_joint = Joint(
+        pos = np.array([0, 0, 0]),
+        mass = 1,                           # Does not matter because it is fixed
+        fixed = True,                       # We do not want it to ever move
+        name = "Bottom Wall Joint"
+    )
+
+    bottom_rigid_actuator = RigidActuator(
+        parent_joint = bottom_actuator_joint,
+        grounded_joint = bottom_wall_joint,
+        name = "Bottom Rigid Actuator",
+        max_length = 10,
+        min_length = 1,
+        control_code = 1                    # Position Control
+    )
+    shared_bottom_rigid_actuator: SharedRigidActuator = base_manager.SharedRigidActuator()
+    bottom_rigid_actuator.attach_shared_attributes(shared_bottom_rigid_actuator)
+    visualizer.attach_shared_attributes(
+        shared_attributes = shared_bottom_rigid_actuator,
+        object_settings = {
+            "type": "cylinder",
+            "color": "black",
+            "radius": 0.5,
+        }
+    )
+
+    # We have an actuator, but it currently does nothing without the RigidActuatorController
+        # We will add the Controller and a DisplacementSensor for something in the real-world to control with
+    bottom_rigid_actuator_controller = RigidActuatorController(
+        controller_settings = {
+            "name": "Bottom Rigid Actuator Controller",
+            "analog_input_channel": 1,
+            "digital_input_channel": 2,
+            "units_per_volt": 2,                    # 2 meters per volt
+            "neutral_length": 1.5,
+            "neutral_voltage": 2.5,                 # At 2.5V, go to neutral_length of 5
+            "controls_pos/vel/accel": 1,            # Controls position
+            "max_length": 10,
+            "min_length": 1
+        }
+    )
+    bottom_rigid_actuator_controller.attach_sim_target(shared_bottom_rigid_actuator)
+    hitl_controller.attach_rigid_actuator_controller(bottom_rigid_actuator_controller)
+
+    shared_bottom_rigid_actuator_controller: SharedRigidActuatorController = base_manager.SharedRigidActuatorController()
+    bottom_rigid_actuator_controller.attach_shared_attributes(shared_bottom_rigid_actuator_controller)
+    controller_plotter.attach_shared_attributes(
+        shared_attributes = shared_bottom_rigid_actuator_controller,
+        plot_settings = {
+            "input_voltage": {
+                "scalar": {
+                    "subplot": 0,
+                    "ylabel": f"Bottom Actuator Controller Voltage",
+                    "color": "b"
+                }
+            },
+            "digital_input": {
+                "scalar": {
+                    "subplot": 1,
+                    "ylabel": f"Bottom Actuator Controller CTRL Input",
+                    "color": "b"
+                }
+            },
+            "disp": {
+                "scalar": {
+                    "subplot": 2,
+                    "ylabel": f"Bottom Actuator Controller Displacement",
+                    "color": "b"
+                }
+            }
+        }
+    )
+
+    # For the displacement sensor, we need 2 shared joints. 1 is the grounded joint of the actuator. the other is the parent
+    shared_bottom_wall_joint: SharedJoint = base_manager.SharedJoint()
+    bottom_wall_joint.attach_shared_attributes(shared_bottom_wall_joint)
+    shared_bottom_actuator_joint: SharedJoint = base_manager.SharedJoint()
+    bottom_actuator_joint.attach_shared_attributes(shared_bottom_actuator_joint)
+    bottom_rigid_actuator_displacement_sensor = DisplacementSensor(
+        parent_joint = shared_bottom_actuator_joint,
+        child_joint = shared_bottom_wall_joint,
+        sensor_settings = {
+            "name": f"Bottom Actuator Displacement Sensor",
+            "output_channel": 1,
+            "volts_per_meter": 1,
+            "neutral_length": 0,            
+            "neutral_voltage": 0,           # Voltage at neutral length
+            "max_length": 8,
+        }
+    )
+    shared_bottom_rigid_actuator_displacement_sensor: SharedDisplacementSensor = base_manager.SharedDisplacementSensor()
+    bottom_rigid_actuator_displacement_sensor.attach_shared_attributes(shared_bottom_rigid_actuator_displacement_sensor)
+    sensor_plotter.attach_shared_attributes(shared_attributes=shared_bottom_rigid_actuator_displacement_sensor,
+        plot_settings = {
+            "length": {
+                    "scalar": {
+                        "subplot": 0,
+                        "ylabel": f"Bottom Actuator Displacement Length",
+                        "color": "b"
+                    }
+            },
+            "voltage": {
+                "true": {               # true or noisy because we didn't add noise
+                    "subplot": 2,
+                    "ylabel": f"Bottom Actuator Displacement Voltage",
+                    "color": "b"
+                }
+            },
+        }
+    )
+    hitl_controller.attach_displacement_sensor(bottom_rigid_actuator_displacement_sensor)
+
+    
+
+    '''Create the physics simulation'''
+    simulation = BFSSimulation(
+        parent_joint = main_mass,           # Because spring is a child of main_mass, it will be reached by BFS
+        settings = {
+            "duration": 1000,               # Run the sim for 1000 seconds
+            "delta_t": None,                # Run in real-time
+            "plotting_update_period": 0.01, # Update the plottable elements every 0.01 seconds
+            "sensor_update_period": 0.01,      # Update the sensor elements every second (because we have none, this doesn't matter)
+            "controller_pull_period": 0.01     # Update the controller elements every seconds (again, we have none)
+        }
+    )
+    simulation.attach_shared_time(shared_sim_time)
+    
+
+
+
+
+    '''Setup the processes and run them'''
+    physics_plotter_process = multiprocessing.Process(target=physics_plotter.run_process)
+    controller_plotter_process = multiprocessing.Process(target=controller_plotter.run_process)
+    sensor_plotter_process = multiprocessing.Process(target=sensor_plotter.run_process)
+    visualization_process = multiprocessing.Process(target=visualizer.run_process)
+    simulation_process = multiprocessing.Process(target=simulation.run_process)
+
+    hitl_controller_process = multiprocessing.Process(target=hitl_controller.run_process)
+    
+    physics_plotter_process.start()
+    controller_plotter_process.start()
+    sensor_plotter_process.start()
+    visualization_process.start()
+
+    hitl_controller_process.start()
+
+    time.sleep(5)                           # Give the plotters and visualizer time to startup before the physics sim runs.
+
+    # Join the process
+    simulation_process.start()
+    simulation_process.join()               # This blocks until the simulation finishes
+
+
+    physics_plotter_process.join()
+    controller_plotter_process.join()
+    sensor_plotter_process.join()
+    visualization_process.join()
+
+    hitl_controller_process.join()
+
+    # Close the manager
+    base_manager.shutdown()
+
+if __name__ == "__main__":
+    main()

code/setup/code/_example_stage_1_test.py

@@ -0,0 +1,514 @@
+import numpy as np
+import multiprocessing
+from multiprocessing.managers import BaseManager
+
+import time
+
+# if TYPE_CHECKING:
+from Visualization import Visualization, Plotter
+from Simulation import BFSSimulation
+from Physics_Elements import Joint, Spring, RigidActuator, SharedRigidActuator, SharedJoint, SharedSpring, SharedRigidActuator
+from Object_Sharing import SharedFloat
+from HITL_Controller import HITLController
+from Sensor_Elements import SharedLoadCellJoint, LoadCellSpring, SharedLoadCellSpring, DisplacementSensor, SharedDisplacementSensor
+from Controller_Input_Elements import RigidActuatorController, SharedRigidActuatorController
+
+
+
+
+steel_elastic_modulus = 200E9 # 200GPa
+gravity = 9.81 # m/s^2
+
+# Stage setup
+stage_mass = 500_000 # Mass in kilograms
+stage_weight = stage_mass * gravity
+max_thrust = 1.5E7 # Max thrust in Newtons. Currently 15_000_000 N
+stage_diameter = 5.5 # meters
+stage_height = 60 # Meters
+wall_thickness = 0.03 # Meters
+    
+# Actuator setup
+rope_area = 5E-4 # Area in m^2. Currently 5 cm^2
+rope_force_at_neutral_actuator = (max_thrust - stage_weight) / 4.0
+
+lateral_offset = 2 # Meters away from the stage wall
+    
+actuator_neutral_length = 2
+actuator_min_length = 0.5
+actuator_max_length = 2.5
+
+actuator_controller_neutral_voltage = 0
+actuator_controller_mps_per_volt = 1 # meters per second per volt for the RMS controller to command the sim
+
+actuator_displacement_sensor_neutral_voltage = 0
+actuator_displacement_sensor_neutral_length = 0
+
+actuator_displacement_sensor_volts_per_meter = 3 # Volts per meter for the actuator displacement sensor to output
+    
+
+rope_volts_per_newton = 1E-6 # 1 volt is 1 million newtons
+
+# Stage calculations for stiffness of rope supporting main mass
+stage_wall_area = np.pi / 4.0 * (stage_diameter**2 - (stage_diameter - wall_thickness*2)**2)
+stage_stiffness = steel_elastic_modulus * stage_wall_area / stage_height # k = EA/l
+stage_unstretched_length = stage_height - stage_weight / stage_stiffness
+
+# Actuator calculations
+rope_stiffness = steel_elastic_modulus * rope_area / stage_height # Estimate due to stageheight != rope length but it is close enough
+rope_unstretched_length = rope_force_at_neutral_actuator / rope_stiffness + 56.45
+actuator_lateral_offset = stage_diameter / 2 + lateral_offset
+
+actuator_angle = np.arctan(stage_height/actuator_lateral_offset)
+actuator_parent_height = actuator_neutral_length * np.sin(actuator_angle)
+actuator_parent_lateral_offset = actuator_neutral_length * np.cos(actuator_angle)
+
+def sinusoid_force1(time):
+    return 100_000*np.sin(0.1*time)
+def sinusoid_force2(time):
+    return 125_000*np.sin(0.2*time+0.3)
+
+def rope_stiffness_func(length, unstretched_length):
+    if length > unstretched_length:
+        return rope_stiffness
+    else:
+        return 0
+
+def stage_setup_1(_max_run_time=100):
+    max_run_time = _max_run_time
+
+
+    # SIM SETUP #############################################
+    manager = BaseManager()
+    BaseManager.register('SharedFloat', SharedFloat)
+    BaseManager.register('SharedJoint', SharedJoint)
+    BaseManager.register('SharedSpring', SharedSpring)
+    BaseManager.register('SharedRigidActuator', SharedRigidActuator)
+
+    BaseManager.register('SharedLoadCellJoint', SharedLoadCellJoint)
+    BaseManager.register('SharedLoadCellSpring', SharedLoadCellSpring)
+
+    BaseManager.register('SharedRigidActuatorController', SharedRigidActuatorController)
+
+    BaseManager.register('SharedDisplacementSensor', SharedDisplacementSensor)
+
+    
+    manager.start()
+
+    stop_event = multiprocessing.Event()
+    stop_event.clear()
+
+    shared_sim_time = manager.SharedFloat()
+    shared_sim_time.set(0.0)
+
+    
+    physics_plotter = Plotter(stop_event=stop_event, plot_settings={
+        "title": "Physics Plotter",
+        "xlabel": "Time (s)",
+        "num_subplots": 5,
+        "step_or_plot": "plot",
+        "subplots": [
+            {
+                "ylabel": "Accel (m/s/s)",
+            },
+            {
+                "ylabel": "Vel (m/s)",
+            },
+            {
+                "ylabel": "Pos (m)",
+            },
+            {
+                "ylabel": "Pos 2 (m)",
+            },
+            {
+                "ylabel": "Force (N)",
+            },
+        ],
+        "window_length": 100,
+        "pull_interval": 10, # in milliseconds
+        "update_interval": 100, # In milliseconds
+        })
+    physics_plotter.attach_shared_x(shared_sim_time)
+
+    sensor_plotter = Plotter(stop_event=stop_event, plot_settings={
+        "title": "Sensor Plotter",
+        "xlabel": "Time (s)",
+        "num_subplots": 4,
+        "step_or_plot": "plot",
+        "subplots": [
+            {
+                "ylabel": "Spring Force (N)",
+            },
+            {
+                "ylabel": "Actuator Length (m)",
+            },
+            {
+                "ylabel": "Actuator Position Output (V)",
+            },
+            {
+                "ylabel": "Load Cell Output (V)",
+            },
+        ],
+        "window_length": 100, # Max data points to keep on the plot
+        "pull_interval": 10, # in milliseconds
+        "update_interval": 100, # In milliseconds.
+        })
+    sensor_plotter.attach_shared_x(shared_sim_time)
+
+    controller_plotter = Plotter(stop_event=stop_event, plot_settings={
+        "title": "Controller Plotter",
+        "xlabel": "Time (s)",
+        "num_subplots": 3,
+        "step_or_plot": "plot",
+        "subplots": [
+            {
+                "ylabel": "Controller Input (V)",
+            },
+            {
+                "ylabel": "Digital Input"
+            },
+            {
+                "ylabel": "Controlled Vel (m/s)",
+            },
+        ],
+        "window_length": 100, # Max data points to keep on the plot
+        "pull_interval": 10, # in milliseconds
+        "update_interval": 100, # In milliseconds.
+        })
+    controller_plotter.attach_shared_x(shared_sim_time)
+
+    visualizer = Visualization(stop_event=stop_event, scene_settings={
+        "canvas_width": 1600,
+        "canvas_height": 1000,
+        "wall_thickness": 0.1,
+        "cube_size": 100
+    })
+
+    hitl_controller = HITLController(stop_event=stop_event, settings={
+        "pull_from_sim_period": 5, # Pull values for the sensors every XXX milliseconds. Shouldn't be faster than the update_period for the sim
+        "plotting_update_period": 10, # Update the shared attributes (for plotting) every YYY milliseconds
+    })
+
+
+    # SIM ELEMENTS SETUP
+    # Changes height to 59.78258707863 rather than 60m to that it is in equilibrium
+    main_mass = Joint(np.array([0, 60, 0]), mass=stage_mass, fixed=False, name="Stage Mass", integration_method="adams-bashforth")
+    main_mass_shared:SharedJoint = manager.SharedJoint()
+    main_mass.attach_shared_attributes(main_mass_shared)
+    main_mass.add_damping(mom_ratio=1.5)
+    main_mass.add_gravity()
+    main_mass.add_variable_force([sinusoid_force1, None, sinusoid_force2])
+    physics_plotter.attach_shared_attributes(main_mass_shared, plot_settings={
+        "accel": {
+            "x": {
+                "subplot": 0,
+                "ylabel": "Main Mass x-Accel",
+                "color": "r"
+            },
+            "y": {
+                "subplot": 0,
+                "ylabel": "Main Mass y-Accel",
+                "color": "g"
+            },
+            "z": {
+                "subplot": 0,
+                "ylabel": "Main Mass z-Accel",
+                "color": "b"
+            },
+        },
+        "vel": {
+            "x": {
+                "subplot": 1,
+                "ylabel": "Main Mass x-Vel",
+                "color": "r"
+            },
+            "y": {
+                "subplot": 1,
+                "ylabel": "Main Mass y-Vel",
+                "color": "g"
+            },
+            "z": {
+                "subplot": 1,
+                "ylabel": "Main Mass z-Vel",
+                "color": "b"
+            }
+        },
+        "pos": {
+            "x": {
+                "subplot": 2,
+                "ylabel": "Main Mass x-Pos",
+                "color": "r"
+            },
+            "y": {
+                "subplot": 3,
+                "ylabel": "Main Mass y-Pos",
+                "color": "g"
+            },
+            "z": {
+                "subplot": 2,
+                "ylabel": "Main Mass z-Pos",
+                "color": "b"
+            }
+        }
+    })
+    visualizer.attach_shared_attributes(main_mass_shared, object_settings={
+        "type": "sphere",
+        "color": "red",
+        "radius": stage_diameter/2.0
+    })
+
+    support_spring_joint = Joint(np.array([0, stage_height*2, 0]), mass=0, fixed=True, name="Support Spring Joint")
+
+    support_spring = Spring(parent_joint=main_mass, child_joint=support_spring_joint, unstretched_length=stage_unstretched_length, constant_stiffness=stage_stiffness, name="Support Spring")
+    support_spring_shared_attributes:SharedSpring = manager.SharedSpring()
+    support_spring.attach_shared_attributes(support_spring_shared_attributes)
+    physics_plotter.attach_shared_attributes(support_spring_shared_attributes, plot_settings={
+        "force": {
+            "y": {
+                "subplot": 4,
+                "ylabel": "Support Spring y-Force",
+                "color": "w"
+            }
+        }
+    })
+
+
+    # Setting up the actuators and ropes
+    for i in range(4):
+        if i == 0:
+            floor_x_pos = actuator_lateral_offset
+            floor_z_pos = 0
+            parent_x_pos = actuator_lateral_offset - actuator_parent_lateral_offset
+            parent_z_pos = 0
+            name = "East Actuator"
+            name2 = "East Spring"
+            color = "r"
+        elif i == 1:
+            floor_x_pos = 0
+            floor_z_pos = actuator_lateral_offset
+            parent_x_pos = 0
+            parent_z_pos = actuator_lateral_offset - actuator_parent_lateral_offset
+            name = "South Actuator"
+            name2 = "South Spring"
+            color = "g"
+        elif i == 2:
+            floor_x_pos = -1*actuator_lateral_offset
+            floor_z_pos = 0
+            parent_x_pos = -1*actuator_lateral_offset + actuator_parent_lateral_offset
+            parent_z_pos = 0
+            name = "West Actuator"
+            name2 = "West Spring"
+            color = "b"
+        elif i == 3:
+            floor_x_pos = 0
+            floor_z_pos = -1*actuator_lateral_offset
+            parent_x_pos = 0
+            parent_z_pos = -1*actuator_lateral_offset + actuator_parent_lateral_offset
+            name = "North Actuator"
+            name2 = "North Spring"
+            color = "w"
+
+        actuator_spring_joint = Joint(np.array([parent_x_pos, actuator_parent_height, parent_z_pos]), mass=0.001, fixed=False, name=f"{name} Spring Joint")
+        actuator_floor_joint = Joint(np.array([floor_x_pos, 0, floor_z_pos]), mass=0.001, fixed=False, name=f"{name} Floor Joint")
+
+        #rope = Spring(parent_joint=main_mass, child_joint=actuator_spring_joint, unstretched_length=rope_unstretched_length, constant_stiffness=rope_stiffness, name=name2)
+        rope = Spring(parent_joint=main_mass, child_joint=actuator_spring_joint, unstretched_length=rope_unstretched_length, stiffness_func=rope_stiffness_func, name=name2)
+        rope_shared:SharedSpring = manager.SharedSpring()
+        rope.attach_shared_attributes(rope_shared)
+        visualizer.attach_shared_attributes(rope_shared, object_settings={
+            "type": "helix",
+            "color": "white",
+            "radius": 0.1,
+            "thickness": 0.1
+        })
+
+        rope_lc = LoadCellSpring(sensor_settings={
+            "name": f"{name2} LC",
+            "mV/V": 1000,
+            "excitation": 10,
+            "full_scale_force": (1/rope_volts_per_newton * 10),
+            "output_channel": i*4 + 3,
+            "noise_settings": {
+                "static_error_band": 0.15,      # % of FSO
+                "non-linearity": 0.15,          # % of FSO
+                "hysteresis": 0.07,             # % of FSO
+                "repeatability": 0.02,          # % of FSO
+                "thermal_error": 0.005,         # % of reading per degree F
+                "temperature_offset": 10        # Degrees F off from calibration temperature 
+            }
+        })
+        rope_lc.attach_sim_source(rope_shared)
+        rope_lc_shared_attributes = manager.SharedLoadCellSpring()
+        rope_lc.attach_shared_attributes(rope_lc_shared_attributes)
+        hitl_controller.attach_load_cell(rope_lc)
+        sensor_plotter.attach_shared_attributes(rope_lc_shared_attributes, plot_settings={
+            "force": {
+                "scalar": {
+                    "subplot": 0,
+                    "ylabel": f"{name2} LC Force",
+                    "color": color
+                }
+            },
+            "noisy_voltage": {
+                "scalar": {
+                    "subplot": 3,
+                    "ylabel": f"{name2} LC Noisy-Voltage",
+                    "color": color
+                }
+            },
+        })  
+
+        actuator = RigidActuator(actuator_spring_joint, actuator_floor_joint, name=name, max_length=actuator_max_length, min_length=actuator_min_length, control_code=1)
+        actuator_shared_attributes:SharedRigidActuator = manager.SharedRigidActuator()
+        actuator.attach_shared_attributes(actuator_shared_attributes)
+        visualizer.attach_shared_attributes(actuator_shared_attributes, object_settings={
+            "type": "cylinder",
+            "color": "black",
+            "radius": 0.3
+        })
+
+        shared_actuator_spring_joint: SharedJoint = manager.SharedJoint()
+        actuator_spring_joint.attach_shared_attributes(shared_actuator_spring_joint)
+
+        shared_actuator_floor_joint: SharedJoint = manager.SharedJoint()
+        actuator_floor_joint.attach_shared_attributes(shared_actuator_floor_joint)
+
+        actuator_disp_sensor = DisplacementSensor(parent_joint=shared_actuator_spring_joint, child_joint=shared_actuator_floor_joint, sensor_settings={
+            "name": f"{name} Sensor",
+            "output_channel": i*4 + 2,
+            "volts_per_meter": actuator_displacement_sensor_volts_per_meter,
+            "neutral_length": actuator_displacement_sensor_neutral_length, # Length at neutral voltage
+            "neutral_voltage": actuator_displacement_sensor_neutral_voltage,
+            "max_length": actuator_max_length,
+            "noise_settings": {
+                "static_error_band": 0.15,      # % of FSO
+                "non-linearity": 0.15,          # % of FSO
+                "hysteresis": 0.07,             # % of FSO
+                "repeatability": 0.02,          # % of FSO
+                "thermal_error": 0.005,         # % of reading per degree F
+                "temperature_offset": 10        # Degrees F off from calibration temperature 
+            }
+        })
+        actuator_disp_sensor_shared:SharedDisplacementSensor = manager.SharedDisplacementSensor()
+        actuator_disp_sensor.attach_shared_attributes(actuator_disp_sensor_shared)
+        sensor_plotter.attach_shared_attributes(actuator_disp_sensor_shared, plot_settings={
+            "length": {
+                    "scalar": {
+                        "subplot": 1,
+                        "ylabel": f"{name} Length",
+                        "color": color
+                    }
+            },
+            "voltage": {
+                "noisy": {
+                    "subplot": 2,
+                    "ylabel": f"{name} Noisy-Voltage",
+                    "color": color
+                }
+            },
+        })
+        hitl_controller.attach_displacement_sensor(actuator_disp_sensor)
+
+        actuator_controller = RigidActuatorController(controller_settings={
+            "name": "Main Actuator Controller",
+            "analog_input_channel": i,
+            "digital_input_channel": i,
+            "units_per_volt": actuator_controller_mps_per_volt, # How far the actuator moves per input volt (m/s or m/s/s if controlling vel or accel)
+            "neutral_length": actuator_neutral_length, # Length at neutral voltage
+            "neutral_voltage": actuator_controller_neutral_voltage, # Voltage to go to neutral length
+            "controls_pos/vel/accel": 1, # 0 = controls pos, 1 = controls vel, 2 = controls accel,
+            "max_length": actuator_max_length,
+            "min_length": actuator_min_length
+        })
+        actuator_controller_shared_attributes = manager.SharedRigidActuatorController() # For plotting
+        actuator_controller.attach_shared_attributes(actuator_controller_shared_attributes)
+
+        actuator_controller.attach_sim_target(actuator_shared_attributes) # So the controller can update sim elements
+
+        hitl_controller.attach_rigid_actuator_controller(actuator_controller)
+        controller_plotter.attach_shared_attributes(actuator_controller_shared_attributes, plot_settings={
+            "input_voltage": {
+                "scalar": {
+                    "subplot": 0,
+                    "ylabel": f"{name} Controller Voltage",
+                    "color": color
+                }
+            },
+            "digital_input": {
+                "scalar": {
+                    "subplot": 1,
+                    "ylabel": f"{name} Controller CTRL Input",
+                    "color": color
+                }
+            },
+            "vel": {
+                "x": {
+                    "subplot": 2,
+                    "ylabel": f"{name} Controlled x-Vel",
+                    "color": color
+                }
+            }
+        })
+
+
+    # PROCESSES #############################################################
+    # Make the simulation process
+    simulation = BFSSimulation(parent_joint=main_mass, settings={
+        "duration": max_run_time,
+        "delta_t": None,
+        "shared_update_period": 0.1, # not used
+        "plotting_update_period": 0.01,
+        "sensor_update_period": 0.01,
+        "controller_pull_period": 0.01
+    })
+    simulation.attach_shared_time(shared_sim_time)
+    simulation_process = multiprocessing.Process(target=simulation.run_process)
+
+    # Start the processes
+    # Start the visualization process
+    visualization_process = multiprocessing.Process(target=visualizer.run_process)
+    visualization_process.start()
+    
+    # Start the physics_plotter process
+    physics_plotter_process = multiprocessing.Process(target=physics_plotter.run_process)
+    physics_plotter_process.start()
+
+    # Start the sensor physics_plotter process
+    sensor_plotter_process = multiprocessing.Process(target=sensor_plotter.run_process)
+    sensor_plotter_process.start()
+
+    # Start the sensor physics_plotter process
+    controller_plotter_process = multiprocessing.Process(target=controller_plotter.run_process)
+    controller_plotter_process.start()
+
+    # Start the HITL interface process
+    hitl_process = multiprocessing.Process(target=hitl_controller.run_process)
+    hitl_process.start()
+
+    time.sleep(5)
+    simulation_process.start()
+
+    # Join the processes
+    simulation_process.join()
+    stop_event.set()
+
+    visualization_process.join()
+    physics_plotter_process.join()
+    sensor_plotter_process.join()
+    controller_plotter_process.join()
+    hitl_process.join()
+
+    # Close the manager
+    manager.shutdown()
+
+
+def main():
+    max_run_time = 10000
+    #multiprocessed_double_mass_spring(100)
+    #single_spring(max_run_time)
+
+    stage_setup_1(max_run_time)
+
+
+if __name__ == "__main__":
+    main()
+

code/setup/code/deprecated/1d_Mass_Spring.py

@@ -0,0 +1,112 @@
+import matplotlib.pyplot as plt
+from copy import deepcopy
+
+class StateSpace():
+    def __init__(self, pos, vel, accel):
+        self.pos = pos
+        self.vel = vel
+        self.accel = accel
+
+    def print(self):
+        print(f"Pos: {self.pos}, Vel: {self.vel}, Accel: {self.accel}")
+
+class Block():
+    def __init__(self):    
+        self.mass = 5
+
+        self.statespace = StateSpace(5,0,0)
+        
+        self.net_force = 0
+
+
+class Spring():
+    def __init__(self):
+        self.mass = 1
+
+        self.pos1 = 0
+        self.pos2 = 10
+        self.vel = 0
+        self.accel = 0
+
+        self.length = self.pos2 - self.pos1
+        self.zero_length = 10
+
+        self.k = 10
+        self.del_l = self.length - self.zero_length
+        self.force = self.del_l * self.k
+    
+    def get_force(self):
+        self.length = self.pos2 - self.pos1
+        self.del_l = self.length - self.zero_length
+        self.force = self.del_l * self.k
+        return self.force
+
+applied_force = 0
+
+block = Block()
+spring = Spring()
+
+
+# Initialize lists to store data for plotting
+t_values = []
+pos_values = []
+vel_values = []
+accel_values = []
+force_net_values = []
+spring_force_values = []
+del_l_values = []
+
+
+del_t = 0.1
+t = 0
+while t < 10:
+    spring.pos1 = block.statespace.pos
+    spring.force = spring.get_force()
+
+    block.net_force = applied_force + spring.force
+    block.statespace.accel =  block.net_force / block.mass
+    
+    block.statespace.vel += block.statespace.accel * del_t
+    block.statespace.pos += block.statespace.vel  * del_t
+    
+    # Store data for plotting
+    t_values.append(t)
+    pos_values.append(block.statespace.pos)
+    vel_values.append(block.statespace.vel)
+    accel_values.append(block.statespace.accel)
+    force_net_values.append(block.net_force)
+    spring_force_values.append(spring.force)
+    del_l_values.append(spring.del_l)
+
+    t += del_t
+
+print(max(pos_values))
+
+# Plot the data
+plt.figure(figsize=(12, 8))
+
+plt.subplot(3, 1, 1)
+plt.plot(t_values, pos_values)
+plt.plot(t_values, del_l_values)
+plt.title('Position vs Time')
+plt.legend(['Position', 'Delta Length'])
+plt.xlabel('Time (s)')
+plt.ylabel('Position')
+
+plt.subplot(3, 1, 2)
+plt.plot(t_values, vel_values)
+plt.title('Velocity vs Time')
+plt.xlabel('Time (s)')
+plt.ylabel('Velocity')
+
+plt.subplot(3, 1, 3)
+plt.plot(t_values, accel_values)
+plt.plot(t_values, force_net_values)
+plt.plot(t_values, spring_force_values)
+plt.legend(['Acceleration', 'Net Force', 'Spring Force'])
+plt.title('Acceleration, Net Force, and Spring Force vs Time')
+plt.xlabel('Time (s)')
+plt.ylabel('Value')
+
+plt.tight_layout()
+plt.show()

code/setup/code/deprecated/Generalized_1d Mass_Spring_Damper_old.py

@@ -0,0 +1,436 @@
+import math
+import numpy as np
+from typing import List
+import matplotlib.pyplot as plt
+
+ZERO_VECTOR = np.array([0.0, 0.0, 0.0])
+
+
+
+class StateSpace():
+    def __init__(self, pos=np.copy(ZERO_VECTOR), vel=np.copy(ZERO_VECTOR), accel=np.copy(ZERO_VECTOR), force=np.copy(ZERO_VECTOR)):
+        self.pos = pos
+        self.vel = vel
+        self.accel = accel
+
+        self.force = force
+
+    def get_pos(self):
+        return self.pos
+    def get_vel(self):
+        return self.vel
+    def get_accel(self):
+        return self.accel
+    def get_force(self):
+        return self.force
+    
+    def set_pos(self, new_pos):
+        self.pos = new_pos
+    def set_vel(self, new_vel):
+        self.vel = new_vel
+    def set_accel(self, new_accel):
+        self.accel = new_accel
+    def set_force(self, new_force):
+        self.force = new_force
+
+    def integrate(self, delta_t):
+        self.vel = self.vel + self.accel * delta_t
+        self.pos = self.pos + self.vel * delta_t
+
+    def print(self):
+        print(f"Pos: {self.pos}, Vel: {self.vel}, Accel: {self.accel}")
+
+
+class Joint():
+    def __init__(self, pos=np.copy(ZERO_VECTOR), fixed: bool=False):
+        self.statespace = StateSpace(pos, ZERO_VECTOR, ZERO_VECTOR, ZERO_VECTOR)
+
+        self.connected_springs: List[Spring] = []
+        self.connected_masses: List[Mass] = []
+        self.connected_rigid_bodies: List[RigidBody] = []
+        self.external_forces: List[List[float, float, float]] = []
+
+        self.fixed = fixed
+
+        self.parent_joints: List[Joint] = []
+        self.child_joints: List[Joint] = []
+    
+    def get_pos(self):
+        return self.statespace.get_pos()
+    def get_vel(self):
+        return self.statespace.get_vel()
+    def get_accel(self):
+        return self.statespace.get_accel()
+    def get_force(self):
+        return self.statespace.get_force()
+    
+    def get_connected_springs(self):
+        return self.connected_springs
+    def get_connected_masses(self):
+        return self.connected_masses
+    def get_connected_rigid_bodies(self):
+        return self.connected_rigid_bodies
+    
+    def is_fixed(self):
+        return self.fixed
+    
+    def add_spring(self, new_spring):
+        self.connected_springs.append(new_spring)
+
+    def add_mass(self, new_mass):
+        self.connected_masses.append(new_mass)
+
+    def add_rigid_body(self, new_rigid_body):
+        self.connected_rigid_bodies.append(new_rigid_body)
+    
+    def add_extenal_force(self, new_force = ZERO_VECTOR):
+        self.external_forces.append(new_force)
+
+    def calc_net_spring_force(self):
+        net_spring_force = 0
+        for spring in self.connected_springs:
+            spring_force = spring.calc_spring_force() # This is the force the spring exerts on the joint ASSUMING self is parent_joint of the spring
+            net_spring_force += spring_force
+        return net_spring_force
+
+    
+    def calc_net_external_force(self):
+        net_external_force = np.copy(ZERO_VECTOR)
+        for force in self.external_forces:
+            net_external_force += force
+        return net_external_force
+    
+    def calc_net_force(self):
+        net_external_force = self.calc_net_external_force()
+        net_spring_force = self.calc_net_spring_force()
+        self.statespace.force = net_external_force + net_spring_force
+        return self.statespace.force
+    
+    def integrate_accel_vel(self, delta_t: float=0.1):
+        self.statespace.integrate(delta_t)
+
+    def set_accel(self, accel):
+        self.statespace.set_accel(accel)
+
+    def find_parent_joints(self):
+        for spring in self.connected_springs:
+            if spring.child_joint == self:
+                if spring.parent_joint not in self.parent_joints:
+                    self.parent_joints.append(spring.parent_joint)
+
+        for mass in self.connected_masses:
+            if mass.child_joint == self:
+                if mass.parent_joint not in self.parent_joints:
+                    self.parent_joints.append(mass.parent_joint)
+
+    def find_child_joints(self):
+        for spring in self.connected_springs:
+            if spring.parent_joint == self:
+                if spring.child_joint not in self.child_joints:
+                    self.child_joints.append(spring.child_joint)
+
+        for mass in self.connected_masses:
+            if mass.parent_joint == self:
+                if mass.child_joint not in self.child_joints:
+                    self.child_joints.append(mass.child_joint)
+
+class Spring():
+    def __init__(self, parent_joint: Joint, child_joint: Joint, zero_length: float=0, stiffness: float=0):
+        self.parent_joint = parent_joint
+        self.child_joint = child_joint
+        
+        self.zero_length = zero_length
+        self.stiffness = stiffness
+
+        self.vector = self.calc_r_vector()
+        self.length = self.calc_length()
+        self.unit_vector = self.calc_r_unit_vector()
+        self.spring_force = self.calc_spring_force()
+
+        self.parent_joint.add_spring(self)
+        self.child_joint.add_spring(self)
+    
+    def calc_r_vector(self):
+        self.vector = self.child_joint.get_pos() - self.parent_joint.get_pos()
+        return self.vector
+
+    def calc_length(self):
+        self.vector = self.calc_r_vector()
+        return np.linalg.norm(self.vector)
+        
+    def calc_r_unit_vector(self):
+        self.vector = self.calc_r_vector()
+        self.length = self.calc_length()
+        self.unit_vector = self.vector / self.length
+        return self.unit_vector
+    
+    def calc_spring_force(self):
+        '''Positive force is in tension. Aka, the spring PULLS on the other object'''
+        self.length = self.calc_length()
+        del_length = self.length - self.zero_length
+        spring_force = del_length * self.stiffness
+
+        self.r_unit_vector = self.calc_r_unit_vector()
+        self.spring_force = spring_force * self.r_unit_vector
+        return self.spring_force
+        
+
+class Mass():
+    '''
+    A mass is a point mass located in the center of 2 joints.
+    It cannot exert a force
+    '''
+    def __init__(self, parent_joint: Joint, child_joint: Joint, mass: float=0):
+        self.parent_joint = parent_joint
+        self.child_joint = child_joint
+
+        self.mass = mass
+
+        self.parent_joint.add_mass(self)
+        self.child_joint.add_mass(self)
+
+
+        self.statespace = StateSpace((child_joint.get_pos() + parent_joint.get_pos()) / 2.0,
+                                     (child_joint.get_vel() + parent_joint.get_vel()) / 2.0,
+                                     (child_joint.get_accel() + parent_joint.get_accel()) / 2.0,
+                                     (child_joint.get_force() + parent_joint.get_force()) / 2.0)
+
+    def set_accel(self, accel):
+        self.statespace.set_accel(accel)
+
+    def integrate_accel_vel(self, delta_t: float=0.1):
+        self.statespace.integrate(delta_t)
+
+
+
+class Simulation():
+    def __init__(self, parent_joint: Joint, duration: float, delta_t: float):
+        self.parent_joint = parent_joint
+        self.duration = duration
+        self.delta_t = delta_t
+
+        self.joints: List[Joint] = []
+        self.masses: List[Mass] = []
+        self.springs: List[Spring] = []
+        self.rigid_bodies: List[RigidBody] = []
+
+    def get_all_nodes_and_edges(self):
+        '''
+        Do a BFS to get all of the joints (nodes) and springs/masses (edges) in the system
+        '''
+        queue: List[Joint] = []
+
+        queue.append(self.parent_joint)
+
+        while queue:
+            node: Joint
+            node = queue.pop(0)
+            if node not in self.joints:
+                self.joints.append(node)
+            
+            connected_springs = node.get_connected_springs()
+            connected_masses = node.get_connected_masses()
+            connected_rigid_bodies = node.get_connected_rigid_bodies()
+            
+            for spring in connected_springs:
+                if spring not in self.springs:
+                    self.springs.append(spring)
+
+                if spring.child_joint not in self.joints and spring.child_joint not in queue:
+                    queue.append(spring.child_joint)
+
+            for mass in connected_masses:
+                if mass not in self.masses:
+                    self.masses.append(mass)
+                if mass.child_joint not in self.joints and mass.child_joint not in queue:
+                    queue.append(mass.child_joint)
+            
+            for rigid_body in connected_rigid_bodies:
+                if rigid_body not in self.rigid_bodies:
+                    self.rigid_bodies.append(rigid_body)
+                if rigid_body.child_joint not in self.joints and rigid_body.child_joint not in queue:
+                    queue.append(rigid_body.child_joint)
+
+    def print_components(self):
+        print("Joints:")
+        count = 0
+        for joint in self.joints:
+            print(f"Accel: {joint.get_accel()}")
+            print(f"Vel: {joint.get_vel()}")
+            print(f"Pos: {joint.get_pos()}")
+            print()
+        
+        print("Masses:")
+        count = 0
+        for mass in self.masses:
+            print(f"Accel: {mass.statespace.get_accel()}")
+            print(f"Vel: {mass.statespace.get_vel()}")
+            print(f"Pos: {mass.statespace.get_pos()}")
+            print()
+        
+        print("Springs:")
+        count = 0
+        for spring in self.springs:
+            print(f"Spring Force: {spring.spring_force}")
+            print(f"Spring Length: {spring.length}")
+            print()
+        
+        print("Rigid Bodies:")
+        count = 0
+        for rigid_body in self.rigid_bodies:
+            print(f"Transfer Force: {rigid_body.force}")
+            print()
+
+    def calc_force_at_every_joint(self):
+        '''
+        At every joint, calculate the net force at each joint (this accounts for springs and external forces).
+        '''
+        for joint in self.joints:
+            joint.calc_net_force()
+
+    def calc_rigid_body_force(self):
+        for body in self.rigid_bodies:
+            body.force = 0
+
+    def calc_accel_at_every_mass(self):
+        '''
+        Using the sum of the forces at the 2 joints on each mass, we calc the accel of the mass
+        '''
+        for mass in self.masses:
+            net_force = mass.parent_joint.get_force() + mass.child_joint.get_force()
+            accel = net_force / mass.mass
+            mass.set_accel(accel)
+
+    def assign_joint_accel(self):
+        '''
+        If the joint is fixed, accel = 0
+        '''
+        net_joint_accel = np.copy(ZERO_VECTOR)
+        for joint in self.joints:
+            if joint.is_fixed() == True:
+                continue
+
+            for mass in joint.get_connected_masses():
+                net_joint_accel += mass.statespace.get_accel()
+            joint.set_accel(net_joint_accel)
+
+    def integrate_timestep(self):
+        self.calc_force_at_every_joint()
+        self.calc_accel_at_every_mass()
+
+        self.assign_joint_accel()
+
+        for joint in self.joints:
+            joint.integrate_accel_vel(self.delta_t)
+
+        for mass in self.masses:
+            mass.integrate_accel_vel(self.delta_t)
+
+
+
+    def run(self):
+        mass_pos_values = []
+        time_values = []
+        t = 0
+        while t < self.duration:
+            self.integrate_timestep()
+            self.print_components()
+            
+            mass_pos_values.append(self.masses[0].statespace.get_pos())
+            time_values.append(t)
+
+            print("*"*100)
+            t += self.delta_t
+
+        plt.close()
+        # Plot the data
+        plt.figure(figsize=(12, 8))
+
+        plt.plot(time_values, mass_pos_values)
+        plt.title('Position vs Time')
+        plt.xlabel('Time (s)')
+        plt.ylabel('Position')
+        plt.show()
+
+
+class BFSSimulation():
+    def __init__(self, parent_joint: Joint, duration: float, delta_t: float):
+        self.parent_joint = parent_joint
+        self.duration = duration
+        self.delta_t = delta_t
+
+        self.joints: List[Joint] = []
+
+        self.get_joints_bfs()
+        self.get_parent_joints_for_every_joint()
+
+
+    def get_joints_bfs(self):
+        queue: List[Joint] = []
+
+        queue.append(self.parent_joint)
+        visited_springs: List[Spring] = []
+        visited_masses: List[Spring] = []
+
+        while queue:
+            node: Joint
+            node = queue.pop(0)
+            if node not in self.joints:
+                self.joints.append(node)
+
+            connected_springs = node.get_connected_springs()
+            connected_masses = node.get_connected_masses()
+            
+            for spring in connected_springs:
+                if spring not in visited_springs:
+                    visited_springs.append(spring)
+
+                if spring.child_joint not in self.joints and spring.child_joint not in queue:
+                    queue.append(spring.child_joint)
+
+            for mass in connected_masses:
+                if mass not in visited_masses:
+                    visited_masses.append(mass)
+                if mass.child_joint not in self.joints and mass.child_joint not in queue:
+                    queue.append(mass.child_joint)
+
+    def get_parent_joints_for_every_joint(self):
+        for joint in self.joints:
+            joint.find_parent_joints()
+
+    def get_child_joints_for_every_joint(self):
+        for joint in self.joints:
+            joint.find_child_joints()
+            
+    def integrate_joints(self):
+        for joint in self.joints:
+            joint.calc_net_force()
+
+def main():
+    joint_left_mass1 = Joint(np.array([0,0,0]), fixed=False)
+    joint_mass1_spring1 = Joint(np.array([0,0,0]), fixed=False)
+
+    joint_spring1_rigidbody1 = Joint(np.array([10,0,0]), fixed=False)
+    joint_rigidbody1_spring2 = Joint(np.array([10,0,0]), fixed=False)
+
+    joint_right_spring2 = Joint(np.array([20,0,0]), fixed=True)
+
+    mass1 = Mass(joint_left_mass1, joint_mass1_spring1, 5)
+    spring1 = Spring(joint_mass1_spring1, joint_spring1_rigidbody1, 10, 10)
+    #rigidbody1 = RigidBody(joint_spring1_rigidbody1, joint_rigidbody1_spring2, 0.0)
+    spring2 = Spring(joint_rigidbody1_spring2, joint_right_spring2, 10, 100)
+
+    joint_left_mass1.add_extenal_force(np.array([5, 0, 0]))
+
+    simulation = BFSSimulation(joint_left_mass1, 1, 0.1)
+
+
+    
+
+    simulation.get_all_nodes_and_edges()
+
+    simulation.run()
+
+if __name__ == "__main__":
+    main()
+    

code/setup/code/deprecated/is_picklable.py

@@ -0,0 +1,30 @@
+import pickle
+from PhysicsElements import StateSpace, ZERO_VECTOR, Joint, Spring
+import numpy as np
+from Object_Sharing import SharedJointList
+
+def is_picklable(obj):
+    try:
+        pickle.dumps(obj)
+        return True
+    except pickle.PicklingError:
+        return False
+
+from multiprocessing.managers import BaseManager
+
+def func(x,y):
+    return 10
+
+if __name__ == '__main__':
+    BaseManager.register('Joint', Joint)
+    BaseManager.register('Spring', Spring)
+
+    manager = BaseManager()
+    manager.start()
+
+    north_wall_joint:Joint = manager.Joint(np.array([10,0,0]), mass=0.001, fixed=True, name="North Wall Joint")
+    main_mass:Joint = manager.Joint(np.array([0,0,0]), mass=3, fixed=False, name="Blue Mass")
+    middle_mass:Joint = manager.Joint(np.array([5,0,0]), mass=5, fixed=False, name="White Mass")
+    spring1 = manager.Spring(parent_joint=middle_mass, child_joint=north_wall_joint, unstretched_length=7.5, stiffness_func=func, name="Spring 1")
+
+    print(is_picklable(spring1))

code/setup/code/deprecated/live_plotter_test.py

@@ -0,0 +1,111 @@
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+import random
+from multiprocessing import Process, Manager
+
+class LivePlot:
+    def __init__(self, mult=1, max_data_points=100):
+        fig, (ax1, ax2, ax3) = plt.subplots(3, 1, figsize=(8, 6))
+        self.fig = fig
+        self.ax1 = ax1
+        self.ax2 = ax2
+        self.ax3 = ax3
+
+        self.mult = mult
+
+        self.ax1.set_ylabel('Plot 1')
+        self.ax2.set_ylabel('Plot 2')
+        self.ax3.set_ylabel('Plot 3')
+        self.ax3.set_xlabel('Time')
+
+        self.max_data_points = max_data_points
+        self.t = 0
+        self.x_data = []
+        self.y_data1 = []
+        self.y_data1_2 = []
+        self.y_data2 = []
+        self.y_data3 = []
+
+        # Initialize lines (empty initially)
+        self.line1, = self.ax1.plot([], [], label='Plot 1')
+        self.line1_2, = self.ax1.plot([], [], label='Plot 1.2')
+        self.line2, = self.ax2.plot([], [], label='Plot 2')
+        self.line3, = self.ax3.plot([], [], label='Plot 3')
+
+        self.ax1.legend()
+        self.ax2.legend()
+        self.ax3.legend()
+
+    def generate_random_data(self):
+        return random.randint(1, 100)
+
+    def update_data_external(self):
+        # Simulate external updates (replace with your actual data update mechanism)
+        new_x = self.t
+        self.t += 1
+        new_y1 = self.generate_random_data() * self.mult
+        new_y2 = self.generate_random_data() * self.mult
+        new_y3 = self.generate_random_data() * self.mult
+
+        self.x_data.append(new_x)
+        self.y_data1.append(new_y1)
+        self.y_data1_2.append(new_y1 * -1)  # Example modification of data
+        self.y_data2.append(new_y2)
+        self.y_data3.append(new_y3)
+
+        # Keep only the last `max_data_points` data points
+        self.x_data = self.x_data[-self.max_data_points:]
+        self.y_data1 = self.y_data1[-self.max_data_points:]
+        self.y_data1_2 = self.y_data1_2[-self.max_data_points:]
+        self.y_data2 = self.y_data2[-self.max_data_points:]
+        self.y_data3 = self.y_data3[-self.max_data_points:]
+
+    def update_plot(self, i):
+        self.update_data_external()
+
+        # Update plot data
+        self.line1.set_data(self.x_data, self.y_data1)
+        self.line1_2.set_data(self.x_data, self.y_data1_2)
+        self.line2.set_data(self.x_data, self.y_data2)
+        self.line3.set_data(self.x_data, self.y_data3)
+
+        # Adjust plot limits (x-axis)
+        if len(self.x_data) > 1:
+            self.ax1.set_xlim(self.x_data[0], self.x_data[-1])
+            self.ax2.set_xlim(self.x_data[0], self.x_data[-1])
+            self.ax3.set_xlim(self.x_data[0], self.x_data[-1])
+
+        # Adjust plot limits (y-axis) - optional
+        self.ax1.relim()
+        self.ax1.autoscale_view()
+        self.ax2.relim()
+        self.ax2.autoscale_view()
+        self.ax3.relim()
+        self.ax3.autoscale_view()
+
+        #return self.line1, self.line2, self.line3
+
+    def animate(self):
+        anim = animation.FuncAnimation(self.fig, self.update_plot, frames=1000, interval=10, blit=False)
+        plt.show()
+        #anim.save(filename="test.mp4")
+
+# Function to run animation in a separate process
+def run_animation(fig, ax1, ax2, ax3):
+    #live_plot = LivePlot(fig, ax1, ax2, ax3)
+    #live_plot.animate()
+    pass
+
+# Example usage with multiprocessing
+if __name__ == '__main__':
+    plot1 = LivePlot(mult=1)
+    plot2 = LivePlot(mult=5)
+
+    process1 = Process(target=plot1.animate)
+    process2 = Process(target=plot2.animate)
+
+    process1.start()
+    process2.start()
+
+    process1.join()
+    process2.join()

code/setup/code/deprecated/mcc_usb1024_test.py

@@ -0,0 +1,59 @@
+from mcculw import ul
+from mcculw.enums import DigitalIODirection
+from mcculw.ul import ULError
+
+# Define the board number
+BOARD_NUM = 0
+
+# Define the ports and the direction of data flow
+PORT_A = 10
+PORT_B = 11
+PORT_C_LOW = 12
+PORT_C_HIGH = 13
+
+# Configure ports as input or output
+def configure_ports():
+    try:
+        ul.d_config_port(BOARD_NUM, PORT_A, DigitalIODirection.OUT)
+        ul.d_config_port(BOARD_NUM, PORT_B, DigitalIODirection.IN)
+        ul.d_config_port(BOARD_NUM, PORT_C_LOW, DigitalIODirection.OUT)
+        ul.d_config_port(BOARD_NUM, PORT_C_HIGH, DigitalIODirection.IN)
+        print("Ports configured successfully.")
+    except ULError as e:
+        print(f"Error configuring ports: {e}")
+
+# Write data to a port
+def write_data(port, data):
+    try:
+        ul.d_out(BOARD_NUM, port, data)
+        print(f"Data {data} written to port {port} successfully.")
+    except ULError as e:
+        print(f"Error writing data to port {port}: {e}")
+
+# Read data from a port
+def read_data(port):
+    try:
+        data = ul.d_in(BOARD_NUM, port)
+        print(f"Data read from port {port}: {data}")
+        return data
+    except ULError as e:
+        print(f"Error reading data from port {port}: {e}")
+        return None
+
+def main():
+    # Configure the ports
+    configure_ports()
+    
+    # Write some data to PORT_A and PORT_C_LOW
+    write_data(PORT_A, 0xFF)  # Write 0xFF (255) to PORT_A
+    write_data(PORT_C_LOW, 0xAA)  # Write 0xAA (170) to PORT_C_LOW
+    
+    # Read data from PORT_B and PORT_C_HIGH
+    data_b = read_data(PORT_B)
+    data_c_high = read_data(PORT_C_HIGH)
+    
+    # Perform additional processing as needed
+    # For example, you might want to perform some logic based on the input data
+    
+if __name__ == "__main__":
+    main()

code/setup/code/deprecated/pstats_reader.py

@@ -0,0 +1,8 @@
+import cProfile
+import pstats
+
+# Load the profiling data from the file
+stats = pstats.Stats('profile_results.txt')
+
+# Print the top 10 functions by cumulative time
+stats.sort_stats('cumulative').print_stats(25)

code/setup/code/deprecated/pyqtgraph_test.py

@@ -0,0 +1,78 @@
+import pyqtgraph as pg
+from pyqtgraph.Qt import QtWidgets, QtCore
+import numpy as np
+
+class RealTimePlot:
+    def __init__(self, update_interval=1, max_display_time=10, num_subplots=2, line_styles=None):
+        # Create the application
+        self.app = QtWidgets.QApplication([])
+
+        # Create a plot window
+        self.win = pg.GraphicsLayoutWidget(show=True, title="Real-Time Plot")
+        self.win.resize(1000, 600)
+
+        # Add plots
+        self.plots = []
+        self.curves = []
+        self.line_styles = line_styles if line_styles is not None else ['line'] * num_subplots
+        for i in range(num_subplots):
+            plot = self.win.addPlot(title=f"Subplot {i+1}")
+            plot.setLabel('left', f'Subplot {i+1} Y-Axis')
+            plot.addLegend()  # Add a legend to each plot
+            self.plots.append(plot)
+
+            if self.line_styles[i] == 'step':
+                curve = plot.plot(pen='y', name=f'Subplot {i+1} Data', stepMode=True)
+            else:
+                curve = plot.plot(pen='y', name=f'Subplot {i+1} Data')
+            
+            self.curves.append(curve)
+            if i < num_subplots - 1:
+                self.win.nextRow()
+
+        # Data buffers
+        self.xdata = [np.empty(0) for _ in range(num_subplots)]
+        self.ydata = [np.empty(0) for _ in range(num_subplots)]
+
+        # Parameters
+        self.update_interval = update_interval
+        self.max_display_time = max_display_time
+        self.timer = QtCore.QTimer()
+        self.timer.timeout.connect(self.update_live_window)
+        self.timer.start(update_interval)
+
+    def update_live_window(self):
+        for i in range(len(self.plots)):
+            # Generate new data
+            t = self.xdata[i][-1] + self.update_interval / 1000.0 if len(self.xdata[i]) > 0 else 0
+            y = np.sin(2 * np.pi * t + i)  # Different phase for each subplot
+
+            # Append new data to buffers
+            self.xdata[i] = np.append(self.xdata[i], t)
+            self.ydata[i] = np.append(self.ydata[i], y)
+
+            # Remove old data to keep the buffer size within max_display_time
+            if t > self.max_display_time:
+                self.xdata[i] = self.xdata[i][self.xdata[i] > t - self.max_display_time]
+                self.ydata[i] = self.ydata[i][-len(self.xdata[i]):]
+
+            # Ensure correct lengths for step mode
+            if self.line_styles[i] == 'step':
+                xdata_step = np.append(self.xdata[i], self.xdata[i][-1] + self.update_interval / 1000.0)
+                self.curves[i].setData(xdata_step, self.ydata[i])
+            else:
+                self.curves[i].setData(self.xdata[i], self.ydata[i])
+
+    def run(self):
+        # Start the Qt event loop
+        QtWidgets.QApplication.instance().exec_()
+
+# Parameters
+update_interval = 100  # milliseconds
+max_display_time = 10  # seconds
+num_subplots = 2  # number of subplots
+line_styles = ['line', 'step']  # specify 'line' or 'step' for each subplot
+
+# Create and run the real-time plot
+rt_plot = RealTimePlot(update_interval, max_display_time, num_subplots, line_styles)
+rt_plot.run()

code/setup/code/deprecated/shared_memory.py

@@ -0,0 +1,99 @@
+import multiprocessing
+from multiprocessing.managers import BaseManager
+import numpy as np
+import time
+import random
+    
+from PhysicsElements import Joint, Spring, SpringWithMass, Actuator, StateSpace
+
+class CustomManager(BaseManager):
+    # nothing
+    pass
+
+
+def worker_process(shared_state_space):
+    while True:
+        print("Worker Process:")
+        shared_state_space.print()
+        time.sleep(0.1)
+
+def worker_process2(shared_joint: Joint):
+    while True:
+        print("Worker Process:")
+        statespace = shared_joint.get_state_space()
+        print(statespace.print())
+        time.sleep(0.1)
+
+def worker_process3(shared_joint_list: list):
+    while True:
+        print("Worker Process:")
+        statespace:StateSpace = shared_joint_list.at(0).get_state_space()
+        statespace.print()
+        time.sleep(0.1)
+
+def statespace_changer(shared_joint_list: list):
+    while True:
+        rand_int = random.randint(0, 10)
+        shared_joint_list.at(0).set_state_space_pos(np.array([rand_int,0,0]))
+        print("Changed pos")
+        time.sleep(1)
+
+class SharedJointList():
+    def __init__(self):
+        self.list: list = []
+
+
+    def append(self, new_joint: Joint):
+        self.list.append(new_joint)
+
+    def at(self, index: int):
+        return self.list[index]
+    
+
+
+
+if __name__ == "__main__":
+    # Create a multiprocessing manager
+    # CustomManager.register('StateSpace', StateSpace)
+    # CustomManager.register('Joint', Joint)
+    # CustomManager.register('SharedJointList', SharedJointList)
+    #BaseManager.register('StateSpace', StateSpace)
+    BaseManager.register('Joint', Joint)
+    BaseManager.register('SharedJointList', SharedJointList)
+
+    with BaseManager() as manager:
+        #shared_statespace = manager.StateSpace()
+
+        shared_joint:Joint = manager.Joint(pos=np.array([0,0,-4]), mass=0.001, fixed=True, name="North Actuator Joint")
+        
+        joint_list:SharedJointList = manager.SharedJointList()
+
+        rand_joint = manager.Joint(pos=np.array([0,2,15]), 
+                                mass=0.001, 
+                                fixed=True, 
+                                name="Random Joint")
+        joint_list.append(rand_joint)
+
+
+        #proc = multiprocessing.Process(target=worker_process, args=(shared_statespace,))
+        #proc = multiprocessing.Process(target=worker_process2, args=(shared_joint,))
+        proc = multiprocessing.Process(target=worker_process3, args=(joint_list,))
+        proc.start()
+
+        changer_proc = multiprocessing.Process(target=statespace_changer, args=(joint_list,))
+        changer_proc.start()
+        time.sleep(2)
+
+        #shared_statespace.set_pos(np.array([-8,-4,-2]))
+        #shared_joint.set_state_space_pos((np.array([-8,-4,-2])))
+        
+        
+        
+        
+        while True:
+            print("Main: ", end="")
+            joint_list.at(0).get_state_space().print()
+            time.sleep(1)
+
+        proc.join()
+        changer_proc.join()

code/setup/code/deprecated/tests/.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "python.REPL.enableREPLSmartSend": false
+}

code/setup/code/setup/dependencies.py

@@ -0,0 +1,23 @@
+import subprocess
+import sys
+
+def install_packages(packages):
+    for package in packages:
+        print(f"Installing {package}...")
+        try:
+            result = subprocess.run([sys.executable, "-m", "pip", "install", package], check=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True)
+            print(result.stdout)
+            if result.stderr:
+                print(result.stderr)
+        except subprocess.CalledProcessError as e:
+            print(f"Failed to install {package}: {e}")
+            sys.exit(1)
+
+if __name__ == "__main__":
+    packages = ['vpython', 'numpy', 'typing', 'pyqtgraph', 'pyqt5' , 'mcculw']
+    source_dir = "./mcculw_files"  # Relative path to the local directory
+    dest_dir = "C:/absolute/path/to/directory"  # Absolute path to the destination directory
+    
+    install_packages(packages)
+    
+    print("Installation and file copy complete.")

code/setup/code/useful_scripts/verlet_coefficients_calculator.py

@@ -0,0 +1,29 @@
+import numpy as np
+
+
+num_points = 7
+num_columns = num_points + 1
+
+RHS = np.zeros(num_columns)
+RHS[2] = 1
+
+taylor_coef_matrix = np.zeros((num_columns, num_columns))
+
+bases = [j for j in range(-num_points+1, 2)]
+
+for i in range(num_columns):
+    row = []
+    for base in bases:
+        row.append(base**i)
+    taylor_coef_matrix[i] = row
+
+x = np.linalg.solve(taylor_coef_matrix, RHS)
+
+
+multiplier = 1 / x[-1]
+x = x[0:-1]
+x = -1*x
+x = np.append(x, [0.5])
+x = x * multiplier
+
+print(x)

code/setup/dependencies.py

@@ -0,0 +1,23 @@
+import subprocess
+import sys
+
+def install_packages(packages):
+    for package in packages:
+        print(f"Installing {package}...")
+        try:
+            result = subprocess.run([sys.executable, "-m", "pip", "install", package], check=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True)
+            print(result.stdout)
+            if result.stderr:
+                print(result.stderr)
+        except subprocess.CalledProcessError as e:
+            print(f"Failed to install {package}: {e}")
+            sys.exit(1)
+
+if __name__ == "__main__":
+    packages = ['vpython', 'numpy', 'typing', 'pyqtgraph', 'pyqt5' , 'mcculw']
+    source_dir = "./mcculw_files"  # Relative path to the local directory
+    dest_dir = "C:/absolute/path/to/directory"  # Absolute path to the destination directory
+    
+    install_packages(packages)
+    
+    print("Installation and file copy complete.")

code/setup/documentation/code_design/.$Process_Diagram.drawio.bkp

@@ -0,0 +1,149 @@
+<mxfile host="Electron" modified="2024-06-11T19:31:46.378Z" agent="Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/24.0.4 Chrome/120.0.6099.109 Electron/28.1.0 Safari/537.36" etag="YiW67u71xuPxX2CrrYUi" version="24.0.4" type="device">
+  <diagram name="Page-1" id="e7e014a7-5840-1c2e-5031-d8a46d1fe8dd">
+    <mxGraphModel dx="2074" dy="1204" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="1169" pageHeight="826" background="none" math="0" shadow="0">
+      <root>
+        <mxCell id="0" />
+        <mxCell id="1" parent="0" />
+        <mxCell id="2" value="Thread 1" style="swimlane;whiteSpace=wrap" parent="1" vertex="1">
+          <mxGeometry x="164.5" y="128" width="280" height="570" as="geometry" />
+        </mxCell>
+        <mxCell id="5" value="" style="ellipse;shape=startState;fillColor=#000000;strokeColor=#ff0000;" parent="2" vertex="1">
+          <mxGeometry x="100" y="40" width="30" height="30" as="geometry" />
+        </mxCell>
+        <mxCell id="6" value="" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;verticalAlign=bottom;endArrow=open;endSize=8;strokeColor=#FF0000;endFill=1;rounded=0" parent="2" source="5" target="7" edge="1">
+          <mxGeometry x="100" y="40" as="geometry">
+            <mxPoint x="115" y="110" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="7" value="idle" style="" parent="2" vertex="1">
+          <mxGeometry x="60" y="110" width="110" height="50" as="geometry" />
+        </mxCell>
+        <mxCell id="8" value="user action" style="" parent="2" vertex="1">
+          <mxGeometry x="60" y="220" width="110" height="50" as="geometry" />
+        </mxCell>
+        <mxCell id="9" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="2" source="7" target="8" edge="1">
+          <mxGeometry relative="1" as="geometry" />
+        </mxCell>
+        <mxCell id="10" value="post command" style="" parent="2" vertex="1">
+          <mxGeometry x="60" y="325" width="110" height="50" as="geometry" />
+        </mxCell>
+        <mxCell id="11" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="2" source="8" target="10" edge="1">
+          <mxGeometry relative="1" as="geometry" />
+        </mxCell>
+        <mxCell id="12" value="" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;strokeColor=#FF0000;endArrow=open;endFill=1;rounded=0" parent="2" source="10" target="7" edge="1">
+          <mxGeometry width="100" height="100" relative="1" as="geometry">
+            <mxPoint x="160" y="290" as="sourcePoint" />
+            <mxPoint x="260" y="190" as="targetPoint" />
+            <Array as="points">
+              <mxPoint x="30" y="250" />
+            </Array>
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="3" value="Thread 2" style="swimlane;whiteSpace=wrap" parent="1" vertex="1">
+          <mxGeometry x="444.5" y="128" width="280" height="570" as="geometry" />
+        </mxCell>
+        <mxCell id="13" value="" style="ellipse;shape=startState;fillColor=#000000;strokeColor=#ff0000;" parent="3" vertex="1">
+          <mxGeometry x="60" y="40" width="30" height="30" as="geometry" />
+        </mxCell>
+        <mxCell id="14" value="" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;verticalAlign=bottom;endArrow=open;endSize=8;strokeColor=#FF0000;endFill=1;rounded=0" parent="3" source="13" target="15" edge="1">
+          <mxGeometry x="40" y="20" as="geometry">
+            <mxPoint x="55" y="90" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="15" value="idle" style="" parent="3" vertex="1">
+          <mxGeometry x="20" y="110" width="110" height="50" as="geometry" />
+        </mxCell>
+        <mxCell id="16" value="check for &#xa;new commands" style="" parent="3" vertex="1">
+          <mxGeometry x="20" y="220" width="110" height="50" as="geometry" />
+        </mxCell>
+        <mxCell id="17" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="3" source="15" target="16" edge="1">
+          <mxGeometry relative="1" as="geometry" />
+        </mxCell>
+        <mxCell id="18" value="command queue" style="" parent="3" vertex="1">
+          <mxGeometry x="20" y="325" width="110" height="50" as="geometry" />
+        </mxCell>
+        <mxCell id="19" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="3" source="16" target="18" edge="1">
+          <mxGeometry relative="1" as="geometry" />
+        </mxCell>
+        <mxCell id="21" value="queue empty" style="rhombus;fillColor=#ffffc0;strokeColor=#ff0000;" parent="3" vertex="1">
+          <mxGeometry x="150" y="225" width="80" height="40" as="geometry" />
+        </mxCell>
+        <mxCell id="22" value="yes" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;align=left;verticalAlign=bottom;endArrow=open;endSize=8;strokeColor=#FF0000;exitX=0.5;exitY=0;endFill=1;rounded=0;entryX=0.75;entryY=0.5;entryPerimeter=0" parent="3" source="21" target="25" edge="1">
+          <mxGeometry x="-1" relative="1" as="geometry">
+            <mxPoint x="160" y="150" as="targetPoint" />
+            <Array as="points">
+              <mxPoint x="190" y="180" />
+            </Array>
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="23" value="no" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;align=left;verticalAlign=top;endArrow=open;endSize=8;strokeColor=#FF0000;endFill=1;rounded=0" parent="3" source="21" target="30" edge="1">
+          <mxGeometry x="-1" relative="1" as="geometry">
+            <mxPoint x="190" y="305" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="24" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="3" source="16" target="21" edge="1">
+          <mxGeometry relative="1" as="geometry" />
+        </mxCell>
+        <mxCell id="25" value="" style="shape=line;strokeWidth=6;strokeColor=#ff0000;rotation=90" parent="3" vertex="1">
+          <mxGeometry x="130" y="127.5" width="50" height="15" as="geometry" />
+        </mxCell>
+        <mxCell id="26" value="" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;verticalAlign=bottom;endArrow=open;endSize=8;strokeColor=#FF0000;endFill=1;rounded=0" parent="3" source="25" target="15" edge="1">
+          <mxGeometry x="130" y="90" as="geometry">
+            <mxPoint x="230" y="140" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="30" value="dispatch&#xa;command&#xa;worker thread" style="" parent="3" vertex="1">
+          <mxGeometry x="140" y="325" width="110" height="50" as="geometry" />
+        </mxCell>
+        <mxCell id="31" value="critical&#xa;section" style="shape=note;whiteSpace=wrap;size=17" parent="3" vertex="1">
+          <mxGeometry x="105" y="490" width="100" height="50" as="geometry" />
+        </mxCell>
+        <mxCell id="32" value="" style="endArrow=none;strokeColor=#FF0000;endFill=0;rounded=0;dashed=1" parent="3" source="18" target="31" edge="1">
+          <mxGeometry relative="1" as="geometry" />
+        </mxCell>
+        <mxCell id="34" value="" style="whiteSpace=wrap;strokeColor=#FF0000;fillColor=#FF0000" parent="3" vertex="1">
+          <mxGeometry x="245" y="395" width="5" height="45" as="geometry" />
+        </mxCell>
+        <mxCell id="41" value="" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;entryX=0;entryY=0.5;strokeColor=#FF0000;endArrow=open;endFill=1;rounded=0" parent="3" source="30" target="34" edge="1">
+          <mxGeometry width="100" height="100" relative="1" as="geometry">
+            <mxPoint x="60" y="480" as="sourcePoint" />
+            <mxPoint x="160" y="380" as="targetPoint" />
+            <Array as="points">
+              <mxPoint x="195" y="400" />
+            </Array>
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="4" value="Thread 3" style="swimlane;whiteSpace=wrap" parent="1" vertex="1">
+          <mxGeometry x="724.5" y="128" width="280" height="570" as="geometry" />
+        </mxCell>
+        <mxCell id="33" value="process&#xa;command" style="" parent="4" vertex="1">
+          <mxGeometry x="90" y="405" width="110" height="50" as="geometry" />
+        </mxCell>
+        <mxCell id="35" value="" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;strokeColor=#FF0000;endArrow=open;endFill=1;rounded=0;entryX=0.25;entryY=0.5;entryPerimeter=0" parent="4" target="25" edge="1">
+          <mxGeometry width="100" height="100" relative="1" as="geometry">
+            <mxPoint x="-30" y="410" as="sourcePoint" />
+            <mxPoint x="-120" y="120" as="targetPoint" />
+            <Array as="points">
+              <mxPoint x="-10" y="135" />
+            </Array>
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="37" value="" style="edgeStyle=none;strokeColor=#FF0000;endArrow=open;endFill=1;rounded=0" parent="4" target="33" edge="1">
+          <mxGeometry width="100" height="100" relative="1" as="geometry">
+            <mxPoint x="-30" y="429.5" as="sourcePoint" />
+            <mxPoint x="90" y="429.5" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="38" value="" style="ellipse;shape=endState;fillColor=#000000;strokeColor=#ff0000" parent="4" vertex="1">
+          <mxGeometry x="130" y="500" width="30" height="30" as="geometry" />
+        </mxCell>
+        <mxCell id="39" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="4" source="33" target="38" edge="1">
+          <mxGeometry relative="1" as="geometry" />
+        </mxCell>
+        <mxCell id="20" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="1" source="10" target="18" edge="1">
+          <mxGeometry relative="1" as="geometry" />
+        </mxCell>
+      </root>
+    </mxGraphModel>
+  </diagram>
+</mxfile>

code/useful_scripts/verlet_coefficients_calculator.py

@@ -0,0 +1,29 @@
+import numpy as np
+
+
+num_points = 7
+num_columns = num_points + 1
+
+RHS = np.zeros(num_columns)
+RHS[2] = 1
+
+taylor_coef_matrix = np.zeros((num_columns, num_columns))
+
+bases = [j for j in range(-num_points+1, 2)]
+
+for i in range(num_columns):
+    row = []
+    for base in bases:
+        row.append(base**i)
+    taylor_coef_matrix[i] = row
+
+x = np.linalg.solve(taylor_coef_matrix, RHS)
+
+
+multiplier = 1 / x[-1]
+x = x[0:-1]
+x = -1*x
+x = np.append(x, [0.5])
+x = x * multiplier
+
+print(x)

documentation/code_design/.$Process_Diagram.drawio.bkp

@@ -0,0 +1,149 @@
+<mxfile host="Electron" modified="2024-06-11T19:31:46.378Z" agent="Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/24.0.4 Chrome/120.0.6099.109 Electron/28.1.0 Safari/537.36" etag="YiW67u71xuPxX2CrrYUi" version="24.0.4" type="device">
+  <diagram name="Page-1" id="e7e014a7-5840-1c2e-5031-d8a46d1fe8dd">
+    <mxGraphModel dx="2074" dy="1204" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="1169" pageHeight="826" background="none" math="0" shadow="0">
+      <root>
+        <mxCell id="0" />
+        <mxCell id="1" parent="0" />
+        <mxCell id="2" value="Thread 1" style="swimlane;whiteSpace=wrap" parent="1" vertex="1">
+          <mxGeometry x="164.5" y="128" width="280" height="570" as="geometry" />
+        </mxCell>
+        <mxCell id="5" value="" style="ellipse;shape=startState;fillColor=#000000;strokeColor=#ff0000;" parent="2" vertex="1">
+          <mxGeometry x="100" y="40" width="30" height="30" as="geometry" />
+        </mxCell>
+        <mxCell id="6" value="" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;verticalAlign=bottom;endArrow=open;endSize=8;strokeColor=#FF0000;endFill=1;rounded=0" parent="2" source="5" target="7" edge="1">
+          <mxGeometry x="100" y="40" as="geometry">
+            <mxPoint x="115" y="110" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="7" value="idle" style="" parent="2" vertex="1">
+          <mxGeometry x="60" y="110" width="110" height="50" as="geometry" />
+        </mxCell>
+        <mxCell id="8" value="user action" style="" parent="2" vertex="1">
+          <mxGeometry x="60" y="220" width="110" height="50" as="geometry" />
+        </mxCell>
+        <mxCell id="9" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="2" source="7" target="8" edge="1">
+          <mxGeometry relative="1" as="geometry" />
+        </mxCell>
+        <mxCell id="10" value="post command" style="" parent="2" vertex="1">
+          <mxGeometry x="60" y="325" width="110" height="50" as="geometry" />
+        </mxCell>
+        <mxCell id="11" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="2" source="8" target="10" edge="1">
+          <mxGeometry relative="1" as="geometry" />
+        </mxCell>
+        <mxCell id="12" value="" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;strokeColor=#FF0000;endArrow=open;endFill=1;rounded=0" parent="2" source="10" target="7" edge="1">
+          <mxGeometry width="100" height="100" relative="1" as="geometry">
+            <mxPoint x="160" y="290" as="sourcePoint" />
+            <mxPoint x="260" y="190" as="targetPoint" />
+            <Array as="points">
+              <mxPoint x="30" y="250" />
+            </Array>
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="3" value="Thread 2" style="swimlane;whiteSpace=wrap" parent="1" vertex="1">
+          <mxGeometry x="444.5" y="128" width="280" height="570" as="geometry" />
+        </mxCell>
+        <mxCell id="13" value="" style="ellipse;shape=startState;fillColor=#000000;strokeColor=#ff0000;" parent="3" vertex="1">
+          <mxGeometry x="60" y="40" width="30" height="30" as="geometry" />
+        </mxCell>
+        <mxCell id="14" value="" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;verticalAlign=bottom;endArrow=open;endSize=8;strokeColor=#FF0000;endFill=1;rounded=0" parent="3" source="13" target="15" edge="1">
+          <mxGeometry x="40" y="20" as="geometry">
+            <mxPoint x="55" y="90" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="15" value="idle" style="" parent="3" vertex="1">
+          <mxGeometry x="20" y="110" width="110" height="50" as="geometry" />
+        </mxCell>
+        <mxCell id="16" value="check for &#xa;new commands" style="" parent="3" vertex="1">
+          <mxGeometry x="20" y="220" width="110" height="50" as="geometry" />
+        </mxCell>
+        <mxCell id="17" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="3" source="15" target="16" edge="1">
+          <mxGeometry relative="1" as="geometry" />
+        </mxCell>
+        <mxCell id="18" value="command queue" style="" parent="3" vertex="1">
+          <mxGeometry x="20" y="325" width="110" height="50" as="geometry" />
+        </mxCell>
+        <mxCell id="19" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="3" source="16" target="18" edge="1">
+          <mxGeometry relative="1" as="geometry" />
+        </mxCell>
+        <mxCell id="21" value="queue empty" style="rhombus;fillColor=#ffffc0;strokeColor=#ff0000;" parent="3" vertex="1">
+          <mxGeometry x="150" y="225" width="80" height="40" as="geometry" />
+        </mxCell>
+        <mxCell id="22" value="yes" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;align=left;verticalAlign=bottom;endArrow=open;endSize=8;strokeColor=#FF0000;exitX=0.5;exitY=0;endFill=1;rounded=0;entryX=0.75;entryY=0.5;entryPerimeter=0" parent="3" source="21" target="25" edge="1">
+          <mxGeometry x="-1" relative="1" as="geometry">
+            <mxPoint x="160" y="150" as="targetPoint" />
+            <Array as="points">
+              <mxPoint x="190" y="180" />
+            </Array>
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="23" value="no" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;align=left;verticalAlign=top;endArrow=open;endSize=8;strokeColor=#FF0000;endFill=1;rounded=0" parent="3" source="21" target="30" edge="1">
+          <mxGeometry x="-1" relative="1" as="geometry">
+            <mxPoint x="190" y="305" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="24" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="3" source="16" target="21" edge="1">
+          <mxGeometry relative="1" as="geometry" />
+        </mxCell>
+        <mxCell id="25" value="" style="shape=line;strokeWidth=6;strokeColor=#ff0000;rotation=90" parent="3" vertex="1">
+          <mxGeometry x="130" y="127.5" width="50" height="15" as="geometry" />
+        </mxCell>
+        <mxCell id="26" value="" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;verticalAlign=bottom;endArrow=open;endSize=8;strokeColor=#FF0000;endFill=1;rounded=0" parent="3" source="25" target="15" edge="1">
+          <mxGeometry x="130" y="90" as="geometry">
+            <mxPoint x="230" y="140" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="30" value="dispatch&#xa;command&#xa;worker thread" style="" parent="3" vertex="1">
+          <mxGeometry x="140" y="325" width="110" height="50" as="geometry" />
+        </mxCell>
+        <mxCell id="31" value="critical&#xa;section" style="shape=note;whiteSpace=wrap;size=17" parent="3" vertex="1">
+          <mxGeometry x="105" y="490" width="100" height="50" as="geometry" />
+        </mxCell>
+        <mxCell id="32" value="" style="endArrow=none;strokeColor=#FF0000;endFill=0;rounded=0;dashed=1" parent="3" source="18" target="31" edge="1">
+          <mxGeometry relative="1" as="geometry" />
+        </mxCell>
+        <mxCell id="34" value="" style="whiteSpace=wrap;strokeColor=#FF0000;fillColor=#FF0000" parent="3" vertex="1">
+          <mxGeometry x="245" y="395" width="5" height="45" as="geometry" />
+        </mxCell>
+        <mxCell id="41" value="" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;entryX=0;entryY=0.5;strokeColor=#FF0000;endArrow=open;endFill=1;rounded=0" parent="3" source="30" target="34" edge="1">
+          <mxGeometry width="100" height="100" relative="1" as="geometry">
+            <mxPoint x="60" y="480" as="sourcePoint" />
+            <mxPoint x="160" y="380" as="targetPoint" />
+            <Array as="points">
+              <mxPoint x="195" y="400" />
+            </Array>
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="4" value="Thread 3" style="swimlane;whiteSpace=wrap" parent="1" vertex="1">
+          <mxGeometry x="724.5" y="128" width="280" height="570" as="geometry" />
+        </mxCell>
+        <mxCell id="33" value="process&#xa;command" style="" parent="4" vertex="1">
+          <mxGeometry x="90" y="405" width="110" height="50" as="geometry" />
+        </mxCell>
+        <mxCell id="35" value="" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;strokeColor=#FF0000;endArrow=open;endFill=1;rounded=0;entryX=0.25;entryY=0.5;entryPerimeter=0" parent="4" target="25" edge="1">
+          <mxGeometry width="100" height="100" relative="1" as="geometry">
+            <mxPoint x="-30" y="410" as="sourcePoint" />
+            <mxPoint x="-120" y="120" as="targetPoint" />
+            <Array as="points">
+              <mxPoint x="-10" y="135" />
+            </Array>
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="37" value="" style="edgeStyle=none;strokeColor=#FF0000;endArrow=open;endFill=1;rounded=0" parent="4" target="33" edge="1">
+          <mxGeometry width="100" height="100" relative="1" as="geometry">
+            <mxPoint x="-30" y="429.5" as="sourcePoint" />
+            <mxPoint x="90" y="429.5" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="38" value="" style="ellipse;shape=endState;fillColor=#000000;strokeColor=#ff0000" parent="4" vertex="1">
+          <mxGeometry x="130" y="500" width="30" height="30" as="geometry" />
+        </mxCell>
+        <mxCell id="39" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="4" source="33" target="38" edge="1">
+          <mxGeometry relative="1" as="geometry" />
+        </mxCell>
+        <mxCell id="20" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="1" source="10" target="18" edge="1">
+          <mxGeometry relative="1" as="geometry" />
+        </mxCell>
+      </root>
+    </mxGraphModel>
+  </diagram>
+</mxfile>