commit 482d724e2023ec4c150ad33033b59c57413c903a Author: SchrodingerError <131827896+SchrodingerError@users.noreply.github.com> Date: Wed Aug 14 14:42:16 2024 -0500 Initial Commit diff --git a/README.md b/README.md new file mode 100644 index 0000000..f32fb04 --- /dev/null +++ b/README.md @@ -0,0 +1,1386 @@ +# Initial Understanding +This project is meant to provide a simple test bed for development and learning of an RMC controller. +Specifically, the project is aimed for modeling hydraulic actuators interfacing with ropes and masses. + +The project provides a physics simulation for general purpose, 'similar-to-life', physics scenarios so that equations of motion +do not need to be derived for a given scenario. The project also incorporates the interfaces required for interacting with real-world devices. +It utilizes the Measurement Computing Company (MCC) [USB-202](https://files.digilent.com/manuals/USB-202.pdf) for analog and digital input and the MCC [USB-3114](https://files.digilent.com/manuals/USB-3114.pdf) for analog outputs. + +To provide a visualization for the user, there is a 3D visualization engine built using [VPython](https://vpython.org/). In addition, there is a wrapper for PyQtGraph so that +real-time data can be plotted for physics, sensors, HITL controller, or any combination. + +# Initial Setup +## Required Software Installation +1. Install `Python 3.11.4 through Company Portal`. This ensure the global configuration is set correctly for installing packages. +2. Install the Measurement Computing Universal Library for Windows with [code/setup/mccdaq.exe](code/setup/mccdaq.exe). DAQami does NOT need to be installed. Only `InstaCal & Universal Library`. +3. Run the Python dependecy installer by executing `python code/setup/dependencies.py`. This includes VPython, PyQtGraph, numpy, typing, and mcculw. vpython and PyQtGraph are significantly larger than the others. They are used for 3D visualization and plotting respectively. numpy is used for faster 'C-like' vectors. typing is for type hints in the code. mcculw is for interfacing with the MCC devices. + +[code/setup/icalsetup.exe](code/setup/icalsetup.exe) is used for InstaCal installation. This is not required as Python performs the device configuration for MCC3114 and MCC202. InstaCal will create a configuration file in `C:/ProgramData/Measurement Computing/DAQ` called `CB.CFG`. This file is created when you configure the MCC devices. Note, 1 configuration file is made for the entire system of devices. It is not per device. The Python code in [code/MCC_Inferface](code/MCC_Interface.py) configures the devices as necessary upon initialization. Therefore, a manual configuration through InstaCal is not needed. + + + +# Setting Up a HITL Simulation +It is strongly recommended to go through this guide with an example up. Before writing the code to setup a HITL simulation, have a clear idea of what joints you will need along with the springs and actuators. Each object will need specific information about how to set it up such as a joint's position, springs characteristics, and an actuator's control behavior. + +## Physics Elements +Controller elements are located in [code/Physics_Elements.py](code/Physics_Elements.py). There are 3 main physics elements. [joints](#joint), [springs](#spring), and [rigid actuators](#rigidactuator). [StateSpace](#statespace) is also included as a physics element. + +### StateSpace +StateSpace is where the Newtonian physics takes place. Each [Joint](#joint) is assigned a StateSpace at initialization. As the simulation runs, the StateSpace will be integrated over its acceleration and velocity to function as the physics engine. + +The user will never need to create a StateSpace manually. It is always handled through the creation of a [Joint](#joint). Understanding the basic aspects of the StateSpace can help understand behavior of the physics simulation. A simple integration method is shown. This is an abstraction of what StateSpace does. +``` +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 + + def integrate(self, delta_t: float): + self.vel = self.vel + self.accel * delta_t + self.pos = self.pos + self.vel * delta_t +``` +By default a StateSpace is initialized with all zeroes. It is typical to assign a position at [Joint](#joint) creation which is passed to the StateSpace. integration_method is passed from the creation of a [Joint](#joint) to specify the type of integration to use. This allows for a mix of speed and accuracy. + +### Joint +A Joint is a single point in space with mass. It has its own StateSpace. Joints can either be fixed in space or free (they can also be driven by an actuator which is a special case handled during creation of an actuator). + +Here is the code to create a Joint +``` +joint = Joint( + pos: array_like, # The initial position of the joint. typically use np.array([x, y, z]) + mass: float, # The mass in kg of the joint. Disregarded if fixed == True + fixed: bool, # If true, the Joint will never move. + name: str, + integration_method: str # "simple" (default), "verlet", "adams-bashforth" +) +``` +Now that the joint is created, it can be connected to a spring, actuator, or nothing if it is fixed. + +#### Joint Extra Attributes +The Joint class has a few extra functions that are useful for certain use. + +``` +joint.add_gravity() # Add gravity at 9.81 m/s^2 + +joint.add_damping( # Add either velocity or momentum damping. If mom_ratio is specified, disregard vel_ratio + vel_ratio: float, # Add a force equal to the negative velocity in m/s scaled by vel_ratio + mom_ratio: float # Add a force equal to the negative momentum in kg*m/s scaled by mom_ratio +) + +joint.add_constant_force( # Add a constant force to be applied to the joint + new_force: array_like # Typically use np.array([x, y, z]) +) + +joint.add_variable_force( # Add a time varying force in the form of a function pointer (NOT lambda function) to be applied to the joint + force_vect: array_like # Typically use np.array([f1, f2, f3]) where f = func1(t:float) +) +``` + +### Spring +A Spring connects 2 Joints and applies a force on them. Springs use the StateSpace of the parent [Joint](#joint) and child [Joint](#joint) attached to them. They calculate their $\vec r = \vec x_{child} - \vec x_{parent}$ and $\Delta l$ to get their force vector. Tension is treated as positive in all cases. + +Here is the code to create a Spring +``` +spring = Spring( + parent_joint: Spring, + child_joint: Spring, + unstretched_length: float, # Zero force length + contant_stiffness: float, # Constant k value. Ignored if stiffness_func != None + stiffness_func: func(length: float, unstretched_length) -> float, # Function pointer for variable stiffness + name: str +) +``` +Now that the Spring is created, it will be simulated in the physics simulation. The physics simulation knows it exists because each [Joint](#joint) keeps track of the springs attached to it. + +The Spring class has no other attributes for the user to work with. + + +### RigidActuator +A RigidActuator connects 2 Joints, a "grounded" joint and a "parent" joint. The grounded [Joint](#joint) is fixed and will not move. The parent joint is controlled in relation to the grounded joint. All RigidActuators must be controlled by something. They will either control the length of the actuator or the velocity. The RigidActuator will only move in the direction of the $\vec r = \vec x_{grounded} - \vec x_{parent}$. + +Here is the code to create a RigidActuator +``` +rigid_actuator = RigidActuator( + parent_joint: Joint, + grounded_joint: Joint, + name: str, + max_length: float, + min_length: float, + control_code: int # 0 for displacement control, 1 for velocity control, 2 for acceleration control +) +``` +Now that the RigidActuator is created, it will be simulated in the physics simulation. The physics simulation knows it exists because each [Joint](#joint) keeps track of the actuators attached to it. + +The RigidActuator class has no other attributes for the user to work with. + + +## Sensor Elements +Sensor elements are located in [code/Sensor_Elements.py](code/Sensor_Elements.py). Sensor elements are how you take something from the physics simulation and bring it to the real-world with voltage outputs. + +Because the physics simulation and HITL controller processes are separate, you must create a shared object to link them. These shared objects are SharedJoint and SharedSpring. They will need to be attached to both the raw physics element ([Joint](#joint), [Spring](#spring)) and the sensor element (LoadCellJoint, LoadCellSpring, DisplacementSensor). + +### LoadCell +Parent class to [LoadCellJoint](#loadcelljoint) and [LoadCellSpring](#loadcellspring) + +### LoadCellJoint +A load cell can be attached to a [Joint](#joint) with LoadCellJoint. This is less developed than LoadCellSpring as the force on a point in space is less representative than a force between joints. This is due to how it represents the force. It takes the net force applied by springs in any direction on the joint and vector sums them. + + +Create and attach a LoadCellJoint with +``` +load_cell_joint = LoadCellJoint(sensor_settings: dict = { + "name": str, + "mV/V": float, + "excitation": float, + "full_scale_force": float, # What is the max force on the load cell + "output_channel": int # The output channel that the load cell outputs through the MCC3114. Must be between [0, 15] +}) +load_cell_joint.attach_sim_source(shared_joint) + +hitl_controller.attach_load_cell(load_cell_joint) +``` +To allow the LoadCellJoint to get data from the physics engine, attach an instance of `SharedJoint` named 'shared_joint' through `load_cell_joint.attach_sim_source(shared_joint)`. + +Now, the attach the LoadCellJoint to the HITL controller through `hitl_controller.attach_load_cell(load_cell_joint)`. Now, the controller will actually know there is a sensor that needs to be output. + + +### LoadCellSpring +A load cell can also be attached to a [Spring](#spring) with LoadCellSpring. This is much more representative of a load cell in the real world as it only outputs a force along a vector (defined as the spring vector $\vec r$). + +Create and attach a LoadCellSpring with +``` +load_cell_Spring = LoadCellSpring(sensor_settings: dict = { + "name": str, + "mV/V": float, + "excitation": float, + "full_scale_force": float, # What is the max force on the load cell + "output_channel": int # The output channel that the load cell outputs through the MCC3114. Must be between [0, 15] +}) +load_cell_spring.attach_sim_source(shared_spring) + +hitl_controller.attach_load_cell(load_cell_spring) +``` +To allow the LoadCellSpring to get data from the physics engine, attach an instance of `SharedSpring` named 'shared_spring' through `load_cell_spring.attach_sim_source(shared_spring)`. + +Now, the attach the LoadCellSpring to the HITL controller through `hitl_controller.attach_load_cell(load_cell_spring)`. Now, the controller will actually know there is a sensor that needs to be output. + +### DisplacementSensor +A position/displacement sensor can be attached to any two joints with DisplacementSensor. This allows for a scalar length between the joints to be output through the MCC3114. + +Create and attach a DisplacementSensor with +``` +displacement_sensor = DisplacementSensor( + parent_joint: SharedJoint, # Must be a SharedJoint for the sensor to get values from the sim + child_joint: SharedJoint, # Must be a SharedJoint for the sensor to get values from the sim + sensor_settings: dict = { + "name": str, + "neutral_length": float, # This is the 0 voltage value. Can be 0 for purely positive voltage output or non-zero to allow negative voltage output + "volts_per_meter": float, # How many volts per meter of length should the MCC3114 output + "output_channel": int # The output channel that the load cell outputs through the MCC3114. Must be between [0, 15] + } +) +hitl_controller.attach_displacement_sensor(displacement_sensor) +``` +The DisplacementSensor gets data from the SharedJoints that are specified as a parent_joint and child_joint. The axis vector of the displacement sensor is given by $\vec r = \vec x_{child} - \vec x_{parent}$. For a scalar sensor, this plays no importance. Length is the vector length of $\vec r$. + +Now, the attach the DisplacementSensor to the HITL controller through `hitl_controller.attach_displacement_sensor(displacement_sensor)`. Now, the controller will actually know there is a sensor that needs to be output. + + +## Controller Elements +Controller elements are located in [code/Controller_Input_Elements.py](code/Controller_Input_Elements.py). Controller elements are how a real-world controller can control something inside of the physics simulation. + +Because the physics simulation and HITL controller processes are separate, you must create a shared object to link them. The only shared objects for a controller element is `SharedRigidActuator` which links the controller to the physics simulation so it can control the actuator. + +In reference to the data sharing paths in the process diagram, the hitl controller pushes the commanded attribute (velocity or position) to the SharedRigidActuator and the physics sim pulls the commanded attribute. + +With the MCC202, the range is [-10, +10] V. Any exceeding past this range can cause damage to the device. Digital inputs operate on [0, 5] V TTL and should not experience voltage below 0 V or above 5.5 V to avoid damage. + +### RigidActuatorController +A [RigidActuator](#rigidactuator) in the physics simulation is useless without a RigidActuatorController to move it. As a reminder, a [RigidActuator](#rigidactuator) connects 2 joints: the grounded and the parent joint. The grounded joint will never move. The parent joint can be controlled along the axis of the actuator. The user can specify whether the controller commands the position or the velocity of the parent joint with respect to the grounded joint. + +Create and attach a RigidActuatorController with +``` +rigid_actuator_controller = RigidActuatorController(controller_settings: dict = { + "name": str, + "analog_input_channel": int, # The analog channel on the MCC202 that controls the rigid actuator. Must be between [0, 7] + "digital_input_channel": int, # The digital channel on the MCC202 that enables control of the rigid actuator. Must be between [0, 7] + "units_per_volt": float, # m/V or m/s/V. How far or how fast the actuator should move per volt + "neutral_length": float, # Length to go to when given neutral voltage (position control). Also used for starting + "neutral_voltage": float, # The voltage at which to go to neutral_length (position control) or zero velocity (velocity control) + "controls_pos/vel/accel": int, # 0 = controls pos, 1 = controls vel, 2 = controls accel + "max_length": float, + "min_lenght": float +}) +rigid_actuator_controller.attach_sim_target(shared_rigid_actuator) + +hitl_controller.attach_rigid_actuator_controller(rigid_actuator_controller) +``` +To allow the RigidActuatorController to command the physics engine to respond to real-life inputs, attach an instance of `SharedRigidActuator` named 'shared_rigid_actuator' with `rigid_actuator_controller.attach_sim_target(shared_rigid_actuator)`. + +Now, the attach the RigidActuatorController to the HITL controller through `hitl_controller.attach_rigid_actuator_controller(rigid_actuator_controller)`. Now, the controller will actually know there is a rigid actuator that it needs to get inputs for and command. + + +## Sharing Elements +Sharing elements refers to the data flow of object between processes using pipes. This is largely hidden from the user as multiprocessing.managers.BaseManager does this for us. It creates AutoProxy elements of each element we "share" and when we "attach" them to an instance of a class, they are given access to the AutoProxy object. The processes that have access to the AutoProxy objects can interact with them in 1 fashion: function calls. Direct access to internal attributes is not possible without a copy of the object. So, there are numerous getters and setters for each "shared" object for data retrieval and setting. + +For the most part, sharing elements is for allowing sensor and controller elements to communicate with the elements in the physics simulation. A [LoadCell](#loadcell) needs a [SharedJoint](#sharedjoint) or a [SharedSpring](#sharedspring), a [DisplacementSensor](#displacementsensor) needs 2 [SharedJoints](#sharedjoint), a [RigidActuatorController](#rigidactuatorcontroller) needs a [SharedRigidActuator](#sharedrigidactuator). A sensor element will be connected by attaching a shared element to the sensor through "attach_sim_source". A controller element will be connected by attaching a shared element to the controller through "attach_sim_target". + +Any element can also be plotted and some can be 3D visualized. Those elements will also need a shared element. This is usually taken care of by attaching the shared element to the base element through "attach_shared_attributes". + +### BaseManager +The BaseManager is how elements can be shared amongst processes. + +Here is the code to make an instance of the BaseManager and register classes to be shared +``` +from multiprocessing.managers import BaseManager +base_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('SharedDisplacementSensor', SharedDisplacementSensor) + +BaseManager.register('SharedRigidActuatorController', SharedRigidActuatorController) + +base_manager.start() +``` +After the instance is created, each class that needs to be shared needs to be registered. When registering classes, the objects must be 'picklable'. I have never run into an instance where I could not share an object except for when the object contained a lambda function. + +Each of the shared elements is setup to have strong similarity with each other and the thing they are representing. They are also supposed to do as little calculation as possible. In a best scenario, they only have getters and setters. + + +### SharedJoint +A SharedJoint is needed to share the attributes of a [Joint](#joint) if it is connected to one of the following: + +1. [Visualization](#visualization) +2. [Plotter](#plotter) +3. [LoadCellJoint](#loadcelljoint) +4. [DisplacementSensor](#displacementsensor) + +Here is the code to make and attach a SharedJoint to a [Joint](#joint) +``` +base_manager = BaseManager() +BaseManager.register('SharedJoint', SharedJoint) # Register SharedJoint under the name 'SharedJoint' +base_manager.start() + +... + +joint = Joint(...) # Previously defined Joint +shared_joint: SharedJoint = base_manager.SharedJoint() + +joint.attach_shared_attributes(shared_joint) # Point the Joint to push physics values to the SharedJoint + +...Attach shared_joint to 3D visualization, plotter +``` + + +### SharedSpring +A SharedSpring is needed to share the attributes of a [Spring](#spring) if it is connected to one of the following: + +1. [Visualization](#visualization) +2. [Plotter](#plotter) +3. [LoadCellSpring](#loadcellspring) + +Here is the code to make and attach a SharedSpring to a [Spring](#spring) +``` +base_manager = BaseManager() +BaseManager.register('SharedSpring', SharedSpring) # Register SharedSpring under the name 'SharedSpring' +base_manager.start() + +... + +spring = Spring(...) # Previously defined Spring +shared_spring: SharedSpring = base_manager.SharedSpring() + +spring.attach_shared_attributes(shared_spring) # Point the Spring to push physics values to the SharedSpring + +...Attach shared_spring to 3D visualization, plotter +``` + + +### SharedRigidActuator +A SharedRigidActuator is needed to share the attributes of a [RigidActuator](#rigidactuator) if it is connected to one of the following: + +1. [Plotter](#plotter) +2. [RigidActuatorController](#rigidactuatorcontroller) + +This means that a SharedRigidActuator is required for the normal use of a [RigidActuator](#rigidactuator) so that the [HITLController](#hitlcontroller-hardware-in-the-loop-controller) can push values to the physics simulation. + +Here is the code to make and attach a SharedRigidActuator to a [RigidActuator](#rigidactuator) +``` +base_manager = BaseManager() +BaseManager.register('SharedRigidActuator', SharedRigidActuator) # Register SharedRigidActuator under the name 'SharedRigidActuator' +base_manager.start() + +hitl_controller = HITLController(...) # Previously defined HITLController + +... + +rigid_actuator = RigidActuator(...) # Previously defined RigidActuator +shared_rigid_actuator: SharedRigidActuator = base_manager.SharedRigidActuator() + +rigid_actuator.attach_shared_attributes(shared_rigid_actuator) # Point the RigidActuator to pull conrolled values from the SharedRigidActuator + +rigid_actuator_controller = RigidActuatorController(...) # Defined RigidActuatorController + +rigid_actuator_controller.attach_sim_target(shared_rigid_actuator) # Point the controller to the SharedRigidActuator + +hitl_controller.attach_rigid_actuator_controller(rigid_actuator_controller) # Attach a RigidActuatorController to the HITLController + +...Attach shared_rigid_actuator to plotter +``` + + +### SharedLoadCellJoint +A SharedLoadCellJoint is needed to share the attributes of a [LoadCellJoint](#loadcelljoint) if it is connected to one of the following: + +1. [Plotter](#plotter) + +Here is the code to make and attach a SharedLoadCellJoint to a [LoadCellJoint](#loadcelljoint) +``` +base_manager = BaseManager() +BaseManager.register('SharedLoadCellJoint', SharedLoadCellJoint) # Register SharedLoadCellJoint under the name 'SharedLoadCellJoint' +base_manager.start() + +hitl_controller = HITLController(...) # Previously defined HITLController + +... +joint = Joint(...) # Previously defined Joint +shared_joint: SharedJoint = base_manager.SharedJoint() + +load_cell_joint = LoadCellJoint(...) # Previously defined LoadCellJoint +load_cell_joint.attach_sim_source(shared_joint) # Point the LoadCellJoint to pull physics values from the SharedJoint + +shared_load_cell_joint: SharedLoadCellJoint = base_manager.SharedLoadCellJoint() + +load_cell_joint.attach_shared_attributes(shared_load_cell_joint) # Point the LoadCellJoint to push values to the SharedLoadCellJoint + +hitl_controller.attach_load_cell(load_cell_joint) # Attach a LoadCellJoint to the HITLController + +...Attach shared_load_cell_joint to plotter +``` + + +### SharedLoadCellSpring +A SharedLoadCellSpring is needed to share the attributes of a [LoadCellSpring](#loadcellspring) if it is connected to one of the following: + +1. [Plotter](#plotter) + +Here is the code to make and attach a SharedLoadCellSpring to a [LoadCellSpring](#loadcellspring) +``` +base_manager = BaseManager() +BaseManager.register('SharedLoadCellSpring', SharedLoadCellSpring) # Register SharedLoadCellSpring under the name 'SharedLoadCellSpring' +base_manager.start() + +hitl_controller = HITLController(...) # Previously defined HITLController + +... +spring = Spring(...) # Previously defined Spring +shared_spring: SharedJoint = base_manager.SharedSpring() + +load_cell_spring = LoadCellSpring(...) # Previously defined LoadCellSpring +load_cell_spring.attach_sim_source(shared_spring) # Point the LoadCellSpring to pull physics values from the SharedSpring + +shared_load_cell_spring: SharedLoadCellSpring = base_manager.SharedLoadCellSpring() + +load_cell_spring.attach_shared_attributes(shared_load_cell_spring) # Point the LoadCellSpring to push values to the SharedLoadCellSpring + +hitl_controller.attach_load_cell(load_cell_spring) # Attach a LoadCellSpring to the HITLController + +...Attach shared_load_cell_spring to plotter +``` + + +### SharedDisplacementSensor +A SharedDisplacementSensor is needed to share the attributes of a [DisplacementSensor](#displacementsensor) if it is connected to one of the following: + +1. [Plotter](#plotter) + +Here is the code to make and attach a SharedDisplacementSensor to a [DisplacementSensor](#displacementsensor) +``` +base_manager = BaseManager() +BaseManager.register('SharedDisplacementSensor', SharedDisplacementSensor) +base_manager.start() + +... +displacement_sensor = DisplacementSensor(...) # Previously defined DisplacementSensor +shared_displacement_sensor: SharedDisplacementSensor = base_manager.SharedDisplacementSensor() + +displacement_sensor.attach_shared_attributes(shared_displacement_sensor) # Point the DisplacementSensor to push values to the SharedDisplacementSensor + +...Attach shared_displacement_sensor to plotter +``` + + +### SharedRigidActuatorController +A SharedRigidActuatorController is needed to share the attributes of a [RigidActuatorController](#rigidactuatorcontroller) if it is connected to one of the following: + +1. [Plotter](#plotter) + +Here is the code to make and attach a SharedRigidActuatorController to a [RigidActuatorController](#rigidactuatorcontroller) +``` +base_manager = BaseManager() +BaseManager.register('SharedRigidActuatorController', SharedRigidActuatorController) +base_manager.start() + +... +rigid_actuator_controller = RigidActuatorController(...) +shared_rigid_actuator_controller: SharedRigidActuatorController = base_manager.SharedRigidActuatorController() + +rigid_actuator_controller.attach_shared_attributes(shared_rigid_actuator_controller) + +...Attach shared_rigid_actuator_controller to plotter +``` + + +## BFSSimulation (Breadth First Search Simulation) +The BFSSimulation class does the physics engine work. Named 'BFS' due to how it searches for all physics elements by starting at a parent [Joint](#joint) and traversing in a BFS style to gather all children. + +The BFSSimulation class is located in [code/Simulation.py](code/Simulation.py). + +The physics engine does not need anything attached to it before the process start. When the BFSSimulation starts, it initializes itself by getting all joints connected to it and sorting them in ways for it perform the simulation well. + +``` +simulation = BFSSimulation( + parent_joint: Joint, # Main joint to start BFS on and get everything connected to the physics simulation + settings: dict = { + "duration": float # The total duration in seconds to run the simulation + "delta_t": float or None, # None for running in real-time. Specified in seconds to have a constant timestep + "plotting_update_period": float, # Period in seconds to update shared elements that are not connected to a sensor or controller + "sensor_update_period": float, # Period in seconds to update shared elements connected to sensors + "controller_pull_period": float # Period in seconds to pull values from shared elements for controlled elements (actuators) + } +) +shared_sim_time: SharedFloat = base_manager.SharedFloat() +simulation.attach_shared_time(shared_sim_time) +simulation_process = multiprocessing.Process(target=simulation.run_process) + +time.sleep(5) # Used so that the plotter, visualization, and controller processes can spool up +simulation_process.start() + # The simulation runs until it has done its full duration +simulation_process.join() +stop_event.set() # Used to tell the other processes there day has come +``` +The physics simulation is fairly easy to setup and start. As long as the physics elements are setup in a way that each object is reachable, it will handle itself. This does mean that there should be a clear chain from parent->child->child->child. + + +## HITLController (Hardware-in-the-Loop Controller) +The HITLController class handles the sensor output through the MCC3114 and controller input from MCC202. With these two devices, there are 16 analog [-10, +10] V inputs on the MCC3114, 8 analog [-10, +10] V inputs on the MCC202, and 8 digital 5 V TTL inputs on the MCC202. + +The HITLController class is located in [code/HITL_Controller.py](code/HITL_Controller.py). + +Here is the code to make a HITLController instance +``` +stop_event = multiprocessing.Event() + +hitl_controller = HITLController( + stop_event = multiprocessing.Event, # Provides a mean for the main process to stop the controller when the simulation is done + settings: dict = { + "pull_from_sim_period": float, # Period in milliseconds to pull values from the physics sim for sensor elements + "plotting_update_period": float # Period in milliseconds to update shared elements that are being plotted + } +) + +...Make controller elements + +hitl_controller.attach_load_cell(load_cell) # Attach either type of load cell +hitl_controller.attach_displacement_sensor(displacement_sensor) +hitl_controller.attach_rigid_actuator_controller(actuator_controller) + +... + +hitl_controller_process = multiprocessing.Process(target=hitl_controller.run_process) +hitl_controller_process.start() + +...Setting other processes and running the physics simulation + +stop_event.set() +hitl_controller_process.join() +``` + +### Adding a [LoadCellJoint](#loadcelljoint) or [LoadCellSpring](#loadcellspring) to a HITLController +Since the [LoadCellJoint](#loadcelljoint) and [LoadCellSpring](#loadcellspring) share the same parent class, [LoadCell](#loadcell), they are attached the same way. + +``` +hitl_controller = HITLController(...) # Previously defined HITLController + +... + +load_cell = LoadCell(...) # LoadCellJoint or LoadCellSpring() + +hitl_controller.attach_load_cell(load_cell) +``` + + +### Adding a [DisplacementSensor](#displacementsensor) to a HITLController +``` +hitl_controller = HITLController(...) # Previously defined HITLController + +... + +displacement_sensor = DisplacementSensor(...) + +hitl_controller.attach_displacement_sensor(displacement_sensor) +``` + + +### Adding a [RigidActuatorController](#rigidactuatorcontroller) to a HITLController +``` +hitl_controller = HITLController(...) # Previously defined HITLController + +... + +rigid_actuator_controller = RigidActuatorController(...) + +hitl_controller.attach_rigid_actuator_controller(rigid_actuator_controller) +``` + + +## Visualization +The Visualization class provides a wrapper of VPython such that it is easy for the user to quickly physics elements to be visualized. It handles only a few types of objects including joints as spheres, springs as helizes, and rigid actuators as cylinders. + +The Visualization class is located in [code/Visualization.py](code/Visualization.py). + +Here is the code to make a Visualization instance +``` +stop_event = multiprocessing.Event() +stop_event.clear() + +visualizer = Visualization( + stop_event=stop_event, + scene_settings: dict = { + "canvas_width": int, # Desired width of the screen in the browser + "canvas_height": int, # Desired height of the screen in the browser + "wall_thickness": float, # Thickness of the walls + "cube_size": float # Size of the cube surrounding the origin + } +) + +...Add elements to be visualized + +visualization_process = multiprocessing.Process(target=visualizer.run_process) +visualization_process.start() + +...Setting other processes and running the physics simulation + +stop_event.set() +visualization_process.join() +``` + +### Adding a [Joint](#joint) to a Visualization +Joints are spheres in the 3D visualization. + +``` +visualizer = Visualization(...) # Previously defined Visualization + +... + + +joint = Joint(...) # Previously defined Joint +shared_joint: SharedJoint = base_manager.SharedJoint() # Required to be visualized +joint.attach_shared_attributes(shared_joint) + +visualizer.attach_shared_attributes( + shared_attributes: shared_joint, + object_settings: dict = { + "type": "sphere", + "color": str, # "blue", "black", "red", "green", "white" + "radius": float + } +) +``` + + +### Adding a [Spring](#spring) to a Visualization +Springs are helixes in the 3D visualization. + +``` +visualizer = Visualization(...) # Previously defined Visualization + +... + + +spring = Spring(...) # Previously defined Spring +shared_spring: SharedSpring = base_manager.SharedSpring() # Required to be visualized +spring.attach_shared_attributes(shared_spring) + +visualizer.attach_shared_attributes( + shared_attributes: shared_spring, + object_settings: dict = { + "type": "helix", + "color": str, # "blue", "black", "red", "green", "white" + "radius": float, # Radius the wire is wrapped in a coil + "thickness": float # Similar to the gauge of the wire that is coiled + } +) +``` + + +### Adding a [RigidActuator](#rigidactuator) to a Visualization +RigidActuators are cylinders in the 3D visualization. + +``` +visualizer = Visualization(...) # Previously defined Visualization + +... + + +rigid_actuator = RigidActuator(...) # Previously defined RigidActuator +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: dict = { + "type": "cylinder", + "color": str, # "blue", "black", "red", "green", "white" + "radius": float + } +) +``` + + + +## Plotter +The Plotter class provides a wrapper of PyQtGraph such that it is easy for the user to quickly add any physics, sensor, or controller element to be plotted in real-time. It is typical to spawn multiple plotter processes for physics, sensor, and controller plotting to be separated. While this is not necessary, it allows the user to control the windows and be more flexible. + +The Plotter class is located in [code/Visualization.py](code/Visualization.py). + +Here is the code to make a Plotter instance +``` +stop_event = multiprocessing.Event() + +plotter = Plotter( + stop_event: multiprocessing.Event, # Provides a mean for the main process to stop the plotter when the simulation is done + plot_settings: dict = { + "title": str, + "xlabel": str, + "num_subplots": int, # Number of subplots to include in the window + "step_or_plot": str, # 'plot' for linear interpolation between points, 'step' for a step function between points + "subplots": list [ # A list of dicts for each subplot starting at index = 0 up to num_subplots - 1 + { + "ylabel": str + }, + ... for each subplot + ], + "window_length": float, # The total time window in seconds of data points to store at any given time + "pull_interval": float, # The number of milliseconds between pulling for datapoints + "update_interval": int, # The number of milliseconds between updating the graph + } +) +plotter.attach_shared_x(shared_float) + +...Adding elements to plot + +plotter_process = multiprocessing.Process(target=plotter.run_process) +plotter_process.start() + +...Setting other processes and running the physics simulation + +stop_event.set() +plotter_process.join() +``` +This creates a plotter object to add elements to plot. After adding all elements to plot, you can start the process. When the physics simulation process joins, you can set the stop_event and join the plotter_process. Note that for PyQtGraph, you must close out the plotter windows for the process to end. This is favorable so the user as the ability to export values and keep it up if desired. + +Note that a step plot has limited functionality such as lack of FF, dy/dx, and Y vs Y'. + +For each element you will see that there is a `plot_settings` dictionary when attaching to the plotter. This dictionary follows the same general structure for each element with the following rules: + +1. Everything except the ylabel must be in lowercase. +2. The ylabel must be unique between everything added to the same plotter. +3. The subplot integer must correspond to a subplot in range [0, num_subplots-1]. + +For general information on PyQtGraph, go to their [website](https://pyqtgraph.readthedocs.io/en/latest/user_guide/index.html). + + +### Adding a [Joint](#joint) to a Plotter +Adding a Joint to a plotter requires a [SharedJoint](#sharedjoint). + +``` +plotter = Plotter(...) + +... + +joint = Joint(...) +shared_joint: SharedJoint = base_manager.SharedJoint() +joint.attach_shared_attributes(shared_joint) + +plotter.attach_shared_attributes( + shared_attributes = shared_joint, + plot_settings: dict = { + "pos": { + "x": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "y": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "z": {...}, + "scalar": {...} + }, + "vel": {...}, + "accel": {...} + } +) + +``` +A Joint can have its position, velocity, and/or its acceleration plotted. Each of these can be specified between x, y, z, and scalar components. Not all of these have to be included. If it doesn't need to be plotted just don't include the key in the dictionary. + +### Adding a [Spring](#spring) to a Plotter +Adding a Spring to a plotter requires a [SharedSpring](#sharedspring). + +``` +plotter = Plotter(...) + +... + +spring = Spring(...) +shared_spring: SharedSpring = base_manager.SharedSpring() +spring.attach_shared_attributes(shared_spring) + +plotter.attach_shared_attributes( + shared_attributes = shared_spring, + plot_settings: dict = { + "force": { + "x": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "y": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "z": {...}, + "scalar": {...} + }, + "length": { + "scalar": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + } + } +) + +``` +A Spring can have its force plotted in x, y, z, and scalar components. It can also have its scalar length plotted. Not all of these have to be included. If it doesn't need to be plotted just don't include the key in the dictionary. + + +### Adding a [RigidActuator](#rigidactuator) to a Plotter +Adding a RigidActuator to a plotter requires a [SharedRigidActuator](#sharedrigidactuator). + +``` +plotter = Plotter(...) + +... + +rigid_actuator = RigidActuator(...) +shared_rigid_actuator: SharedRigidActuator = base_manager.SharedRigidActuator() +rigid_actuator.attach_shared_attributes(shared_rigid_actuator) + +plotter.attach_shared_attributes( + shared_attributes = shared_rigid_actuator, + plot_settings: dict = { + "pos": { + "x": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "y": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "z": {...}, + "scalar": {...} + }, + "vel": {...}, + "length": { + "scalar": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + } + } +) + +``` +A RigidActuator can have its position and/or velocity plotted in x, y, z, and scalar components. It can also have its scalar length plotted. Not all of these have to be included. If it doesn't need to be plotted just don't include the key in the dictionary. + + +### Adding a [LoadCellJoint](#loadcelljoint) to a Plotter +Adding a LoadCellJoint to a plotter requires a [SharedLoadCellJoint](#sharedloadcelljoint). + +``` +plotter = Plotter(...) + +... + +load_cell_joint = LoadCellJoint(...) +shared_load_cell_joint: SharedLoadCellJoint = base_manager.SharedLoadCellJoint() +load_cell_joint.attach_shared_attributes(shared_load_cell_joint) + +plotter.attach_shared_attributes( + shared_attributes = shared_load_cell_joint, + plot_settings: dict = { + "force": { + "x": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "y": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "z": {...}, + "scalar": {...} + }, + "true_voltage": { + "scalar": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + }, + "noisy_voltage": { + "scalar": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + } + } +) + +``` +A LoadCellJoint can have its force plotted in x, y, z, and scalar components. It can also have the voltage output from the sensor plotted. The voltage can be the true voltage directly from the force on the load cell or the noisy voltage after the true voltage has gone through noise generation. Not all of these have to be included. If it doesn't need to be plotted just don't include the key in the dictionary. + +### Adding a [LoadCellSpring](#loadcellspring) to a Plotter +Adding a LoadCellSpring to a plotter requires a [SharedLoadCellSpring](#sharedloadcellspring). + +``` +plotter = Plotter(...) + +... + +load_cell_spring = LoadCellJoint(...) +shared_load_cell_spring: SharedLoadCellSpring = base_manager.SharedLoadCellSpring() +load_cell_spring.attach_shared_attributes(shared_load_cell_spring) + +plotter.attach_shared_attributes( + shared_attributes = shared_load_cell_spring, + plot_settings: dict = { + "force": { + "x": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "y": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "z": {...}, + "scalar": {...} + }, + "true_voltage": { + "scalar": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + }, + "noisy_voltage": { + "scalar": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + } + } +) + +``` +A LoadCellSpring can have its force plotted in x, y, z, and scalar components. It can also have the voltage output from the sensor plotted. The voltage can be the true voltage directly from the force on the load cell or the noisy voltage after the true voltage has gone through noise generation. Not all of these have to be included. If it doesn't need to be plotted just don't include the key in the dictionary. + + +### Adding a [DisplacementSensor](#displacementsensor) to a Plotter +Adding a DisplacementSensor to a plotter requires 2 [SharedJoints](#sharedjoint). There is a parent joint and child joint. Displacement is defined as $\vec r = \vec x_{child} - \vec x_{parent}$ + +``` +plotter = Plotter(...) + +... + +parent_joint = Joint(...) +child_joint = Joint(...) + +shared_parent_joint: SharedJoint = base_manager.SharedJoint() +shared_child_joint: SharedJoint = base_manager.SharedJoint() + +parent_joint.attach_shared_attributes(shared_parent_joint) +child_joint.attach_shared_attributes(shared_child_joint) + +displacement_sensor = DisplacementSensor(...) +shared_displacement_sensor: SharedDisplacementSensor = base_manager.SharedDisplacementSensor() + +plotter.attach_shared_attributes( + shared_attributes = shared_displacement_sensor, + plot_settings: dict = { + "disp": { + "scalar": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + }, + "voltage": { + "noisy": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "true": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + } + } +) + +``` +A DisplacementSensor can have its displacement scalar plotted. It can also have the voltage output from the sensor plotted. The voltage can be the true voltage directly from the force on the load cell or the noisy voltage after the true voltage has gone through noise generation. Not all of these have to be included. If it doesn't need to be plotted just don't include the key in the dictionary. + + +### Adding a [RigidActuatorController](#rigidactuatorcontroller) to a Plotter +Adding a RigidActuatorController to a plotter requires a [SharedRigidActuatorController](#sharedrigidactuatorcontroller). + +``` +plotter = Plotter(...) + +... + +rigid_actuator_controller = RigidActuatorController(...) +shared_rigid_actuator_controller: SharedRigidActuatorController = base_manager.SharedRigidActuatorController() +rigid_actuator_controller.attach_shared_attributes(shared_rigid_actuator_controller) + +plotter.attach_shared_attributes( + shared_attributes = shared_rigid_actuator_controller, + plot_settings: dict = { + "pos": { + "x": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "y": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "z": {...}, + "scalar": {...} + }, + "vel": {...}, + "input_voltage": { + "scalar": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + }, + "digital_voltage": { + "scalar": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + } + } +) + +``` +A RigidActuatorController can have its position or velocity plotted in x, y, z, and scalar components. It can also have the voltage input and/or digital input plotted. Not all of these have to be included. If it doesn't need to be plotted just don't include the key in the dictionary. + + +# General Code Review +## Process Diagram and Data Sharing +In order to achieve real-time physics simulation in parallel with sensor and controller I/O, a complex process and threading structure was developed. Due to the Global Interpreter Lock of Python, multiprocessing is the primary means of parallelism. Individual processes can and do use threading. In a C++ or similar mid-level language, threading could be used to achieve the same effect that multiprocessing does without the complex data sharing paths. + +For a graphical view of the processes and threads, look in [documentation/code_design/Process_Diagram.drawio](documentation/code_design/Process_Diagram.drawio). There you will find when the processes are created, the main sequence of events they do when they are started, and what ends them. + + + +# The Physics Engine +The physics engine is a variable time-step Newtonian physics engine with a few noteworthy features. + +## Initialization +``` +class BFFSimulation(): + def __init__(self, parent_joint: Joint, settings:dict): + self.parent_joint = parent_joint + self.joints: List[Joint] = [] + self.springs: List[Spring] = [] + self.actuators: List[RigidActuator] = [] + self.rigid_actuators: List[RigidActuator] = [] + ... + self.get_joints_bfs() + ... + self.sort_shared_attributes_into_frequencies() +... +``` +`self.get_joints_bfs()` fills the lists of joints, springs, and actuators. This is directly used in the simulation to calculate attributes for physics modeling. + +`self.sort_shared_attributes_into_frequencies()` is used for updating shared attributes. Elements that are shared for visualization, sensors, and controllers might want to be updated at different frequencies. Updating shared attributes is cost heavy and should be minimized when possible. Therefore, we only update when we need to. + +## Acceleration and Integration Techniques +With the contraint of running in real-time, having an accurate yet computationally-inexpensive method for integrating acceleration into velocity and position is necessary. Errors arrive solely from the deviance of instantaneous acceleration at $t = t_0$ from average acceleration over the period $[t_0,t_1]$. If they were the same, then the physics simulation would need nothing better than simple integration. The problem arises when trying to generalize a solution for calculating the average acceleration rather than the straightforward instantaneous acceleration. Integration techniques help overcome the deviance and can provide other benefits such as energy-reducing systems rather than energy-gaining systems that tend to become extremely unstable. + +All of these integration techniques (with the slight exception of Predictor-Corrector) are implemented in StateSpace of [code/Physics_Elements.py](code/Physics_Elements.py). + +### Simple (Euler) Integration +``` +self.vel = self.vel + self.accel * delta_t +self.pos = self.pos + self.vel * delta_t +``` + +A tried and true method for integration. Works well with small delta_t. The cheapest method in terms of computation and memory cost. + +### Verlet Integration +``` +self.vel = self.vel + self.accel * 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 + ... +``` +Verlet integration is a multistep method that comes from the idea of finite differences. It is possible to calculate the instantaneous acceleration of an object using its past position values. Re-arranging the equation can yield an expression for the next position as a function of current instantaneous acceleration and the past positions. + +In order to calculate the coefficients for each term, you must solve a system of equations of the Taylor expansion about each point. A script to do this can be found in [code/useful_scripts/verlet_coefficients_calculator.py](code/useful_scripts/verlet_coefficients_calculator.py). + +This method has proven worse than simple integration for reasons unknown for now. Increasing the number of position points seems to make the error worse. The leading theory is that the past positions all have error from their instantaneous acceleration and any error continues to propagate rather than dissipate. It is worth noting that Verlet integration assumes a constant timestep which is not true for this simulation. + +How the integration method is setup now, the velocity and position are very loosely related and likely to drift from each other. This can become a signifant issue. There is also not a direct way for a controller to control the velocity of a joint and the position be automatically integrated. It is possible to perform the same matrix solver for calcuting the next velocity from past acceleration. + +### Adams-Bashforth Integration +``` +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]) + ... + self.pos = self.pos + self.vel * delta_t +``` +Adams-Bashforth integration is a multistep method similar to Verlet integration. However, it takes a more direct approach on working around the source of the error from the acceleration. Adams-Bashforth + +Adams-Bashforth integration has proven to do quite well while maintaining a fairly cheap computation and memory cost. 2 steps peforms similar to Euler integration while decaying the energy of the system. For that reason, it is default. 3 steps performs even better at maintaining energy. However, it tends to increase the total energy over time. Steps above 3 offer little to sometimes worse performance. + +It is worth noting that Adams-Bashforth integration assumes a constant timestep which is not true for this simulation. + +### Mixed Integration Technique +In order to prioritize certain joints that may experience larger peturbations or need a higher fidelity, it is possible to specify the integration technique upon [Joint](#joint) creation. Adams-Bashforth is the default, however, for simulations with large number of joints, it can be beneficial to set them to use simple integration. + +### Predictor-Corrector Methods for Acceleration (Not Implemented) +Re-iterating the issues that causes needs for better integration, we want to minimize the difference between the instantaneous acceleration that we apply as if it was the average acceleration over the entire timestep. Reducing timestep is a way to minimize the error propagated into velocity and position values. It is also possible to estimate better accelerations. Adams-Bashforth integration does this by weighting past accelerations to get a better idea of the average acceleration over the next timestep. + +Predictor-Corrector methods can come in many forms. Common ones are to calculate the instantaneous acceleration at points $x_0$ and $x_0 + \Delta x$ then average them to be applied over the timestep. Another variant is to calculate the acceleration at $x_0 + \frac{1}{2} \Delta x$. The new estimated acceleration is applied and integrated over the timestep to get the next velocity and position. + +It is also possible to iteratively run a predictor-corrector method until a finite error is achieved. By continuously integrating an acceleration, calculating new acceleration at the pseudo position, and looping until the changes in acceleration approach some value, the error can be controlled. However, for real-time systems, this is hard to be useful as the timestep increases at O(n) with n iterations and the accuracy of Euler integration is O($\Delta t^2$) for position. + + +### Mixed Predictor-Corrector Acceleration Technique (Not Implemented) +Tying back to mixed integration technique, applying predictor-corrector to only a handful of joint is a valid technique for minimizing the impact to timestep while getting better position estimates. For a completely generalized approach, it is possible for the simulation to decide which joints need more attention than others. By keeping track of factors such as last acceleration, current acceleration, and velocity, you can estimate how rapidly the calculated change in position would change if a more accurate acceleration was calculated. In a more definable method, a 1 step predictor-corrector method can be applied to every joint. For joints with large discrepancies in the predicted and corrected acclerations, more iterations can be completed. This has the benefit of minimizing the waste of timestep used to calculate accelerations that end up with diminishing returns. + +As an example, a center joint of large mass is surrounded in a circle by much smaller masses connected through springs. The larger mass would find that the variance between the initial acceleration and the corrected acceleration would be small due to the symmetrical properties of the springs and its large mass. On the other hand, the smaller masses would experience large differences in initial and corrected accelerations due to their single spring nature and smaller mass. + +General code would take the form +``` +real_statespace = list of all joint statespace +predicted_statespace = list of all joint statespace for use in correcting acceleration +corrected_statespace = list of all joint statespace after using corrected acceleration + +while self.sim_time < self.duration: + calculate predicted spring forces using real_statespace positions + calculate predicted force per spring and acceleration per spring at each joint using real_statespace positions + + integrate predicted_statespace starting at t_0 over 1/2 delta_t using predicted accelerations + + calculate corrected spring forces using predicted_statespace positions + calculate corrected force per spring and acceleration per spring at each joint using predicted_statespace positions + + for every joint: + for every spring connected to joint: + while (corrected acceleration - predicted acceleration) / corrected_acceleration is large: + integrate predicted_statespace starting at t_0 over 1/2 delta_t using corrected accelerations + + save previous corrected accelerations into predicted accelerations for comparison + + calculate corrected spring forces using predicted_statespace positions + calculate corrected force per spring and acceleration per spring at each joint using predicted_statespace positions + + set real_statespace acceleratiosn to corrected acceleration + + integrate real_statespace starting at t_0 over delta_t using corrected acceleration +``` +Because calculating the length of the springs is a large part of the time complexity, taking a shortcut could be beneficial during each iteration. Rather than recalculate the $\vec r$ for every spring at each iteration, assuming a constant $\vec r$ should suffice with such small changes. In addition, to recalculate the length at a correction step, one could take $\Delta l = l_0 + [(\vec v_{parent} - \vec v_{child})(\Delta t) + \frac{1}{2}(\vec a_{parent} - \vec a_{child})(\Delta t)^2]\cdot (\vec r)$ where parent and child refer to the parent joint and child joint of the spring. $\vec r_{spring}$ is defined as $\vec x_{child} - \vec x_{parent}$ and should not be updated in correct steps. + +Determining what is considered a large change in corrected vs predicted acceleration is abstract. It can be a pre-computed constant or a dynamic number that changes depending on the scenario. In situations where joints are close to each other (the length of the spring is small), what is considered a large acceleration should be smaller than if the distances were far apart. A simple method could be used to calculate what a large acceleration is. Assuming a constant acceleration, the error of change in position (error represented by $\delta$) due to error in acceleration is given as $\delta \Delta \vec x = \frac{1}{2}(\delta \vec a)(\Delta t)^2$. Since $\Delta t$ is known and $\delta \vec a$ was calculated, defining a large acceleration as some ratio $\frac{\delta \Delta \vec x}{l_{spring}}$ can be of use. + +A notable downfall of this method is in situations such as the large mass surrounded by smaller masses. The generalized solution would end up having to apply the iterative method to each joint due to the setup. + + +### Collision Dection +None :) + +Adding a collision detection would prove useful for dealing with runaway energy systems. However, it is computationally expensive, requiring distance calculations for n choose 2 or $(\frac{n}{2})$ which is $\frac{n^2 - n}{2}$. A more practical approach is to only consider collisions between joints with springs between them. This way the length is already computed. + +Once a collision takes place, performing an elastic collision between the 2 is trivial. Ideally, collision detection would not be a thing. In a more life-like simulation, there should be repulsive springs between every joint. This would also be $\frac{n^2 - n}{2}$ springs that are computed at all times. However, timesteps would currently be too large to be of usefulness. + + +# Examples +## 1D Mass-Spring Oscillator (No HITL Elements) +The example code in located in [code/_example_1d_mass_spring_oscillator.py](/code/_example_1d_mass_spring_oscillator.py) provides a simple getting started with Joints, Springs, the plotter, and visualizer. + +![3D Visualization of Mass-Spring Oscillator](documentation/examples/1d_mass_spring_oscillator/3D_Visualization.png) + +Above is the 3D Visualization before the physics simulation starts. The red sphere represents `main_mass` while the white helix on the left represents `spring`. + + +``` +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 +``` +We start with the necessary imports. + + +``` +'''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() +``` +We continue by setting up the BaseManager for object sharing. We need an instance of BaseManager and we need to register the objects we intend to share. Then we start it. + +``` +'''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 = {...} +) +mass_plotter.attach_shared_x(shared_sim_time) + +spring_plotter = Plotter( + stop_event = stop_event, + plot_settings = {...} +) +spring_plotter.attach_shared_x(shared_sim_time) + + +# Create a 3D visualization for the entire model +visualizer = Visualization( + stop_event = stop_event, + scene_settings = {...} +) +``` +This section creates instances for plotting and visualization. It starts by making an Event for simple communication between the processes. After physics simulation ends, the stop_event is set and the plotter and visualization processes will know to stop. There is also a SharedFloat. This is created for synchronization between physics simulation and plotters. It is initialized at t = 0.0. In this example, there is a dedicated plotter for the main_mass and the spring. This does not have to be this way. Multiple physics elements can be combined on the same plotter. Only a single visualization is made. + +``` +'''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" +) + +``` +These are all of the elements needed for the physics simulation. `main_mass` starts in the center raised by 5 meters. `wall_joint` is created so that there is a second joint for the spring to be attached to. It is invisible (because it was not added to the 3D visualizer) and is on the left wall. It is required for `fixed = True`. Finally the spring is made. Please note that `parent_joint = main_mass`. When we create the BFSSimulation, we specifiy `main_mass` as the parent_joint for the physics simulation. Because of how we defined `spring`, the breadth-first-search will see the main_mass, then the spring, then the wall_joint. If the spring had its parent be the wall_joint, the physics simulation would never see the wall_joint. + + +``` +'''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 = {...} +) + +spring_plotter.attach_shared_attributes( + shared_attributes = shared_spring, + plot_settings = {...} +) +``` +We first create and attach the shared objects so that data can be passed from the physics simulation to the plotters. Then we attach them to the plotter with plot settings. + + +``` +'''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 = {...} +) + +visualizer.attach_shared_attributes( + shared_attributes = shared_spring, + object_settings = {...} +) +``` +Because the shared objects are already created, we don't need to create them again. So all we need to do is attach them to the visualization object with the object settings. + + +``` +'''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 = {...} +) +simulation.attach_shared_time(shared_sim_time) +``` +Like mentioned, specifiying `parent_joint = main_mass` provides a mean for the BFS to reach every element in the we created and linked together. We then attach the SharedFloat we created to synchronize the time bewteen the processes. + + +``` +'''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() +``` +For each plotter, visualizer, and physics simulation, we create a process using the built in `run_process()` function. Then we start the plotters and visualizers so that they can launch their windows and be ready for when the physics simulation starts. We sleep for 5 seconds and then start the simulation process. This will run until the duration is up and then join back in the main code. We join the rest of the processes and shutdown the base_manager to exit the program. + +## 1D Mass-Spring Oscillator with Actuator + + +## 2D Mass-Spring Oscillator with 2 Actuators + + +## Stage 1 Hanging Mass + +# Debugging +For a graphical view of the processes and threads, look in [documentation/code_design/Process_Diagram.drawio](documentation/code_design/Process_Diagram.drawio). + +## Not Plotting Correctly? +### Plotter Window Doesn't Spawn? +Make sure you made the Plotter instance, make a process for it, and started it. + +### Plotter Comes Up But Doesn't Show Lines? +Make sure you made a `Shared______` and attached it to the plotter AND the physics engine source correctly. + +### Plotting Some Parts But Not All? +This is likely a problem in Plotter.pull_data(). There is a chain of `if-elif` statements that check for every type of `Shared______`. For example, if a Joint is not being plotted correctly, go to `elif isinstance(foo, SharedJoint):`. It is possible there is a missed attribute that needs to be considered and handled correctly. + +Make sure you gave the `ylabel` a unique name. + +## Not Visualizing Correctly? +### Visualization Doesn't Spawn? +The Visualizer spwawns in your default browser. Make sure it isn't spawning in a browser you didn't know was open. + +Make sure you made the Visualizer instance, make a process for it, and started it. +### Visualization Doesn't Show an Object? +Make sure you made a `Shared______` and attached it to the visualizer correctly. + +Make sure the physics engine source (Joint, Spring, RigidActuator) has a unique name. \ No newline at end of file diff --git a/code/.vscode/launch.json b/code/.vscode/launch.json new file mode 100644 index 0000000..23d0afc --- /dev/null +++ b/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" + ] + } + ] +} \ No newline at end of file diff --git a/code/.vscode/settings.json b/code/.vscode/settings.json new file mode 100644 index 0000000..2c39ae2 --- /dev/null +++ b/code/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "python.REPL.enableREPLSmartSend": false +} \ No newline at end of file diff --git a/code/Controller_Input_Elements.py b/code/Controller_Input_Elements.py new file mode 100644 index 0000000..a7602cb --- /dev/null +++ b/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 diff --git a/code/HITL_Controller.py b/code/HITL_Controller.py new file mode 100644 index 0000000..ef22713 --- /dev/null +++ b/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() + diff --git a/code/MCC_Interface.py b/code/MCC_Interface.py new file mode 100644 index 0000000..fdb7768 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/code/Object_Sharing.py b/code/Object_Sharing.py new file mode 100644 index 0000000..f601779 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/code/Physics_Elements.py b/code/Physics_Elements.py new file mode 100644 index 0000000..45c3113 --- /dev/null +++ b/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 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 \ No newline at end of file diff --git a/code/Sensor_Elements.py b/code/Sensor_Elements.py new file mode 100644 index 0000000..ca43ff1 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/code/Simulation.py b/code/Simulation.py new file mode 100644 index 0000000..0653f51 --- /dev/null +++ b/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}") \ No newline at end of file diff --git a/code/Visualization.py b/code/Visualization.py new file mode 100644 index 0000000..30bce3e --- /dev/null +++ b/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() + \ No newline at end of file diff --git a/code/__pycache__/Controller_Input_Elements.cpython-311.pyc b/code/__pycache__/Controller_Input_Elements.cpython-311.pyc new file mode 100644 index 0000000..2b06845 Binary files /dev/null and b/code/__pycache__/Controller_Input_Elements.cpython-311.pyc differ diff --git a/code/__pycache__/Controller_Interface.cpython-311.pyc b/code/__pycache__/Controller_Interface.cpython-311.pyc new file mode 100644 index 0000000..c28b7e8 Binary files /dev/null and b/code/__pycache__/Controller_Interface.cpython-311.pyc differ diff --git a/code/__pycache__/HITL_Controller.cpython-311.pyc b/code/__pycache__/HITL_Controller.cpython-311.pyc new file mode 100644 index 0000000..9e25c67 Binary files /dev/null and b/code/__pycache__/HITL_Controller.cpython-311.pyc differ diff --git a/code/__pycache__/MCC_Interface.cpython-311.pyc b/code/__pycache__/MCC_Interface.cpython-311.pyc new file mode 100644 index 0000000..ab442ee Binary files /dev/null and b/code/__pycache__/MCC_Interface.cpython-311.pyc differ diff --git a/code/__pycache__/Object_Sharing.cpython-311.pyc b/code/__pycache__/Object_Sharing.cpython-311.pyc new file mode 100644 index 0000000..62aa12d Binary files /dev/null and b/code/__pycache__/Object_Sharing.cpython-311.pyc differ diff --git a/code/__pycache__/PhysicsElements.cpython-311.pyc b/code/__pycache__/PhysicsElements.cpython-311.pyc new file mode 100644 index 0000000..cd9470a Binary files /dev/null and b/code/__pycache__/PhysicsElements.cpython-311.pyc differ diff --git a/code/__pycache__/Physics_Elements.cpython-311.pyc b/code/__pycache__/Physics_Elements.cpython-311.pyc new file mode 100644 index 0000000..21449b2 Binary files /dev/null and b/code/__pycache__/Physics_Elements.cpython-311.pyc differ diff --git a/code/__pycache__/Sensor_Elements.cpython-311.pyc b/code/__pycache__/Sensor_Elements.cpython-311.pyc new file mode 100644 index 0000000..f0d7b7c Binary files /dev/null and b/code/__pycache__/Sensor_Elements.cpython-311.pyc differ diff --git a/code/__pycache__/Simulation.cpython-311.pyc b/code/__pycache__/Simulation.cpython-311.pyc new file mode 100644 index 0000000..f643ada Binary files /dev/null and b/code/__pycache__/Simulation.cpython-311.pyc differ diff --git a/code/__pycache__/Visualization.cpython-311.pyc b/code/__pycache__/Visualization.cpython-311.pyc new file mode 100644 index 0000000..0c7de3b Binary files /dev/null and b/code/__pycache__/Visualization.cpython-311.pyc differ diff --git a/code/_example_1d_mass_spring_oscillator.py b/code/_example_1d_mass_spring_oscillator.py new file mode 100644 index 0000000..14da762 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/code/_example_1d_mass_spring_oscillator_with_actuator.py b/code/_example_1d_mass_spring_oscillator_with_actuator.py new file mode 100644 index 0000000..b976c4f --- /dev/null +++ b/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() \ No newline at end of file diff --git a/code/_example_2d_mass_spring_oscillator_with_actuators.py b/code/_example_2d_mass_spring_oscillator_with_actuators.py new file mode 100644 index 0000000..1c045f0 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/code/_example_stage_1_test.py b/code/_example_stage_1_test.py new file mode 100644 index 0000000..11d1e7a --- /dev/null +++ b/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() + diff --git a/code/deprecated/1d_Mass_Spring.py b/code/deprecated/1d_Mass_Spring.py new file mode 100644 index 0000000..0b56c06 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/code/deprecated/Generalized_1d Mass_Spring_Damper_old.py b/code/deprecated/Generalized_1d Mass_Spring_Damper_old.py new file mode 100644 index 0000000..2be8fc6 --- /dev/null +++ b/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() + \ No newline at end of file diff --git a/code/deprecated/is_picklable.py b/code/deprecated/is_picklable.py new file mode 100644 index 0000000..bcf637f --- /dev/null +++ b/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)) \ No newline at end of file diff --git a/code/deprecated/live_plotter_test.py b/code/deprecated/live_plotter_test.py new file mode 100644 index 0000000..7a9c660 --- /dev/null +++ b/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() diff --git a/code/deprecated/mcc_usb1024_test.py b/code/deprecated/mcc_usb1024_test.py new file mode 100644 index 0000000..02fb52e --- /dev/null +++ b/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() diff --git a/code/deprecated/pstats_reader.py b/code/deprecated/pstats_reader.py new file mode 100644 index 0000000..c4319ec --- /dev/null +++ b/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) \ No newline at end of file diff --git a/code/deprecated/pyqtgraph_test.py b/code/deprecated/pyqtgraph_test.py new file mode 100644 index 0000000..42dca58 --- /dev/null +++ b/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() diff --git a/code/deprecated/shared_memory.py b/code/deprecated/shared_memory.py new file mode 100644 index 0000000..e85df8c --- /dev/null +++ b/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() diff --git a/code/deprecated/tests/.vscode/settings.json b/code/deprecated/tests/.vscode/settings.json new file mode 100644 index 0000000..2c39ae2 --- /dev/null +++ b/code/deprecated/tests/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "python.REPL.enableREPLSmartSend": false +} \ No newline at end of file diff --git a/code/deprecated/tests/__pycache__/Object_Sharing.cpython-311.pyc b/code/deprecated/tests/__pycache__/Object_Sharing.cpython-311.pyc new file mode 100644 index 0000000..bb0ee96 Binary files /dev/null and b/code/deprecated/tests/__pycache__/Object_Sharing.cpython-311.pyc differ diff --git a/code/deprecated/tests/__pycache__/PhysicsElements.cpython-311.pyc b/code/deprecated/tests/__pycache__/PhysicsElements.cpython-311.pyc new file mode 100644 index 0000000..269ab88 Binary files /dev/null and b/code/deprecated/tests/__pycache__/PhysicsElements.cpython-311.pyc differ diff --git a/code/deprecated/tests/__pycache__/SimLibrary.cpython-311.pyc b/code/deprecated/tests/__pycache__/SimLibrary.cpython-311.pyc new file mode 100644 index 0000000..4a69ae3 Binary files /dev/null and b/code/deprecated/tests/__pycache__/SimLibrary.cpython-311.pyc differ diff --git a/code/deprecated/tests/__pycache__/Simulation.cpython-311.pyc b/code/deprecated/tests/__pycache__/Simulation.cpython-311.pyc new file mode 100644 index 0000000..16d880d Binary files /dev/null and b/code/deprecated/tests/__pycache__/Simulation.cpython-311.pyc differ diff --git a/code/deprecated/tests/__pycache__/Visualization.cpython-311.pyc b/code/deprecated/tests/__pycache__/Visualization.cpython-311.pyc new file mode 100644 index 0000000..0069429 Binary files /dev/null and b/code/deprecated/tests/__pycache__/Visualization.cpython-311.pyc differ diff --git a/code/deprecated/tests/__pycache__/pstats.cpython-311.pyc b/code/deprecated/tests/__pycache__/pstats.cpython-311.pyc new file mode 100644 index 0000000..b4e8b79 Binary files /dev/null and b/code/deprecated/tests/__pycache__/pstats.cpython-311.pyc differ diff --git a/code/setup/README.md b/code/setup/README.md new file mode 100644 index 0000000..f32fb04 --- /dev/null +++ b/code/setup/README.md @@ -0,0 +1,1386 @@ +# Initial Understanding +This project is meant to provide a simple test bed for development and learning of an RMC controller. +Specifically, the project is aimed for modeling hydraulic actuators interfacing with ropes and masses. + +The project provides a physics simulation for general purpose, 'similar-to-life', physics scenarios so that equations of motion +do not need to be derived for a given scenario. The project also incorporates the interfaces required for interacting with real-world devices. +It utilizes the Measurement Computing Company (MCC) [USB-202](https://files.digilent.com/manuals/USB-202.pdf) for analog and digital input and the MCC [USB-3114](https://files.digilent.com/manuals/USB-3114.pdf) for analog outputs. + +To provide a visualization for the user, there is a 3D visualization engine built using [VPython](https://vpython.org/). In addition, there is a wrapper for PyQtGraph so that +real-time data can be plotted for physics, sensors, HITL controller, or any combination. + +# Initial Setup +## Required Software Installation +1. Install `Python 3.11.4 through Company Portal`. This ensure the global configuration is set correctly for installing packages. +2. Install the Measurement Computing Universal Library for Windows with [code/setup/mccdaq.exe](code/setup/mccdaq.exe). DAQami does NOT need to be installed. Only `InstaCal & Universal Library`. +3. Run the Python dependecy installer by executing `python code/setup/dependencies.py`. This includes VPython, PyQtGraph, numpy, typing, and mcculw. vpython and PyQtGraph are significantly larger than the others. They are used for 3D visualization and plotting respectively. numpy is used for faster 'C-like' vectors. typing is for type hints in the code. mcculw is for interfacing with the MCC devices. + +[code/setup/icalsetup.exe](code/setup/icalsetup.exe) is used for InstaCal installation. This is not required as Python performs the device configuration for MCC3114 and MCC202. InstaCal will create a configuration file in `C:/ProgramData/Measurement Computing/DAQ` called `CB.CFG`. This file is created when you configure the MCC devices. Note, 1 configuration file is made for the entire system of devices. It is not per device. The Python code in [code/MCC_Inferface](code/MCC_Interface.py) configures the devices as necessary upon initialization. Therefore, a manual configuration through InstaCal is not needed. + + + +# Setting Up a HITL Simulation +It is strongly recommended to go through this guide with an example up. Before writing the code to setup a HITL simulation, have a clear idea of what joints you will need along with the springs and actuators. Each object will need specific information about how to set it up such as a joint's position, springs characteristics, and an actuator's control behavior. + +## Physics Elements +Controller elements are located in [code/Physics_Elements.py](code/Physics_Elements.py). There are 3 main physics elements. [joints](#joint), [springs](#spring), and [rigid actuators](#rigidactuator). [StateSpace](#statespace) is also included as a physics element. + +### StateSpace +StateSpace is where the Newtonian physics takes place. Each [Joint](#joint) is assigned a StateSpace at initialization. As the simulation runs, the StateSpace will be integrated over its acceleration and velocity to function as the physics engine. + +The user will never need to create a StateSpace manually. It is always handled through the creation of a [Joint](#joint). Understanding the basic aspects of the StateSpace can help understand behavior of the physics simulation. A simple integration method is shown. This is an abstraction of what StateSpace does. +``` +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 + + def integrate(self, delta_t: float): + self.vel = self.vel + self.accel * delta_t + self.pos = self.pos + self.vel * delta_t +``` +By default a StateSpace is initialized with all zeroes. It is typical to assign a position at [Joint](#joint) creation which is passed to the StateSpace. integration_method is passed from the creation of a [Joint](#joint) to specify the type of integration to use. This allows for a mix of speed and accuracy. + +### Joint +A Joint is a single point in space with mass. It has its own StateSpace. Joints can either be fixed in space or free (they can also be driven by an actuator which is a special case handled during creation of an actuator). + +Here is the code to create a Joint +``` +joint = Joint( + pos: array_like, # The initial position of the joint. typically use np.array([x, y, z]) + mass: float, # The mass in kg of the joint. Disregarded if fixed == True + fixed: bool, # If true, the Joint will never move. + name: str, + integration_method: str # "simple" (default), "verlet", "adams-bashforth" +) +``` +Now that the joint is created, it can be connected to a spring, actuator, or nothing if it is fixed. + +#### Joint Extra Attributes +The Joint class has a few extra functions that are useful for certain use. + +``` +joint.add_gravity() # Add gravity at 9.81 m/s^2 + +joint.add_damping( # Add either velocity or momentum damping. If mom_ratio is specified, disregard vel_ratio + vel_ratio: float, # Add a force equal to the negative velocity in m/s scaled by vel_ratio + mom_ratio: float # Add a force equal to the negative momentum in kg*m/s scaled by mom_ratio +) + +joint.add_constant_force( # Add a constant force to be applied to the joint + new_force: array_like # Typically use np.array([x, y, z]) +) + +joint.add_variable_force( # Add a time varying force in the form of a function pointer (NOT lambda function) to be applied to the joint + force_vect: array_like # Typically use np.array([f1, f2, f3]) where f = func1(t:float) +) +``` + +### Spring +A Spring connects 2 Joints and applies a force on them. Springs use the StateSpace of the parent [Joint](#joint) and child [Joint](#joint) attached to them. They calculate their $\vec r = \vec x_{child} - \vec x_{parent}$ and $\Delta l$ to get their force vector. Tension is treated as positive in all cases. + +Here is the code to create a Spring +``` +spring = Spring( + parent_joint: Spring, + child_joint: Spring, + unstretched_length: float, # Zero force length + contant_stiffness: float, # Constant k value. Ignored if stiffness_func != None + stiffness_func: func(length: float, unstretched_length) -> float, # Function pointer for variable stiffness + name: str +) +``` +Now that the Spring is created, it will be simulated in the physics simulation. The physics simulation knows it exists because each [Joint](#joint) keeps track of the springs attached to it. + +The Spring class has no other attributes for the user to work with. + + +### RigidActuator +A RigidActuator connects 2 Joints, a "grounded" joint and a "parent" joint. The grounded [Joint](#joint) is fixed and will not move. The parent joint is controlled in relation to the grounded joint. All RigidActuators must be controlled by something. They will either control the length of the actuator or the velocity. The RigidActuator will only move in the direction of the $\vec r = \vec x_{grounded} - \vec x_{parent}$. + +Here is the code to create a RigidActuator +``` +rigid_actuator = RigidActuator( + parent_joint: Joint, + grounded_joint: Joint, + name: str, + max_length: float, + min_length: float, + control_code: int # 0 for displacement control, 1 for velocity control, 2 for acceleration control +) +``` +Now that the RigidActuator is created, it will be simulated in the physics simulation. The physics simulation knows it exists because each [Joint](#joint) keeps track of the actuators attached to it. + +The RigidActuator class has no other attributes for the user to work with. + + +## Sensor Elements +Sensor elements are located in [code/Sensor_Elements.py](code/Sensor_Elements.py). Sensor elements are how you take something from the physics simulation and bring it to the real-world with voltage outputs. + +Because the physics simulation and HITL controller processes are separate, you must create a shared object to link them. These shared objects are SharedJoint and SharedSpring. They will need to be attached to both the raw physics element ([Joint](#joint), [Spring](#spring)) and the sensor element (LoadCellJoint, LoadCellSpring, DisplacementSensor). + +### LoadCell +Parent class to [LoadCellJoint](#loadcelljoint) and [LoadCellSpring](#loadcellspring) + +### LoadCellJoint +A load cell can be attached to a [Joint](#joint) with LoadCellJoint. This is less developed than LoadCellSpring as the force on a point in space is less representative than a force between joints. This is due to how it represents the force. It takes the net force applied by springs in any direction on the joint and vector sums them. + + +Create and attach a LoadCellJoint with +``` +load_cell_joint = LoadCellJoint(sensor_settings: dict = { + "name": str, + "mV/V": float, + "excitation": float, + "full_scale_force": float, # What is the max force on the load cell + "output_channel": int # The output channel that the load cell outputs through the MCC3114. Must be between [0, 15] +}) +load_cell_joint.attach_sim_source(shared_joint) + +hitl_controller.attach_load_cell(load_cell_joint) +``` +To allow the LoadCellJoint to get data from the physics engine, attach an instance of `SharedJoint` named 'shared_joint' through `load_cell_joint.attach_sim_source(shared_joint)`. + +Now, the attach the LoadCellJoint to the HITL controller through `hitl_controller.attach_load_cell(load_cell_joint)`. Now, the controller will actually know there is a sensor that needs to be output. + + +### LoadCellSpring +A load cell can also be attached to a [Spring](#spring) with LoadCellSpring. This is much more representative of a load cell in the real world as it only outputs a force along a vector (defined as the spring vector $\vec r$). + +Create and attach a LoadCellSpring with +``` +load_cell_Spring = LoadCellSpring(sensor_settings: dict = { + "name": str, + "mV/V": float, + "excitation": float, + "full_scale_force": float, # What is the max force on the load cell + "output_channel": int # The output channel that the load cell outputs through the MCC3114. Must be between [0, 15] +}) +load_cell_spring.attach_sim_source(shared_spring) + +hitl_controller.attach_load_cell(load_cell_spring) +``` +To allow the LoadCellSpring to get data from the physics engine, attach an instance of `SharedSpring` named 'shared_spring' through `load_cell_spring.attach_sim_source(shared_spring)`. + +Now, the attach the LoadCellSpring to the HITL controller through `hitl_controller.attach_load_cell(load_cell_spring)`. Now, the controller will actually know there is a sensor that needs to be output. + +### DisplacementSensor +A position/displacement sensor can be attached to any two joints with DisplacementSensor. This allows for a scalar length between the joints to be output through the MCC3114. + +Create and attach a DisplacementSensor with +``` +displacement_sensor = DisplacementSensor( + parent_joint: SharedJoint, # Must be a SharedJoint for the sensor to get values from the sim + child_joint: SharedJoint, # Must be a SharedJoint for the sensor to get values from the sim + sensor_settings: dict = { + "name": str, + "neutral_length": float, # This is the 0 voltage value. Can be 0 for purely positive voltage output or non-zero to allow negative voltage output + "volts_per_meter": float, # How many volts per meter of length should the MCC3114 output + "output_channel": int # The output channel that the load cell outputs through the MCC3114. Must be between [0, 15] + } +) +hitl_controller.attach_displacement_sensor(displacement_sensor) +``` +The DisplacementSensor gets data from the SharedJoints that are specified as a parent_joint and child_joint. The axis vector of the displacement sensor is given by $\vec r = \vec x_{child} - \vec x_{parent}$. For a scalar sensor, this plays no importance. Length is the vector length of $\vec r$. + +Now, the attach the DisplacementSensor to the HITL controller through `hitl_controller.attach_displacement_sensor(displacement_sensor)`. Now, the controller will actually know there is a sensor that needs to be output. + + +## Controller Elements +Controller elements are located in [code/Controller_Input_Elements.py](code/Controller_Input_Elements.py). Controller elements are how a real-world controller can control something inside of the physics simulation. + +Because the physics simulation and HITL controller processes are separate, you must create a shared object to link them. The only shared objects for a controller element is `SharedRigidActuator` which links the controller to the physics simulation so it can control the actuator. + +In reference to the data sharing paths in the process diagram, the hitl controller pushes the commanded attribute (velocity or position) to the SharedRigidActuator and the physics sim pulls the commanded attribute. + +With the MCC202, the range is [-10, +10] V. Any exceeding past this range can cause damage to the device. Digital inputs operate on [0, 5] V TTL and should not experience voltage below 0 V or above 5.5 V to avoid damage. + +### RigidActuatorController +A [RigidActuator](#rigidactuator) in the physics simulation is useless without a RigidActuatorController to move it. As a reminder, a [RigidActuator](#rigidactuator) connects 2 joints: the grounded and the parent joint. The grounded joint will never move. The parent joint can be controlled along the axis of the actuator. The user can specify whether the controller commands the position or the velocity of the parent joint with respect to the grounded joint. + +Create and attach a RigidActuatorController with +``` +rigid_actuator_controller = RigidActuatorController(controller_settings: dict = { + "name": str, + "analog_input_channel": int, # The analog channel on the MCC202 that controls the rigid actuator. Must be between [0, 7] + "digital_input_channel": int, # The digital channel on the MCC202 that enables control of the rigid actuator. Must be between [0, 7] + "units_per_volt": float, # m/V or m/s/V. How far or how fast the actuator should move per volt + "neutral_length": float, # Length to go to when given neutral voltage (position control). Also used for starting + "neutral_voltage": float, # The voltage at which to go to neutral_length (position control) or zero velocity (velocity control) + "controls_pos/vel/accel": int, # 0 = controls pos, 1 = controls vel, 2 = controls accel + "max_length": float, + "min_lenght": float +}) +rigid_actuator_controller.attach_sim_target(shared_rigid_actuator) + +hitl_controller.attach_rigid_actuator_controller(rigid_actuator_controller) +``` +To allow the RigidActuatorController to command the physics engine to respond to real-life inputs, attach an instance of `SharedRigidActuator` named 'shared_rigid_actuator' with `rigid_actuator_controller.attach_sim_target(shared_rigid_actuator)`. + +Now, the attach the RigidActuatorController to the HITL controller through `hitl_controller.attach_rigid_actuator_controller(rigid_actuator_controller)`. Now, the controller will actually know there is a rigid actuator that it needs to get inputs for and command. + + +## Sharing Elements +Sharing elements refers to the data flow of object between processes using pipes. This is largely hidden from the user as multiprocessing.managers.BaseManager does this for us. It creates AutoProxy elements of each element we "share" and when we "attach" them to an instance of a class, they are given access to the AutoProxy object. The processes that have access to the AutoProxy objects can interact with them in 1 fashion: function calls. Direct access to internal attributes is not possible without a copy of the object. So, there are numerous getters and setters for each "shared" object for data retrieval and setting. + +For the most part, sharing elements is for allowing sensor and controller elements to communicate with the elements in the physics simulation. A [LoadCell](#loadcell) needs a [SharedJoint](#sharedjoint) or a [SharedSpring](#sharedspring), a [DisplacementSensor](#displacementsensor) needs 2 [SharedJoints](#sharedjoint), a [RigidActuatorController](#rigidactuatorcontroller) needs a [SharedRigidActuator](#sharedrigidactuator). A sensor element will be connected by attaching a shared element to the sensor through "attach_sim_source". A controller element will be connected by attaching a shared element to the controller through "attach_sim_target". + +Any element can also be plotted and some can be 3D visualized. Those elements will also need a shared element. This is usually taken care of by attaching the shared element to the base element through "attach_shared_attributes". + +### BaseManager +The BaseManager is how elements can be shared amongst processes. + +Here is the code to make an instance of the BaseManager and register classes to be shared +``` +from multiprocessing.managers import BaseManager +base_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('SharedDisplacementSensor', SharedDisplacementSensor) + +BaseManager.register('SharedRigidActuatorController', SharedRigidActuatorController) + +base_manager.start() +``` +After the instance is created, each class that needs to be shared needs to be registered. When registering classes, the objects must be 'picklable'. I have never run into an instance where I could not share an object except for when the object contained a lambda function. + +Each of the shared elements is setup to have strong similarity with each other and the thing they are representing. They are also supposed to do as little calculation as possible. In a best scenario, they only have getters and setters. + + +### SharedJoint +A SharedJoint is needed to share the attributes of a [Joint](#joint) if it is connected to one of the following: + +1. [Visualization](#visualization) +2. [Plotter](#plotter) +3. [LoadCellJoint](#loadcelljoint) +4. [DisplacementSensor](#displacementsensor) + +Here is the code to make and attach a SharedJoint to a [Joint](#joint) +``` +base_manager = BaseManager() +BaseManager.register('SharedJoint', SharedJoint) # Register SharedJoint under the name 'SharedJoint' +base_manager.start() + +... + +joint = Joint(...) # Previously defined Joint +shared_joint: SharedJoint = base_manager.SharedJoint() + +joint.attach_shared_attributes(shared_joint) # Point the Joint to push physics values to the SharedJoint + +...Attach shared_joint to 3D visualization, plotter +``` + + +### SharedSpring +A SharedSpring is needed to share the attributes of a [Spring](#spring) if it is connected to one of the following: + +1. [Visualization](#visualization) +2. [Plotter](#plotter) +3. [LoadCellSpring](#loadcellspring) + +Here is the code to make and attach a SharedSpring to a [Spring](#spring) +``` +base_manager = BaseManager() +BaseManager.register('SharedSpring', SharedSpring) # Register SharedSpring under the name 'SharedSpring' +base_manager.start() + +... + +spring = Spring(...) # Previously defined Spring +shared_spring: SharedSpring = base_manager.SharedSpring() + +spring.attach_shared_attributes(shared_spring) # Point the Spring to push physics values to the SharedSpring + +...Attach shared_spring to 3D visualization, plotter +``` + + +### SharedRigidActuator +A SharedRigidActuator is needed to share the attributes of a [RigidActuator](#rigidactuator) if it is connected to one of the following: + +1. [Plotter](#plotter) +2. [RigidActuatorController](#rigidactuatorcontroller) + +This means that a SharedRigidActuator is required for the normal use of a [RigidActuator](#rigidactuator) so that the [HITLController](#hitlcontroller-hardware-in-the-loop-controller) can push values to the physics simulation. + +Here is the code to make and attach a SharedRigidActuator to a [RigidActuator](#rigidactuator) +``` +base_manager = BaseManager() +BaseManager.register('SharedRigidActuator', SharedRigidActuator) # Register SharedRigidActuator under the name 'SharedRigidActuator' +base_manager.start() + +hitl_controller = HITLController(...) # Previously defined HITLController + +... + +rigid_actuator = RigidActuator(...) # Previously defined RigidActuator +shared_rigid_actuator: SharedRigidActuator = base_manager.SharedRigidActuator() + +rigid_actuator.attach_shared_attributes(shared_rigid_actuator) # Point the RigidActuator to pull conrolled values from the SharedRigidActuator + +rigid_actuator_controller = RigidActuatorController(...) # Defined RigidActuatorController + +rigid_actuator_controller.attach_sim_target(shared_rigid_actuator) # Point the controller to the SharedRigidActuator + +hitl_controller.attach_rigid_actuator_controller(rigid_actuator_controller) # Attach a RigidActuatorController to the HITLController + +...Attach shared_rigid_actuator to plotter +``` + + +### SharedLoadCellJoint +A SharedLoadCellJoint is needed to share the attributes of a [LoadCellJoint](#loadcelljoint) if it is connected to one of the following: + +1. [Plotter](#plotter) + +Here is the code to make and attach a SharedLoadCellJoint to a [LoadCellJoint](#loadcelljoint) +``` +base_manager = BaseManager() +BaseManager.register('SharedLoadCellJoint', SharedLoadCellJoint) # Register SharedLoadCellJoint under the name 'SharedLoadCellJoint' +base_manager.start() + +hitl_controller = HITLController(...) # Previously defined HITLController + +... +joint = Joint(...) # Previously defined Joint +shared_joint: SharedJoint = base_manager.SharedJoint() + +load_cell_joint = LoadCellJoint(...) # Previously defined LoadCellJoint +load_cell_joint.attach_sim_source(shared_joint) # Point the LoadCellJoint to pull physics values from the SharedJoint + +shared_load_cell_joint: SharedLoadCellJoint = base_manager.SharedLoadCellJoint() + +load_cell_joint.attach_shared_attributes(shared_load_cell_joint) # Point the LoadCellJoint to push values to the SharedLoadCellJoint + +hitl_controller.attach_load_cell(load_cell_joint) # Attach a LoadCellJoint to the HITLController + +...Attach shared_load_cell_joint to plotter +``` + + +### SharedLoadCellSpring +A SharedLoadCellSpring is needed to share the attributes of a [LoadCellSpring](#loadcellspring) if it is connected to one of the following: + +1. [Plotter](#plotter) + +Here is the code to make and attach a SharedLoadCellSpring to a [LoadCellSpring](#loadcellspring) +``` +base_manager = BaseManager() +BaseManager.register('SharedLoadCellSpring', SharedLoadCellSpring) # Register SharedLoadCellSpring under the name 'SharedLoadCellSpring' +base_manager.start() + +hitl_controller = HITLController(...) # Previously defined HITLController + +... +spring = Spring(...) # Previously defined Spring +shared_spring: SharedJoint = base_manager.SharedSpring() + +load_cell_spring = LoadCellSpring(...) # Previously defined LoadCellSpring +load_cell_spring.attach_sim_source(shared_spring) # Point the LoadCellSpring to pull physics values from the SharedSpring + +shared_load_cell_spring: SharedLoadCellSpring = base_manager.SharedLoadCellSpring() + +load_cell_spring.attach_shared_attributes(shared_load_cell_spring) # Point the LoadCellSpring to push values to the SharedLoadCellSpring + +hitl_controller.attach_load_cell(load_cell_spring) # Attach a LoadCellSpring to the HITLController + +...Attach shared_load_cell_spring to plotter +``` + + +### SharedDisplacementSensor +A SharedDisplacementSensor is needed to share the attributes of a [DisplacementSensor](#displacementsensor) if it is connected to one of the following: + +1. [Plotter](#plotter) + +Here is the code to make and attach a SharedDisplacementSensor to a [DisplacementSensor](#displacementsensor) +``` +base_manager = BaseManager() +BaseManager.register('SharedDisplacementSensor', SharedDisplacementSensor) +base_manager.start() + +... +displacement_sensor = DisplacementSensor(...) # Previously defined DisplacementSensor +shared_displacement_sensor: SharedDisplacementSensor = base_manager.SharedDisplacementSensor() + +displacement_sensor.attach_shared_attributes(shared_displacement_sensor) # Point the DisplacementSensor to push values to the SharedDisplacementSensor + +...Attach shared_displacement_sensor to plotter +``` + + +### SharedRigidActuatorController +A SharedRigidActuatorController is needed to share the attributes of a [RigidActuatorController](#rigidactuatorcontroller) if it is connected to one of the following: + +1. [Plotter](#plotter) + +Here is the code to make and attach a SharedRigidActuatorController to a [RigidActuatorController](#rigidactuatorcontroller) +``` +base_manager = BaseManager() +BaseManager.register('SharedRigidActuatorController', SharedRigidActuatorController) +base_manager.start() + +... +rigid_actuator_controller = RigidActuatorController(...) +shared_rigid_actuator_controller: SharedRigidActuatorController = base_manager.SharedRigidActuatorController() + +rigid_actuator_controller.attach_shared_attributes(shared_rigid_actuator_controller) + +...Attach shared_rigid_actuator_controller to plotter +``` + + +## BFSSimulation (Breadth First Search Simulation) +The BFSSimulation class does the physics engine work. Named 'BFS' due to how it searches for all physics elements by starting at a parent [Joint](#joint) and traversing in a BFS style to gather all children. + +The BFSSimulation class is located in [code/Simulation.py](code/Simulation.py). + +The physics engine does not need anything attached to it before the process start. When the BFSSimulation starts, it initializes itself by getting all joints connected to it and sorting them in ways for it perform the simulation well. + +``` +simulation = BFSSimulation( + parent_joint: Joint, # Main joint to start BFS on and get everything connected to the physics simulation + settings: dict = { + "duration": float # The total duration in seconds to run the simulation + "delta_t": float or None, # None for running in real-time. Specified in seconds to have a constant timestep + "plotting_update_period": float, # Period in seconds to update shared elements that are not connected to a sensor or controller + "sensor_update_period": float, # Period in seconds to update shared elements connected to sensors + "controller_pull_period": float # Period in seconds to pull values from shared elements for controlled elements (actuators) + } +) +shared_sim_time: SharedFloat = base_manager.SharedFloat() +simulation.attach_shared_time(shared_sim_time) +simulation_process = multiprocessing.Process(target=simulation.run_process) + +time.sleep(5) # Used so that the plotter, visualization, and controller processes can spool up +simulation_process.start() + # The simulation runs until it has done its full duration +simulation_process.join() +stop_event.set() # Used to tell the other processes there day has come +``` +The physics simulation is fairly easy to setup and start. As long as the physics elements are setup in a way that each object is reachable, it will handle itself. This does mean that there should be a clear chain from parent->child->child->child. + + +## HITLController (Hardware-in-the-Loop Controller) +The HITLController class handles the sensor output through the MCC3114 and controller input from MCC202. With these two devices, there are 16 analog [-10, +10] V inputs on the MCC3114, 8 analog [-10, +10] V inputs on the MCC202, and 8 digital 5 V TTL inputs on the MCC202. + +The HITLController class is located in [code/HITL_Controller.py](code/HITL_Controller.py). + +Here is the code to make a HITLController instance +``` +stop_event = multiprocessing.Event() + +hitl_controller = HITLController( + stop_event = multiprocessing.Event, # Provides a mean for the main process to stop the controller when the simulation is done + settings: dict = { + "pull_from_sim_period": float, # Period in milliseconds to pull values from the physics sim for sensor elements + "plotting_update_period": float # Period in milliseconds to update shared elements that are being plotted + } +) + +...Make controller elements + +hitl_controller.attach_load_cell(load_cell) # Attach either type of load cell +hitl_controller.attach_displacement_sensor(displacement_sensor) +hitl_controller.attach_rigid_actuator_controller(actuator_controller) + +... + +hitl_controller_process = multiprocessing.Process(target=hitl_controller.run_process) +hitl_controller_process.start() + +...Setting other processes and running the physics simulation + +stop_event.set() +hitl_controller_process.join() +``` + +### Adding a [LoadCellJoint](#loadcelljoint) or [LoadCellSpring](#loadcellspring) to a HITLController +Since the [LoadCellJoint](#loadcelljoint) and [LoadCellSpring](#loadcellspring) share the same parent class, [LoadCell](#loadcell), they are attached the same way. + +``` +hitl_controller = HITLController(...) # Previously defined HITLController + +... + +load_cell = LoadCell(...) # LoadCellJoint or LoadCellSpring() + +hitl_controller.attach_load_cell(load_cell) +``` + + +### Adding a [DisplacementSensor](#displacementsensor) to a HITLController +``` +hitl_controller = HITLController(...) # Previously defined HITLController + +... + +displacement_sensor = DisplacementSensor(...) + +hitl_controller.attach_displacement_sensor(displacement_sensor) +``` + + +### Adding a [RigidActuatorController](#rigidactuatorcontroller) to a HITLController +``` +hitl_controller = HITLController(...) # Previously defined HITLController + +... + +rigid_actuator_controller = RigidActuatorController(...) + +hitl_controller.attach_rigid_actuator_controller(rigid_actuator_controller) +``` + + +## Visualization +The Visualization class provides a wrapper of VPython such that it is easy for the user to quickly physics elements to be visualized. It handles only a few types of objects including joints as spheres, springs as helizes, and rigid actuators as cylinders. + +The Visualization class is located in [code/Visualization.py](code/Visualization.py). + +Here is the code to make a Visualization instance +``` +stop_event = multiprocessing.Event() +stop_event.clear() + +visualizer = Visualization( + stop_event=stop_event, + scene_settings: dict = { + "canvas_width": int, # Desired width of the screen in the browser + "canvas_height": int, # Desired height of the screen in the browser + "wall_thickness": float, # Thickness of the walls + "cube_size": float # Size of the cube surrounding the origin + } +) + +...Add elements to be visualized + +visualization_process = multiprocessing.Process(target=visualizer.run_process) +visualization_process.start() + +...Setting other processes and running the physics simulation + +stop_event.set() +visualization_process.join() +``` + +### Adding a [Joint](#joint) to a Visualization +Joints are spheres in the 3D visualization. + +``` +visualizer = Visualization(...) # Previously defined Visualization + +... + + +joint = Joint(...) # Previously defined Joint +shared_joint: SharedJoint = base_manager.SharedJoint() # Required to be visualized +joint.attach_shared_attributes(shared_joint) + +visualizer.attach_shared_attributes( + shared_attributes: shared_joint, + object_settings: dict = { + "type": "sphere", + "color": str, # "blue", "black", "red", "green", "white" + "radius": float + } +) +``` + + +### Adding a [Spring](#spring) to a Visualization +Springs are helixes in the 3D visualization. + +``` +visualizer = Visualization(...) # Previously defined Visualization + +... + + +spring = Spring(...) # Previously defined Spring +shared_spring: SharedSpring = base_manager.SharedSpring() # Required to be visualized +spring.attach_shared_attributes(shared_spring) + +visualizer.attach_shared_attributes( + shared_attributes: shared_spring, + object_settings: dict = { + "type": "helix", + "color": str, # "blue", "black", "red", "green", "white" + "radius": float, # Radius the wire is wrapped in a coil + "thickness": float # Similar to the gauge of the wire that is coiled + } +) +``` + + +### Adding a [RigidActuator](#rigidactuator) to a Visualization +RigidActuators are cylinders in the 3D visualization. + +``` +visualizer = Visualization(...) # Previously defined Visualization + +... + + +rigid_actuator = RigidActuator(...) # Previously defined RigidActuator +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: dict = { + "type": "cylinder", + "color": str, # "blue", "black", "red", "green", "white" + "radius": float + } +) +``` + + + +## Plotter +The Plotter class provides a wrapper of PyQtGraph such that it is easy for the user to quickly add any physics, sensor, or controller element to be plotted in real-time. It is typical to spawn multiple plotter processes for physics, sensor, and controller plotting to be separated. While this is not necessary, it allows the user to control the windows and be more flexible. + +The Plotter class is located in [code/Visualization.py](code/Visualization.py). + +Here is the code to make a Plotter instance +``` +stop_event = multiprocessing.Event() + +plotter = Plotter( + stop_event: multiprocessing.Event, # Provides a mean for the main process to stop the plotter when the simulation is done + plot_settings: dict = { + "title": str, + "xlabel": str, + "num_subplots": int, # Number of subplots to include in the window + "step_or_plot": str, # 'plot' for linear interpolation between points, 'step' for a step function between points + "subplots": list [ # A list of dicts for each subplot starting at index = 0 up to num_subplots - 1 + { + "ylabel": str + }, + ... for each subplot + ], + "window_length": float, # The total time window in seconds of data points to store at any given time + "pull_interval": float, # The number of milliseconds between pulling for datapoints + "update_interval": int, # The number of milliseconds between updating the graph + } +) +plotter.attach_shared_x(shared_float) + +...Adding elements to plot + +plotter_process = multiprocessing.Process(target=plotter.run_process) +plotter_process.start() + +...Setting other processes and running the physics simulation + +stop_event.set() +plotter_process.join() +``` +This creates a plotter object to add elements to plot. After adding all elements to plot, you can start the process. When the physics simulation process joins, you can set the stop_event and join the plotter_process. Note that for PyQtGraph, you must close out the plotter windows for the process to end. This is favorable so the user as the ability to export values and keep it up if desired. + +Note that a step plot has limited functionality such as lack of FF, dy/dx, and Y vs Y'. + +For each element you will see that there is a `plot_settings` dictionary when attaching to the plotter. This dictionary follows the same general structure for each element with the following rules: + +1. Everything except the ylabel must be in lowercase. +2. The ylabel must be unique between everything added to the same plotter. +3. The subplot integer must correspond to a subplot in range [0, num_subplots-1]. + +For general information on PyQtGraph, go to their [website](https://pyqtgraph.readthedocs.io/en/latest/user_guide/index.html). + + +### Adding a [Joint](#joint) to a Plotter +Adding a Joint to a plotter requires a [SharedJoint](#sharedjoint). + +``` +plotter = Plotter(...) + +... + +joint = Joint(...) +shared_joint: SharedJoint = base_manager.SharedJoint() +joint.attach_shared_attributes(shared_joint) + +plotter.attach_shared_attributes( + shared_attributes = shared_joint, + plot_settings: dict = { + "pos": { + "x": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "y": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "z": {...}, + "scalar": {...} + }, + "vel": {...}, + "accel": {...} + } +) + +``` +A Joint can have its position, velocity, and/or its acceleration plotted. Each of these can be specified between x, y, z, and scalar components. Not all of these have to be included. If it doesn't need to be plotted just don't include the key in the dictionary. + +### Adding a [Spring](#spring) to a Plotter +Adding a Spring to a plotter requires a [SharedSpring](#sharedspring). + +``` +plotter = Plotter(...) + +... + +spring = Spring(...) +shared_spring: SharedSpring = base_manager.SharedSpring() +spring.attach_shared_attributes(shared_spring) + +plotter.attach_shared_attributes( + shared_attributes = shared_spring, + plot_settings: dict = { + "force": { + "x": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "y": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "z": {...}, + "scalar": {...} + }, + "length": { + "scalar": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + } + } +) + +``` +A Spring can have its force plotted in x, y, z, and scalar components. It can also have its scalar length plotted. Not all of these have to be included. If it doesn't need to be plotted just don't include the key in the dictionary. + + +### Adding a [RigidActuator](#rigidactuator) to a Plotter +Adding a RigidActuator to a plotter requires a [SharedRigidActuator](#sharedrigidactuator). + +``` +plotter = Plotter(...) + +... + +rigid_actuator = RigidActuator(...) +shared_rigid_actuator: SharedRigidActuator = base_manager.SharedRigidActuator() +rigid_actuator.attach_shared_attributes(shared_rigid_actuator) + +plotter.attach_shared_attributes( + shared_attributes = shared_rigid_actuator, + plot_settings: dict = { + "pos": { + "x": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "y": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "z": {...}, + "scalar": {...} + }, + "vel": {...}, + "length": { + "scalar": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + } + } +) + +``` +A RigidActuator can have its position and/or velocity plotted in x, y, z, and scalar components. It can also have its scalar length plotted. Not all of these have to be included. If it doesn't need to be plotted just don't include the key in the dictionary. + + +### Adding a [LoadCellJoint](#loadcelljoint) to a Plotter +Adding a LoadCellJoint to a plotter requires a [SharedLoadCellJoint](#sharedloadcelljoint). + +``` +plotter = Plotter(...) + +... + +load_cell_joint = LoadCellJoint(...) +shared_load_cell_joint: SharedLoadCellJoint = base_manager.SharedLoadCellJoint() +load_cell_joint.attach_shared_attributes(shared_load_cell_joint) + +plotter.attach_shared_attributes( + shared_attributes = shared_load_cell_joint, + plot_settings: dict = { + "force": { + "x": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "y": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "z": {...}, + "scalar": {...} + }, + "true_voltage": { + "scalar": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + }, + "noisy_voltage": { + "scalar": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + } + } +) + +``` +A LoadCellJoint can have its force plotted in x, y, z, and scalar components. It can also have the voltage output from the sensor plotted. The voltage can be the true voltage directly from the force on the load cell or the noisy voltage after the true voltage has gone through noise generation. Not all of these have to be included. If it doesn't need to be plotted just don't include the key in the dictionary. + +### Adding a [LoadCellSpring](#loadcellspring) to a Plotter +Adding a LoadCellSpring to a plotter requires a [SharedLoadCellSpring](#sharedloadcellspring). + +``` +plotter = Plotter(...) + +... + +load_cell_spring = LoadCellJoint(...) +shared_load_cell_spring: SharedLoadCellSpring = base_manager.SharedLoadCellSpring() +load_cell_spring.attach_shared_attributes(shared_load_cell_spring) + +plotter.attach_shared_attributes( + shared_attributes = shared_load_cell_spring, + plot_settings: dict = { + "force": { + "x": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "y": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "z": {...}, + "scalar": {...} + }, + "true_voltage": { + "scalar": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + }, + "noisy_voltage": { + "scalar": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + } + } +) + +``` +A LoadCellSpring can have its force plotted in x, y, z, and scalar components. It can also have the voltage output from the sensor plotted. The voltage can be the true voltage directly from the force on the load cell or the noisy voltage after the true voltage has gone through noise generation. Not all of these have to be included. If it doesn't need to be plotted just don't include the key in the dictionary. + + +### Adding a [DisplacementSensor](#displacementsensor) to a Plotter +Adding a DisplacementSensor to a plotter requires 2 [SharedJoints](#sharedjoint). There is a parent joint and child joint. Displacement is defined as $\vec r = \vec x_{child} - \vec x_{parent}$ + +``` +plotter = Plotter(...) + +... + +parent_joint = Joint(...) +child_joint = Joint(...) + +shared_parent_joint: SharedJoint = base_manager.SharedJoint() +shared_child_joint: SharedJoint = base_manager.SharedJoint() + +parent_joint.attach_shared_attributes(shared_parent_joint) +child_joint.attach_shared_attributes(shared_child_joint) + +displacement_sensor = DisplacementSensor(...) +shared_displacement_sensor: SharedDisplacementSensor = base_manager.SharedDisplacementSensor() + +plotter.attach_shared_attributes( + shared_attributes = shared_displacement_sensor, + plot_settings: dict = { + "disp": { + "scalar": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + }, + "voltage": { + "noisy": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "true": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + } + } +) + +``` +A DisplacementSensor can have its displacement scalar plotted. It can also have the voltage output from the sensor plotted. The voltage can be the true voltage directly from the force on the load cell or the noisy voltage after the true voltage has gone through noise generation. Not all of these have to be included. If it doesn't need to be plotted just don't include the key in the dictionary. + + +### Adding a [RigidActuatorController](#rigidactuatorcontroller) to a Plotter +Adding a RigidActuatorController to a plotter requires a [SharedRigidActuatorController](#sharedrigidactuatorcontroller). + +``` +plotter = Plotter(...) + +... + +rigid_actuator_controller = RigidActuatorController(...) +shared_rigid_actuator_controller: SharedRigidActuatorController = base_manager.SharedRigidActuatorController() +rigid_actuator_controller.attach_shared_attributes(shared_rigid_actuator_controller) + +plotter.attach_shared_attributes( + shared_attributes = shared_rigid_actuator_controller, + plot_settings: dict = { + "pos": { + "x": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "y": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + }, + "z": {...}, + "scalar": {...} + }, + "vel": {...}, + "input_voltage": { + "scalar": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + }, + "digital_voltage": { + "scalar": { + "subplot": int, # Must be in range [0, num_subplots-1] + "ylabel": str, + "color": str # White is default if not listed. "b", "g", "r", "c", "m", "y", "k", "w" + } + } + } +) + +``` +A RigidActuatorController can have its position or velocity plotted in x, y, z, and scalar components. It can also have the voltage input and/or digital input plotted. Not all of these have to be included. If it doesn't need to be plotted just don't include the key in the dictionary. + + +# General Code Review +## Process Diagram and Data Sharing +In order to achieve real-time physics simulation in parallel with sensor and controller I/O, a complex process and threading structure was developed. Due to the Global Interpreter Lock of Python, multiprocessing is the primary means of parallelism. Individual processes can and do use threading. In a C++ or similar mid-level language, threading could be used to achieve the same effect that multiprocessing does without the complex data sharing paths. + +For a graphical view of the processes and threads, look in [documentation/code_design/Process_Diagram.drawio](documentation/code_design/Process_Diagram.drawio). There you will find when the processes are created, the main sequence of events they do when they are started, and what ends them. + + + +# The Physics Engine +The physics engine is a variable time-step Newtonian physics engine with a few noteworthy features. + +## Initialization +``` +class BFFSimulation(): + def __init__(self, parent_joint: Joint, settings:dict): + self.parent_joint = parent_joint + self.joints: List[Joint] = [] + self.springs: List[Spring] = [] + self.actuators: List[RigidActuator] = [] + self.rigid_actuators: List[RigidActuator] = [] + ... + self.get_joints_bfs() + ... + self.sort_shared_attributes_into_frequencies() +... +``` +`self.get_joints_bfs()` fills the lists of joints, springs, and actuators. This is directly used in the simulation to calculate attributes for physics modeling. + +`self.sort_shared_attributes_into_frequencies()` is used for updating shared attributes. Elements that are shared for visualization, sensors, and controllers might want to be updated at different frequencies. Updating shared attributes is cost heavy and should be minimized when possible. Therefore, we only update when we need to. + +## Acceleration and Integration Techniques +With the contraint of running in real-time, having an accurate yet computationally-inexpensive method for integrating acceleration into velocity and position is necessary. Errors arrive solely from the deviance of instantaneous acceleration at $t = t_0$ from average acceleration over the period $[t_0,t_1]$. If they were the same, then the physics simulation would need nothing better than simple integration. The problem arises when trying to generalize a solution for calculating the average acceleration rather than the straightforward instantaneous acceleration. Integration techniques help overcome the deviance and can provide other benefits such as energy-reducing systems rather than energy-gaining systems that tend to become extremely unstable. + +All of these integration techniques (with the slight exception of Predictor-Corrector) are implemented in StateSpace of [code/Physics_Elements.py](code/Physics_Elements.py). + +### Simple (Euler) Integration +``` +self.vel = self.vel + self.accel * delta_t +self.pos = self.pos + self.vel * delta_t +``` + +A tried and true method for integration. Works well with small delta_t. The cheapest method in terms of computation and memory cost. + +### Verlet Integration +``` +self.vel = self.vel + self.accel * 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 + ... +``` +Verlet integration is a multistep method that comes from the idea of finite differences. It is possible to calculate the instantaneous acceleration of an object using its past position values. Re-arranging the equation can yield an expression for the next position as a function of current instantaneous acceleration and the past positions. + +In order to calculate the coefficients for each term, you must solve a system of equations of the Taylor expansion about each point. A script to do this can be found in [code/useful_scripts/verlet_coefficients_calculator.py](code/useful_scripts/verlet_coefficients_calculator.py). + +This method has proven worse than simple integration for reasons unknown for now. Increasing the number of position points seems to make the error worse. The leading theory is that the past positions all have error from their instantaneous acceleration and any error continues to propagate rather than dissipate. It is worth noting that Verlet integration assumes a constant timestep which is not true for this simulation. + +How the integration method is setup now, the velocity and position are very loosely related and likely to drift from each other. This can become a signifant issue. There is also not a direct way for a controller to control the velocity of a joint and the position be automatically integrated. It is possible to perform the same matrix solver for calcuting the next velocity from past acceleration. + +### Adams-Bashforth Integration +``` +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]) + ... + self.pos = self.pos + self.vel * delta_t +``` +Adams-Bashforth integration is a multistep method similar to Verlet integration. However, it takes a more direct approach on working around the source of the error from the acceleration. Adams-Bashforth + +Adams-Bashforth integration has proven to do quite well while maintaining a fairly cheap computation and memory cost. 2 steps peforms similar to Euler integration while decaying the energy of the system. For that reason, it is default. 3 steps performs even better at maintaining energy. However, it tends to increase the total energy over time. Steps above 3 offer little to sometimes worse performance. + +It is worth noting that Adams-Bashforth integration assumes a constant timestep which is not true for this simulation. + +### Mixed Integration Technique +In order to prioritize certain joints that may experience larger peturbations or need a higher fidelity, it is possible to specify the integration technique upon [Joint](#joint) creation. Adams-Bashforth is the default, however, for simulations with large number of joints, it can be beneficial to set them to use simple integration. + +### Predictor-Corrector Methods for Acceleration (Not Implemented) +Re-iterating the issues that causes needs for better integration, we want to minimize the difference between the instantaneous acceleration that we apply as if it was the average acceleration over the entire timestep. Reducing timestep is a way to minimize the error propagated into velocity and position values. It is also possible to estimate better accelerations. Adams-Bashforth integration does this by weighting past accelerations to get a better idea of the average acceleration over the next timestep. + +Predictor-Corrector methods can come in many forms. Common ones are to calculate the instantaneous acceleration at points $x_0$ and $x_0 + \Delta x$ then average them to be applied over the timestep. Another variant is to calculate the acceleration at $x_0 + \frac{1}{2} \Delta x$. The new estimated acceleration is applied and integrated over the timestep to get the next velocity and position. + +It is also possible to iteratively run a predictor-corrector method until a finite error is achieved. By continuously integrating an acceleration, calculating new acceleration at the pseudo position, and looping until the changes in acceleration approach some value, the error can be controlled. However, for real-time systems, this is hard to be useful as the timestep increases at O(n) with n iterations and the accuracy of Euler integration is O($\Delta t^2$) for position. + + +### Mixed Predictor-Corrector Acceleration Technique (Not Implemented) +Tying back to mixed integration technique, applying predictor-corrector to only a handful of joint is a valid technique for minimizing the impact to timestep while getting better position estimates. For a completely generalized approach, it is possible for the simulation to decide which joints need more attention than others. By keeping track of factors such as last acceleration, current acceleration, and velocity, you can estimate how rapidly the calculated change in position would change if a more accurate acceleration was calculated. In a more definable method, a 1 step predictor-corrector method can be applied to every joint. For joints with large discrepancies in the predicted and corrected acclerations, more iterations can be completed. This has the benefit of minimizing the waste of timestep used to calculate accelerations that end up with diminishing returns. + +As an example, a center joint of large mass is surrounded in a circle by much smaller masses connected through springs. The larger mass would find that the variance between the initial acceleration and the corrected acceleration would be small due to the symmetrical properties of the springs and its large mass. On the other hand, the smaller masses would experience large differences in initial and corrected accelerations due to their single spring nature and smaller mass. + +General code would take the form +``` +real_statespace = list of all joint statespace +predicted_statespace = list of all joint statespace for use in correcting acceleration +corrected_statespace = list of all joint statespace after using corrected acceleration + +while self.sim_time < self.duration: + calculate predicted spring forces using real_statespace positions + calculate predicted force per spring and acceleration per spring at each joint using real_statespace positions + + integrate predicted_statespace starting at t_0 over 1/2 delta_t using predicted accelerations + + calculate corrected spring forces using predicted_statespace positions + calculate corrected force per spring and acceleration per spring at each joint using predicted_statespace positions + + for every joint: + for every spring connected to joint: + while (corrected acceleration - predicted acceleration) / corrected_acceleration is large: + integrate predicted_statespace starting at t_0 over 1/2 delta_t using corrected accelerations + + save previous corrected accelerations into predicted accelerations for comparison + + calculate corrected spring forces using predicted_statespace positions + calculate corrected force per spring and acceleration per spring at each joint using predicted_statespace positions + + set real_statespace acceleratiosn to corrected acceleration + + integrate real_statespace starting at t_0 over delta_t using corrected acceleration +``` +Because calculating the length of the springs is a large part of the time complexity, taking a shortcut could be beneficial during each iteration. Rather than recalculate the $\vec r$ for every spring at each iteration, assuming a constant $\vec r$ should suffice with such small changes. In addition, to recalculate the length at a correction step, one could take $\Delta l = l_0 + [(\vec v_{parent} - \vec v_{child})(\Delta t) + \frac{1}{2}(\vec a_{parent} - \vec a_{child})(\Delta t)^2]\cdot (\vec r)$ where parent and child refer to the parent joint and child joint of the spring. $\vec r_{spring}$ is defined as $\vec x_{child} - \vec x_{parent}$ and should not be updated in correct steps. + +Determining what is considered a large change in corrected vs predicted acceleration is abstract. It can be a pre-computed constant or a dynamic number that changes depending on the scenario. In situations where joints are close to each other (the length of the spring is small), what is considered a large acceleration should be smaller than if the distances were far apart. A simple method could be used to calculate what a large acceleration is. Assuming a constant acceleration, the error of change in position (error represented by $\delta$) due to error in acceleration is given as $\delta \Delta \vec x = \frac{1}{2}(\delta \vec a)(\Delta t)^2$. Since $\Delta t$ is known and $\delta \vec a$ was calculated, defining a large acceleration as some ratio $\frac{\delta \Delta \vec x}{l_{spring}}$ can be of use. + +A notable downfall of this method is in situations such as the large mass surrounded by smaller masses. The generalized solution would end up having to apply the iterative method to each joint due to the setup. + + +### Collision Dection +None :) + +Adding a collision detection would prove useful for dealing with runaway energy systems. However, it is computationally expensive, requiring distance calculations for n choose 2 or $(\frac{n}{2})$ which is $\frac{n^2 - n}{2}$. A more practical approach is to only consider collisions between joints with springs between them. This way the length is already computed. + +Once a collision takes place, performing an elastic collision between the 2 is trivial. Ideally, collision detection would not be a thing. In a more life-like simulation, there should be repulsive springs between every joint. This would also be $\frac{n^2 - n}{2}$ springs that are computed at all times. However, timesteps would currently be too large to be of usefulness. + + +# Examples +## 1D Mass-Spring Oscillator (No HITL Elements) +The example code in located in [code/_example_1d_mass_spring_oscillator.py](/code/_example_1d_mass_spring_oscillator.py) provides a simple getting started with Joints, Springs, the plotter, and visualizer. + +![3D Visualization of Mass-Spring Oscillator](documentation/examples/1d_mass_spring_oscillator/3D_Visualization.png) + +Above is the 3D Visualization before the physics simulation starts. The red sphere represents `main_mass` while the white helix on the left represents `spring`. + + +``` +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 +``` +We start with the necessary imports. + + +``` +'''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() +``` +We continue by setting up the BaseManager for object sharing. We need an instance of BaseManager and we need to register the objects we intend to share. Then we start it. + +``` +'''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 = {...} +) +mass_plotter.attach_shared_x(shared_sim_time) + +spring_plotter = Plotter( + stop_event = stop_event, + plot_settings = {...} +) +spring_plotter.attach_shared_x(shared_sim_time) + + +# Create a 3D visualization for the entire model +visualizer = Visualization( + stop_event = stop_event, + scene_settings = {...} +) +``` +This section creates instances for plotting and visualization. It starts by making an Event for simple communication between the processes. After physics simulation ends, the stop_event is set and the plotter and visualization processes will know to stop. There is also a SharedFloat. This is created for synchronization between physics simulation and plotters. It is initialized at t = 0.0. In this example, there is a dedicated plotter for the main_mass and the spring. This does not have to be this way. Multiple physics elements can be combined on the same plotter. Only a single visualization is made. + +``` +'''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" +) + +``` +These are all of the elements needed for the physics simulation. `main_mass` starts in the center raised by 5 meters. `wall_joint` is created so that there is a second joint for the spring to be attached to. It is invisible (because it was not added to the 3D visualizer) and is on the left wall. It is required for `fixed = True`. Finally the spring is made. Please note that `parent_joint = main_mass`. When we create the BFSSimulation, we specifiy `main_mass` as the parent_joint for the physics simulation. Because of how we defined `spring`, the breadth-first-search will see the main_mass, then the spring, then the wall_joint. If the spring had its parent be the wall_joint, the physics simulation would never see the wall_joint. + + +``` +'''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 = {...} +) + +spring_plotter.attach_shared_attributes( + shared_attributes = shared_spring, + plot_settings = {...} +) +``` +We first create and attach the shared objects so that data can be passed from the physics simulation to the plotters. Then we attach them to the plotter with plot settings. + + +``` +'''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 = {...} +) + +visualizer.attach_shared_attributes( + shared_attributes = shared_spring, + object_settings = {...} +) +``` +Because the shared objects are already created, we don't need to create them again. So all we need to do is attach them to the visualization object with the object settings. + + +``` +'''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 = {...} +) +simulation.attach_shared_time(shared_sim_time) +``` +Like mentioned, specifiying `parent_joint = main_mass` provides a mean for the BFS to reach every element in the we created and linked together. We then attach the SharedFloat we created to synchronize the time bewteen the processes. + + +``` +'''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() +``` +For each plotter, visualizer, and physics simulation, we create a process using the built in `run_process()` function. Then we start the plotters and visualizers so that they can launch their windows and be ready for when the physics simulation starts. We sleep for 5 seconds and then start the simulation process. This will run until the duration is up and then join back in the main code. We join the rest of the processes and shutdown the base_manager to exit the program. + +## 1D Mass-Spring Oscillator with Actuator + + +## 2D Mass-Spring Oscillator with 2 Actuators + + +## Stage 1 Hanging Mass + +# Debugging +For a graphical view of the processes and threads, look in [documentation/code_design/Process_Diagram.drawio](documentation/code_design/Process_Diagram.drawio). + +## Not Plotting Correctly? +### Plotter Window Doesn't Spawn? +Make sure you made the Plotter instance, make a process for it, and started it. + +### Plotter Comes Up But Doesn't Show Lines? +Make sure you made a `Shared______` and attached it to the plotter AND the physics engine source correctly. + +### Plotting Some Parts But Not All? +This is likely a problem in Plotter.pull_data(). There is a chain of `if-elif` statements that check for every type of `Shared______`. For example, if a Joint is not being plotted correctly, go to `elif isinstance(foo, SharedJoint):`. It is possible there is a missed attribute that needs to be considered and handled correctly. + +Make sure you gave the `ylabel` a unique name. + +## Not Visualizing Correctly? +### Visualization Doesn't Spawn? +The Visualizer spwawns in your default browser. Make sure it isn't spawning in a browser you didn't know was open. + +Make sure you made the Visualizer instance, make a process for it, and started it. +### Visualization Doesn't Show an Object? +Make sure you made a `Shared______` and attached it to the visualizer correctly. + +Make sure the physics engine source (Joint, Spring, RigidActuator) has a unique name. \ No newline at end of file diff --git a/code/setup/code/.vscode/launch.json b/code/setup/code/.vscode/launch.json new file mode 100644 index 0000000..23d0afc --- /dev/null +++ b/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" + ] + } + ] +} \ No newline at end of file diff --git a/code/setup/code/.vscode/settings.json b/code/setup/code/.vscode/settings.json new file mode 100644 index 0000000..2c39ae2 --- /dev/null +++ b/code/setup/code/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "python.REPL.enableREPLSmartSend": false +} \ No newline at end of file diff --git a/code/setup/code/Controller_Input_Elements.py b/code/setup/code/Controller_Input_Elements.py new file mode 100644 index 0000000..a7602cb --- /dev/null +++ b/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 diff --git a/code/setup/code/HITL_Controller.py b/code/setup/code/HITL_Controller.py new file mode 100644 index 0000000..ef22713 --- /dev/null +++ b/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() + diff --git a/code/setup/code/MCC_Interface.py b/code/setup/code/MCC_Interface.py new file mode 100644 index 0000000..fdb7768 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/code/setup/code/Object_Sharing.py b/code/setup/code/Object_Sharing.py new file mode 100644 index 0000000..f601779 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/code/setup/code/Physics_Elements.py b/code/setup/code/Physics_Elements.py new file mode 100644 index 0000000..45c3113 --- /dev/null +++ b/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 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 \ No newline at end of file diff --git a/code/setup/code/Sensor_Elements.py b/code/setup/code/Sensor_Elements.py new file mode 100644 index 0000000..ca43ff1 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/code/setup/code/Simulation.py b/code/setup/code/Simulation.py new file mode 100644 index 0000000..0653f51 --- /dev/null +++ b/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}") \ No newline at end of file diff --git a/code/setup/code/Visualization.py b/code/setup/code/Visualization.py new file mode 100644 index 0000000..30bce3e --- /dev/null +++ b/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() + \ No newline at end of file diff --git a/code/setup/code/__pycache__/Controller_Input_Elements.cpython-311.pyc b/code/setup/code/__pycache__/Controller_Input_Elements.cpython-311.pyc new file mode 100644 index 0000000..2b06845 Binary files /dev/null and b/code/setup/code/__pycache__/Controller_Input_Elements.cpython-311.pyc differ diff --git a/code/setup/code/__pycache__/Controller_Interface.cpython-311.pyc b/code/setup/code/__pycache__/Controller_Interface.cpython-311.pyc new file mode 100644 index 0000000..c28b7e8 Binary files /dev/null and b/code/setup/code/__pycache__/Controller_Interface.cpython-311.pyc differ diff --git a/code/setup/code/__pycache__/HITL_Controller.cpython-311.pyc b/code/setup/code/__pycache__/HITL_Controller.cpython-311.pyc new file mode 100644 index 0000000..9e25c67 Binary files /dev/null and b/code/setup/code/__pycache__/HITL_Controller.cpython-311.pyc differ diff --git a/code/setup/code/__pycache__/MCC_Interface.cpython-311.pyc b/code/setup/code/__pycache__/MCC_Interface.cpython-311.pyc new file mode 100644 index 0000000..ab442ee Binary files /dev/null and b/code/setup/code/__pycache__/MCC_Interface.cpython-311.pyc differ diff --git a/code/setup/code/__pycache__/Object_Sharing.cpython-311.pyc b/code/setup/code/__pycache__/Object_Sharing.cpython-311.pyc new file mode 100644 index 0000000..62aa12d Binary files /dev/null and b/code/setup/code/__pycache__/Object_Sharing.cpython-311.pyc differ diff --git a/code/setup/code/__pycache__/PhysicsElements.cpython-311.pyc b/code/setup/code/__pycache__/PhysicsElements.cpython-311.pyc new file mode 100644 index 0000000..cd9470a Binary files /dev/null and b/code/setup/code/__pycache__/PhysicsElements.cpython-311.pyc differ diff --git a/code/setup/code/__pycache__/Physics_Elements.cpython-311.pyc b/code/setup/code/__pycache__/Physics_Elements.cpython-311.pyc new file mode 100644 index 0000000..21449b2 Binary files /dev/null and b/code/setup/code/__pycache__/Physics_Elements.cpython-311.pyc differ diff --git a/code/setup/code/__pycache__/Sensor_Elements.cpython-311.pyc b/code/setup/code/__pycache__/Sensor_Elements.cpython-311.pyc new file mode 100644 index 0000000..f0d7b7c Binary files /dev/null and b/code/setup/code/__pycache__/Sensor_Elements.cpython-311.pyc differ diff --git a/code/setup/code/__pycache__/Simulation.cpython-311.pyc b/code/setup/code/__pycache__/Simulation.cpython-311.pyc new file mode 100644 index 0000000..f643ada Binary files /dev/null and b/code/setup/code/__pycache__/Simulation.cpython-311.pyc differ diff --git a/code/setup/code/__pycache__/Visualization.cpython-311.pyc b/code/setup/code/__pycache__/Visualization.cpython-311.pyc new file mode 100644 index 0000000..0c7de3b Binary files /dev/null and b/code/setup/code/__pycache__/Visualization.cpython-311.pyc differ diff --git a/code/setup/code/_example_1d_mass_spring_oscillator.py b/code/setup/code/_example_1d_mass_spring_oscillator.py new file mode 100644 index 0000000..14da762 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/code/setup/code/_example_1d_mass_spring_oscillator_with_actuator.py b/code/setup/code/_example_1d_mass_spring_oscillator_with_actuator.py new file mode 100644 index 0000000..b976c4f --- /dev/null +++ b/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() \ No newline at end of file diff --git a/code/setup/code/_example_2d_mass_spring_oscillator_with_actuators.py b/code/setup/code/_example_2d_mass_spring_oscillator_with_actuators.py new file mode 100644 index 0000000..1c045f0 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/code/setup/code/_example_stage_1_test.py b/code/setup/code/_example_stage_1_test.py new file mode 100644 index 0000000..11d1e7a --- /dev/null +++ b/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() + diff --git a/code/setup/code/deprecated/1d_Mass_Spring.py b/code/setup/code/deprecated/1d_Mass_Spring.py new file mode 100644 index 0000000..0b56c06 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/code/setup/code/deprecated/Generalized_1d Mass_Spring_Damper_old.py b/code/setup/code/deprecated/Generalized_1d Mass_Spring_Damper_old.py new file mode 100644 index 0000000..2be8fc6 --- /dev/null +++ b/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() + \ No newline at end of file diff --git a/code/setup/code/deprecated/is_picklable.py b/code/setup/code/deprecated/is_picklable.py new file mode 100644 index 0000000..bcf637f --- /dev/null +++ b/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)) \ No newline at end of file diff --git a/code/setup/code/deprecated/live_plotter_test.py b/code/setup/code/deprecated/live_plotter_test.py new file mode 100644 index 0000000..7a9c660 --- /dev/null +++ b/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() diff --git a/code/setup/code/deprecated/mcc_usb1024_test.py b/code/setup/code/deprecated/mcc_usb1024_test.py new file mode 100644 index 0000000..02fb52e --- /dev/null +++ b/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() diff --git a/code/setup/code/deprecated/pstats_reader.py b/code/setup/code/deprecated/pstats_reader.py new file mode 100644 index 0000000..c4319ec --- /dev/null +++ b/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) \ No newline at end of file diff --git a/code/setup/code/deprecated/pyqtgraph_test.py b/code/setup/code/deprecated/pyqtgraph_test.py new file mode 100644 index 0000000..42dca58 --- /dev/null +++ b/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() diff --git a/code/setup/code/deprecated/shared_memory.py b/code/setup/code/deprecated/shared_memory.py new file mode 100644 index 0000000..e85df8c --- /dev/null +++ b/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() diff --git a/code/setup/code/deprecated/tests/.vscode/settings.json b/code/setup/code/deprecated/tests/.vscode/settings.json new file mode 100644 index 0000000..2c39ae2 --- /dev/null +++ b/code/setup/code/deprecated/tests/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "python.REPL.enableREPLSmartSend": false +} \ No newline at end of file diff --git a/code/setup/code/deprecated/tests/__pycache__/Object_Sharing.cpython-311.pyc b/code/setup/code/deprecated/tests/__pycache__/Object_Sharing.cpython-311.pyc new file mode 100644 index 0000000..bb0ee96 Binary files /dev/null and b/code/setup/code/deprecated/tests/__pycache__/Object_Sharing.cpython-311.pyc differ diff --git a/code/setup/code/deprecated/tests/__pycache__/PhysicsElements.cpython-311.pyc b/code/setup/code/deprecated/tests/__pycache__/PhysicsElements.cpython-311.pyc new file mode 100644 index 0000000..269ab88 Binary files /dev/null and b/code/setup/code/deprecated/tests/__pycache__/PhysicsElements.cpython-311.pyc differ diff --git a/code/setup/code/deprecated/tests/__pycache__/SimLibrary.cpython-311.pyc b/code/setup/code/deprecated/tests/__pycache__/SimLibrary.cpython-311.pyc new file mode 100644 index 0000000..4a69ae3 Binary files /dev/null and b/code/setup/code/deprecated/tests/__pycache__/SimLibrary.cpython-311.pyc differ diff --git a/code/setup/code/deprecated/tests/__pycache__/Simulation.cpython-311.pyc b/code/setup/code/deprecated/tests/__pycache__/Simulation.cpython-311.pyc new file mode 100644 index 0000000..16d880d Binary files /dev/null and b/code/setup/code/deprecated/tests/__pycache__/Simulation.cpython-311.pyc differ diff --git a/code/setup/code/deprecated/tests/__pycache__/Visualization.cpython-311.pyc b/code/setup/code/deprecated/tests/__pycache__/Visualization.cpython-311.pyc new file mode 100644 index 0000000..0069429 Binary files /dev/null and b/code/setup/code/deprecated/tests/__pycache__/Visualization.cpython-311.pyc differ diff --git a/code/setup/code/deprecated/tests/__pycache__/pstats.cpython-311.pyc b/code/setup/code/deprecated/tests/__pycache__/pstats.cpython-311.pyc new file mode 100644 index 0000000..b4e8b79 Binary files /dev/null and b/code/setup/code/deprecated/tests/__pycache__/pstats.cpython-311.pyc differ diff --git a/code/setup/code/setup/dependencies.py b/code/setup/code/setup/dependencies.py new file mode 100644 index 0000000..5d825e0 --- /dev/null +++ b/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.") diff --git a/code/setup/code/setup/icalsetup.exe b/code/setup/code/setup/icalsetup.exe new file mode 100644 index 0000000..c50a3c0 Binary files /dev/null and b/code/setup/code/setup/icalsetup.exe differ diff --git a/code/setup/code/useful_scripts/verlet_coefficients_calculator.py b/code/setup/code/useful_scripts/verlet_coefficients_calculator.py new file mode 100644 index 0000000..e0007d6 --- /dev/null +++ b/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) diff --git a/code/setup/dependencies.py b/code/setup/dependencies.py new file mode 100644 index 0000000..5d825e0 --- /dev/null +++ b/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.") diff --git a/code/setup/documentation/Progress_Document.docx b/code/setup/documentation/Progress_Document.docx new file mode 100644 index 0000000..2cea777 Binary files /dev/null and b/code/setup/documentation/Progress_Document.docx differ diff --git a/code/setup/documentation/RMC_HITL BOM.xlsx b/code/setup/documentation/RMC_HITL BOM.xlsx new file mode 100644 index 0000000..9285f9e Binary files /dev/null and b/code/setup/documentation/RMC_HITL BOM.xlsx differ diff --git a/code/setup/documentation/RMC_HITL_Overview.docx b/code/setup/documentation/RMC_HITL_Overview.docx new file mode 100644 index 0000000..8dcf15b Binary files /dev/null and b/code/setup/documentation/RMC_HITL_Overview.docx differ diff --git a/code/setup/documentation/code_design/.$Process_Diagram.drawio.bkp b/code/setup/documentation/code_design/.$Process_Diagram.drawio.bkp new file mode 100644 index 0000000..6bd0799 --- /dev/null +++ b/code/setup/documentation/code_design/.$Process_Diagram.drawio.bkp @@ -0,0 +1,149 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/code/setup/documentation/code_design/Process_Diagram.drawio b/code/setup/documentation/code_design/Process_Diagram.drawio new file mode 100644 index 0000000..037a9de --- /dev/null +++ b/code/setup/documentation/code_design/Process_Diagram.drawio @@ -0,0 +1,1236 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/code/setup/documentation/code_design/Process_Diagram.png b/code/setup/documentation/code_design/Process_Diagram.png new file mode 100644 index 0000000..65f19ac Binary files /dev/null and b/code/setup/documentation/code_design/Process_Diagram.png differ diff --git a/code/setup/documentation/examples/1d_mass_spring_oscillator/3D_Visualization.png b/code/setup/documentation/examples/1d_mass_spring_oscillator/3D_Visualization.png new file mode 100644 index 0000000..0129dcb Binary files /dev/null and b/code/setup/documentation/examples/1d_mass_spring_oscillator/3D_Visualization.png differ diff --git a/code/setup/icalsetup.exe b/code/setup/icalsetup.exe new file mode 100644 index 0000000..c50a3c0 Binary files /dev/null and b/code/setup/icalsetup.exe differ diff --git a/code/useful_scripts/verlet_coefficients_calculator.py b/code/useful_scripts/verlet_coefficients_calculator.py new file mode 100644 index 0000000..e0007d6 --- /dev/null +++ b/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) diff --git a/documentation/Progress_Document.docx b/documentation/Progress_Document.docx new file mode 100644 index 0000000..2cea777 Binary files /dev/null and b/documentation/Progress_Document.docx differ diff --git a/documentation/RMC_HITL BOM.xlsx b/documentation/RMC_HITL BOM.xlsx new file mode 100644 index 0000000..9285f9e Binary files /dev/null and b/documentation/RMC_HITL BOM.xlsx differ diff --git a/documentation/RMC_HITL_Overview.docx b/documentation/RMC_HITL_Overview.docx new file mode 100644 index 0000000..8dcf15b Binary files /dev/null and b/documentation/RMC_HITL_Overview.docx differ diff --git a/documentation/code_design/.$Process_Diagram.drawio.bkp b/documentation/code_design/.$Process_Diagram.drawio.bkp new file mode 100644 index 0000000..6bd0799 --- /dev/null +++ b/documentation/code_design/.$Process_Diagram.drawio.bkp @@ -0,0 +1,149 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/documentation/code_design/Process_Diagram.drawio b/documentation/code_design/Process_Diagram.drawio new file mode 100644 index 0000000..037a9de --- /dev/null +++ b/documentation/code_design/Process_Diagram.drawio @@ -0,0 +1,1236 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/documentation/code_design/Process_Diagram.png b/documentation/code_design/Process_Diagram.png new file mode 100644 index 0000000..65f19ac Binary files /dev/null and b/documentation/code_design/Process_Diagram.png differ diff --git a/documentation/examples/1d_mass_spring_oscillator/3D_Visualization.png b/documentation/examples/1d_mass_spring_oscillator/3D_Visualization.png new file mode 100644 index 0000000..0129dcb Binary files /dev/null and b/documentation/examples/1d_mass_spring_oscillator/3D_Visualization.png differ