mccdaq
This commit is contained in:
SchrodingerError
parent
482d724e20
commit
9671846e4c
1386
code/setup/README.md
1386
code/setup/README.md
File diff suppressed because it is too large
Load Diff
21
code/setup/code/.vscode/launch.json
vendored
21
code/setup/code/.vscode/launch.json
vendored
@ -1,21 +0,0 @@
|
||||
{
|
||||
// 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"
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
3
code/setup/code/.vscode/settings.json
vendored
3
code/setup/code/.vscode/settings.json
vendored
@ -1,3 +0,0 @@
|
||||
{
|
||||
"python.REPL.enableREPLSmartSend": false
|
||||
}
|
||||
@ -1,197 +0,0 @@
|
||||
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
|
||||
@ -1,147 +0,0 @@
|
||||
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()
|
||||
|
||||
@ -1,200 +0,0 @@
|
||||
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()
|
||||
@ -1,9 +0,0 @@
|
||||
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
|
||||
@ -1,711 +0,0 @@
|
||||
from typing import List, Callable
|
||||
import numpy as np
|
||||
|
||||
|
||||
from typing import TYPE_CHECKING
|
||||
if TYPE_CHECKING:
|
||||
from Physics_Elements import StateSpace, Joint, Spring, SharedRigidActuator, SharedSpring
|
||||
|
||||
ZERO_VECTOR = np.array([0.0, 0.0, 0.0])
|
||||
|
||||
|
||||
'''
|
||||
TODO
|
||||
1. Change spring_force to just force
|
||||
Include getters and setters, lest important for attributes
|
||||
2. Add Object_Sharing objects to this file
|
||||
'''
|
||||
|
||||
|
||||
def stiffness_function_const_then_rigid(length, unstretched_length):
|
||||
percent_length = 0.9 # For the first bit of length in compression, it follors k*x
|
||||
k = 1000
|
||||
|
||||
if length < (1-percent_length)* unstretched_length:
|
||||
# It is really compressed, so make k = k *10
|
||||
k = k + k*10/(1 + np.exp(-10*(unstretched_length - length - percent_length*unstretched_length)))
|
||||
return k
|
||||
|
||||
def damping_force_x(mass: 'Joint', t):
|
||||
x_vel = mass.statespace.get_vel()[0]
|
||||
a = 1
|
||||
b = 0.01
|
||||
return x_vel*a*-1 + x_vel**2 * b*-1
|
||||
|
||||
def damping_force_y(mass: 'Joint', t):
|
||||
y_vel = mass.statespace.get_vel()[1]
|
||||
a = 1
|
||||
b = 0.01
|
||||
return y_vel*a*-1 + y_vel**2 * b*-1
|
||||
|
||||
def damping_force_z(mass: 'Joint', t):
|
||||
z_vel = mass.statespace.get_vel()[2]
|
||||
a = 1
|
||||
b = 0.01
|
||||
return z_vel*a*-1 + z_vel**2 * b*-1
|
||||
|
||||
'''
|
||||
TODO
|
||||
'''
|
||||
class StateSpace():
|
||||
def __init__(self, pos=np.copy(ZERO_VECTOR), vel=np.copy(ZERO_VECTOR), accel=np.copy(ZERO_VECTOR), force=np.copy(ZERO_VECTOR), integration_method="simple"):
|
||||
self.pos = pos
|
||||
self.vel = vel
|
||||
self.accel = accel
|
||||
|
||||
self.force = force
|
||||
|
||||
self.integration_method = self.parse_integration_method(integration_method)
|
||||
|
||||
self.max_saved = 2
|
||||
self.previous_positions: list[list[float, float, float]] = []
|
||||
self.previous_vel: list[list[float, float, float]] = []
|
||||
self.previous_accel: list[list[float, float, float]] = []
|
||||
|
||||
self.verlet_constants = [
|
||||
[],
|
||||
[],
|
||||
[-1, 2, 1],
|
||||
[0, -1, 2, 1],
|
||||
[1/11, -4/11, -6/11, 20/11, 12/11],
|
||||
]
|
||||
|
||||
def parse_integration_method(self, method_str):
|
||||
if method_str == "simple":
|
||||
return 1
|
||||
elif method_str == "verlet":
|
||||
return 2
|
||||
elif method_str == "adams-bashforth":
|
||||
return 3
|
||||
|
||||
|
||||
def integrate(self, delta_t):
|
||||
'''
|
||||
NOTE: This is run after accel has been updated.
|
||||
'''
|
||||
if self.integration_method == 1:
|
||||
self.vel = self.vel + self.accel * delta_t
|
||||
self.pos = self.pos + self.vel * delta_t
|
||||
elif self.integration_method == 2:
|
||||
# Verlet integration
|
||||
self.save_last()
|
||||
if delta_t >= 0.005:
|
||||
self.vel = self.vel + self.accel * delta_t
|
||||
self.pos = self.pos + self.vel * delta_t
|
||||
else:
|
||||
self.vel = self.vel + self.accel * delta_t
|
||||
self.verlet_integration(delta_t)
|
||||
elif self.integration_method == 3: # Adams-Bashforth
|
||||
self.save_last()
|
||||
if delta_t >= 0.005:
|
||||
self.vel = self.vel + self.accel * delta_t
|
||||
self.pos = self.pos + self.vel * delta_t
|
||||
else:
|
||||
self.adams_bashforth_integration(delta_t)
|
||||
|
||||
def print(self):
|
||||
print(f"Pos: {self.pos}, Vel: {self.vel}, Accel: {self.accel}")
|
||||
|
||||
def adams_bashforth_integration(self, delta_t):
|
||||
'''
|
||||
Testing.
|
||||
2 points seems to damp the system. 3 does better at maintaining. Above 3 it is worse
|
||||
'''
|
||||
num_prev_accel = len(self.previous_accel)
|
||||
match num_prev_accel:
|
||||
case 0 | 1:
|
||||
self.vel = self.vel + self.accel * delta_t
|
||||
case 2:
|
||||
self.vel = self.vel + delta_t * (1.5 * self.previous_accel[-1] - 0.5 * self.previous_accel[-2])
|
||||
case 3:
|
||||
self.vel = self.vel + delta_t * (23/12 * self.previous_accel[-1] - 4/3 * self.previous_accel[-2] + 5/12 * self.previous_accel[-3])
|
||||
case 4:
|
||||
self.vel = self.vel + delta_t * (55/24 * self.previous_accel[-1] - 59/24 * self.previous_accel[-2] + 37/24 * self.previous_accel[-3] - 9/24 * self.previous_accel[-4])
|
||||
case 5:
|
||||
self.vel = self.vel + delta_t * ((1901 * self.previous_accel[-1] - 2774 * self.previous_accel[-2] + 2616 * self.previous_accel[-3] - 1274 * self.previous_accel[-4] + 251 * self.previous_accel[-5]) / 720)
|
||||
case _:
|
||||
self.vel = self.vel + self.accel * delta_t
|
||||
self.pos = self.pos + self.vel * delta_t
|
||||
|
||||
def verlet_integration(self, delta_t):
|
||||
num_prev_positions = len(self.previous_positions)
|
||||
match num_prev_positions:
|
||||
case 0 | 1:
|
||||
self.pos = self.pos + self.vel * delta_t
|
||||
case 2:
|
||||
new_pos = np.array([0.0, 0.0, 0.0])
|
||||
for i in range(2):
|
||||
new_pos += self.verlet_constants[2][i] * self.previous_positions[i]
|
||||
new_pos += self.verlet_constants[2][-1] * delta_t * self.accel * delta_t
|
||||
self.pos = new_pos
|
||||
case 3:
|
||||
new_pos = np.array([0.0, 0.0, 0.0])
|
||||
for i in range(3):
|
||||
new_pos += self.verlet_constants[3][i] * self.previous_positions[i]
|
||||
new_pos += self.verlet_constants[3][-1] * delta_t * self.accel * delta_t
|
||||
self.pos = new_pos
|
||||
case 4:
|
||||
new_pos = np.array([0.0, 0.0, 0.0])
|
||||
for i in range(4):
|
||||
new_pos += self.verlet_constants[4][i] * self.previous_positions[i]
|
||||
new_pos += self.verlet_constants[4][-1] * delta_t * self.accel * delta_t
|
||||
self.pos = new_pos
|
||||
case _:
|
||||
self.pos = self.pos + self.vel * delta_t
|
||||
|
||||
def save_last(self):
|
||||
self.previous_positions.append(np.copy(self.pos))
|
||||
self.previous_positions = self.previous_positions[-1*self.max_saved:]
|
||||
self.previous_vel.append(np.copy(self.vel))
|
||||
self.previous_vel = self.previous_vel[-1*self.max_saved:]
|
||||
self.previous_accel.append(np.copy(self.accel))
|
||||
self.previous_accel = self.previous_accel[-1*self.max_saved:]
|
||||
|
||||
def save_last_statespace(self):
|
||||
if self.last_statespace is None:
|
||||
self.last_statespace = StateSpace()
|
||||
self.last_statespace.pos = np.copy(self.pos)
|
||||
self.last_statespace.vel = np.copy(self.vel)
|
||||
self.last_statespace.accel = np.copy(self.accel)
|
||||
|
||||
def set_pos(self, new_pos):
|
||||
self.pos = new_pos
|
||||
def set_vel(self, new_vel):
|
||||
self.vel = new_vel
|
||||
def set_accel(self, new_accel):
|
||||
self.accel = new_accel
|
||||
def set_force(self, new_force):
|
||||
self.force = new_force
|
||||
|
||||
def get_pos(self):
|
||||
return self.pos
|
||||
def get_vel(self):
|
||||
return self.vel
|
||||
def get_accel(self):
|
||||
return self.accel
|
||||
def get_force(self):
|
||||
return self.force
|
||||
|
||||
class Joint():
|
||||
def __init__(self, pos=np.copy(ZERO_VECTOR), mass: float=0.1, fixed: bool=False, name: str="Joint", integration_method:str="simple"):
|
||||
self.name = name
|
||||
self.statespace = StateSpace(pos, ZERO_VECTOR, ZERO_VECTOR, ZERO_VECTOR, integration_method=integration_method)
|
||||
|
||||
self.connected_springs: List[Spring] = []
|
||||
self.constant_external_forces: List[List[float, float, float]] = []
|
||||
|
||||
self.connected_actuators: List[RigidActuator] = []
|
||||
|
||||
self.variable_external_forces = [[], [], []]
|
||||
|
||||
self.fixed = fixed
|
||||
self.connected_to_rigid_actuator = False
|
||||
self.mass = mass
|
||||
|
||||
self.parent_joints: List[Joint] = []
|
||||
self.child_joints: List[Joint] = []
|
||||
|
||||
self.damping = False
|
||||
|
||||
self.sharing_attributes = False
|
||||
self.shared_attributes = None
|
||||
|
||||
def is_same(self, other_joint: 'Joint'):
|
||||
if self.name == other_joint.get_name():
|
||||
return True
|
||||
return False
|
||||
|
||||
def get_statespace(self):
|
||||
return self.statespace
|
||||
|
||||
def get_pos(self):
|
||||
return self.statespace.get_pos()
|
||||
|
||||
def get_vel(self):
|
||||
return self.statespace.get_vel()
|
||||
|
||||
def get_accel(self):
|
||||
return self.statespace.get_accel()
|
||||
|
||||
def set_state_space_pos(self, new_pos):
|
||||
self.statespace.set_pos(new_pos)
|
||||
|
||||
def set_state_space_vel(self, new_vel):
|
||||
self.statespace.set_vel(new_vel)
|
||||
|
||||
def set_connected_to_rigid_actuator(self, state):
|
||||
self.connected_to_rigid_actuator = state
|
||||
|
||||
def get_connected_springs(self):
|
||||
return self.connected_springs
|
||||
|
||||
def get_connected_actuators(self):
|
||||
return self.connected_actuators
|
||||
|
||||
def get_name(self):
|
||||
return self.name
|
||||
|
||||
def add_spring(self, new_spring):
|
||||
self.connected_springs.append(new_spring)
|
||||
|
||||
def add_actuator(self, new_actuator):
|
||||
self.connected_actuators.append(new_actuator)
|
||||
|
||||
def add_constant_force(self, new_force):
|
||||
self.constant_external_forces.append(new_force)
|
||||
|
||||
def integrate_statespace(self, delta_t):
|
||||
self.statespace.integrate(delta_t)
|
||||
|
||||
def add_gravity(self):
|
||||
gravity_force = np.array([
|
||||
0,
|
||||
-9.81 * self.mass,
|
||||
0
|
||||
])
|
||||
self.add_constant_force(gravity_force)
|
||||
|
||||
def add_variable_force(self, force_vect: list):
|
||||
if force_vect[0] is not None:
|
||||
self.variable_external_forces[0].append(force_vect[0])
|
||||
if force_vect[1] is not None:
|
||||
self.variable_external_forces[1].append(force_vect[1])
|
||||
if force_vect[2] is not None:
|
||||
self.variable_external_forces[2].append(force_vect[2])
|
||||
|
||||
def calc_net_spring_force(self):
|
||||
net_spring_force = 0
|
||||
for spring in self.get_connected_springs():
|
||||
if self.is_same(spring.get_parent_joint()):
|
||||
spring_force = spring.get_spring_force() # This is the force the spring exerts on the joint ASSUMING self is parent_joint of the spring
|
||||
else:
|
||||
spring_force = -1*spring.get_spring_force()
|
||||
net_spring_force += spring_force
|
||||
return net_spring_force
|
||||
|
||||
def calc_net_constant_external_force(self):
|
||||
net_external_force = np.copy(ZERO_VECTOR)
|
||||
for force in self.constant_external_forces:
|
||||
net_external_force += force
|
||||
return net_external_force
|
||||
|
||||
def calc_net_variable_external_force(self, t):
|
||||
net_variable_external_force = np.array([0.0, 0.0, 0.0])
|
||||
for i in range(3):
|
||||
for func in self.variable_external_forces[i]:
|
||||
force = func(t)
|
||||
net_variable_external_force[i] += force
|
||||
return net_variable_external_force
|
||||
|
||||
def calc_net_force(self, t):
|
||||
net_force = np.copy(ZERO_VECTOR)
|
||||
if len(self.constant_external_forces) != 0:
|
||||
net_force += self.calc_net_constant_external_force()
|
||||
if len(self.variable_external_forces) != 0:
|
||||
net_force += self.calc_net_variable_external_force(t)
|
||||
|
||||
net_spring_force = self.calc_net_spring_force()
|
||||
|
||||
if self.damping == True:
|
||||
net_force += self.calc_damping(net_spring_force)
|
||||
|
||||
net_force += net_spring_force
|
||||
|
||||
self.statespace.set_force(net_force)
|
||||
return self.statespace.get_force()
|
||||
|
||||
def add_damping(self, vel_ratio=0.25, mom_ratio=None):
|
||||
self.damping = True
|
||||
if mom_ratio is not None:
|
||||
self.damping_vel_ratio = self.mass * mom_ratio
|
||||
else:
|
||||
self.damping_vel_ratio = vel_ratio
|
||||
|
||||
def calc_damping(self, spring_force):
|
||||
vel_damping = self.damping_vel_ratio * (self.statespace.get_vel()) * -1
|
||||
|
||||
return vel_damping
|
||||
|
||||
def calc_accel(self):
|
||||
if self.fixed == False and self.connected_to_rigid_actuator == False:
|
||||
self.statespace.set_accel(self.statespace.get_force() / self.mass)
|
||||
|
||||
def is_in_list(self, joints: list['Joint']):
|
||||
for j in joints:
|
||||
if self.is_same(j):
|
||||
return True
|
||||
return False
|
||||
|
||||
def find_parent_joints(self):
|
||||
for spring in self.connected_springs:
|
||||
if self.is_same(spring.get_child_joint()):
|
||||
parent_joint = spring.get_parent_joint()
|
||||
if not parent_joint.is_in_list(self.parent_joints):
|
||||
self.parent_joints.append(parent_joint)
|
||||
|
||||
def find_child_joints(self):
|
||||
for spring in self.connected_springs:
|
||||
if self.is_same(spring.get_parent_joint()):
|
||||
child_joint = spring.get_child_joint()
|
||||
if not child_joint.is_in_list(self.child_joints):
|
||||
self.child_joints.append(child_joint)
|
||||
|
||||
def print_attributes(self):
|
||||
print(f"Joint: {self.name}")
|
||||
print(f"Force: {self.statespace.get_force()}")
|
||||
print(f"Accel: {self.statespace.get_accel()}")
|
||||
print(f"Vel: {self.statespace.get_vel()}")
|
||||
print(f"Pos: {self.statespace.get_pos()}")
|
||||
|
||||
def attach_shared_attributes(self, shared_attributes: 'SharedJoint'):
|
||||
if not isinstance(shared_attributes._getvalue(), SharedJoint):
|
||||
raise TypeError(f"shared_attributes for Joint {self.name} is not a SharedJoint")
|
||||
self.sharing_attributes = True
|
||||
self.shared_attributes = shared_attributes
|
||||
|
||||
self.shared_attributes.set_name(f"{self.name}")
|
||||
|
||||
self.update_shared_attributes()
|
||||
|
||||
def update_shared_attributes(self):
|
||||
if self.shared_attributes is not None:
|
||||
self.shared_attributes.set_statespace(self.statespace)
|
||||
|
||||
class Spring():
|
||||
def __init__(self, parent_joint: 'Joint', child_joint: 'Joint', unstretched_length: float=0, constant_stiffness: float = None, stiffness_func: Callable[[float, float], float]=None, name: str="Spring"):
|
||||
self.name = name
|
||||
|
||||
self.parent_joint = parent_joint
|
||||
self.child_joint = child_joint
|
||||
|
||||
self.unstretched_length = unstretched_length
|
||||
self.stiffness_func = stiffness_func
|
||||
self.constant_stiffness = constant_stiffness
|
||||
self.current_stiffness = 0.0
|
||||
|
||||
self.vector = self.calc_r_vector()
|
||||
self.length = self.calc_length()
|
||||
self.unit_vector: list[float] = self.calc_r_unit_vector()
|
||||
self.spring_force_scalar: float = 0.0
|
||||
self.spring_force: list[float] = self.calc_spring_force()
|
||||
|
||||
|
||||
self.parent_joint.add_spring(self)
|
||||
self.child_joint.add_spring(self)
|
||||
|
||||
self.sharing_attributes = False
|
||||
self.shared_attributes: 'SharedSpring' = None
|
||||
|
||||
def get_name(self):
|
||||
return self.name
|
||||
|
||||
def get_parent_joint(self):
|
||||
return self.parent_joint
|
||||
|
||||
def get_child_joint(self):
|
||||
return self.child_joint
|
||||
|
||||
def get_spring_force(self):
|
||||
return self.spring_force
|
||||
|
||||
def calc_r_vector(self):
|
||||
return self.child_joint.get_statespace().get_pos() - self.parent_joint.get_statespace().get_pos()
|
||||
|
||||
def calc_length(self):
|
||||
return ((self.vector[0])**2 + (self.vector[1])**2 + (self.vector[2])**2) ** 0.5
|
||||
|
||||
def calc_r_unit_vector(self):
|
||||
return self.vector / self.length
|
||||
|
||||
def calc_stiffness(self):
|
||||
if self.stiffness_func is not None:
|
||||
return self.stiffness_func(self.length, self.unstretched_length)
|
||||
else:
|
||||
return self.constant_stiffness
|
||||
|
||||
def calc_spring_force(self):
|
||||
'''Positive force is in tension. Aka, the spring PULLS on the other object'''
|
||||
self.length = self.calc_length()
|
||||
del_length = self.length - self.unstretched_length
|
||||
self.current_stiffness = self.calc_stiffness()
|
||||
self.spring_force_scalar = del_length * self.current_stiffness
|
||||
|
||||
self.vector = self.calc_r_vector()
|
||||
self.r_unit_vector = self.calc_r_unit_vector()
|
||||
self.spring_force = self.spring_force_scalar * self.r_unit_vector
|
||||
return self.spring_force
|
||||
|
||||
def print_attributes(self):
|
||||
print(f"Spring: {self.name}")
|
||||
print(f"Length: {self.length}")
|
||||
print(f"Spring Force: {self.spring_force}")
|
||||
|
||||
def get_pos(self):
|
||||
return self.parent_statespace.get_pos()
|
||||
def get_axis_vector(self):
|
||||
return self.child_statespace.get_pos() - self.parent_statespace.get_pos()
|
||||
|
||||
def attach_shared_attributes(self, shared_attributes: 'SharedSpring'):
|
||||
if not isinstance(shared_attributes._getvalue(), SharedSpring):
|
||||
raise TypeError(f"shared_attributes for Spring {self.name} is not a SharedSpring")
|
||||
self.sharing_attributes = True
|
||||
self.shared_attributes = shared_attributes
|
||||
|
||||
self.update_shared_attributes()
|
||||
|
||||
def update_shared_attributes(self):
|
||||
if self.shared_attributes is not None:
|
||||
self.shared_attributes.set_child_statespace(self.child_joint.get_statespace())
|
||||
self.shared_attributes.set_parent_statespace(self.parent_joint.get_statespace())
|
||||
self.shared_attributes.set_spring_force(self.spring_force)
|
||||
self.shared_attributes.set_spring_force_scalar(self.spring_force_scalar)
|
||||
self.shared_attributes.set_length(self.length)
|
||||
self.shared_attributes.set_unstretched_length(self.unstretched_length)
|
||||
self.shared_attributes.set_stiffness(self.current_stiffness)
|
||||
|
||||
def is_same(self, other_spring: 'Spring'):
|
||||
if self.name == other_spring.get_name():
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
def is_in_list(self, springs: list['Spring']):
|
||||
for j in springs:
|
||||
if self.is_same(j):
|
||||
return True
|
||||
return False
|
||||
|
||||
'''
|
||||
It connects 2 joints.
|
||||
1 joint is completely fixed. Cannot move. Ever. It will be referred to as "grounded".
|
||||
The other joint is contrained to the grounded joint. The actuator controls the vector displacement <x,y,z> from the grounded joint
|
||||
'''
|
||||
class RigidActuator():
|
||||
def __init__(self, parent_joint: 'Joint', grounded_joint: 'Joint', name: str = "Rigid Actuator", max_length:float=1, min_length:float=0.1, control_code=0):
|
||||
self.name = name
|
||||
|
||||
self.parent_joint = parent_joint
|
||||
self.grounded_joint = grounded_joint # Same as child joint
|
||||
|
||||
self.parent_joint.add_actuator(self)
|
||||
self.parent_joint.set_connected_to_rigid_actuator(True)
|
||||
self.grounded_joint.add_actuator(self)
|
||||
self.grounded_joint.set_connected_to_rigid_actuator(True)
|
||||
|
||||
self.max_length = max_length
|
||||
self.min_length = min_length
|
||||
self.control_code = control_code # 0 for pos, 1 for vel, 2 for accel
|
||||
|
||||
self.calc_r_unit_vector()
|
||||
|
||||
self.shared_attributes: 'SharedRigidActuator' = None
|
||||
|
||||
def get_name(self):
|
||||
return self.name
|
||||
|
||||
def get_parent_joint(self):
|
||||
return self.parent_joint
|
||||
|
||||
def get_child_joint(self):
|
||||
return self.grounded_joint
|
||||
|
||||
def is_same(self, other: 'RigidActuator'):
|
||||
if self.name == other.get_name():
|
||||
return True
|
||||
return False
|
||||
|
||||
def is_in_list(self, actuators: list['RigidActuator']):
|
||||
for j in actuators:
|
||||
if self.is_same(j):
|
||||
return True
|
||||
return False
|
||||
|
||||
def calc_r_vector(self):
|
||||
return self.grounded_joint.get_statespace().get_pos() - self.parent_joint.get_statespace().get_pos()
|
||||
|
||||
def calc_length(self):
|
||||
return ((self.vector[0])**2 + (self.vector[1])**2 + (self.vector[2])**2) ** 0.5
|
||||
|
||||
def calc_r_unit_vector(self):
|
||||
self.vector = self.calc_r_vector()
|
||||
self.length = self.calc_length()
|
||||
return self.vector / self.length
|
||||
|
||||
def get_pos(self):
|
||||
return self.parent_joint.get_statespace().get_pos()
|
||||
|
||||
def get_axis_vector(self):
|
||||
return self.grounded_joint.get_statespace().get_pos() - self.parent_joint.get_statespace().get_pos()
|
||||
|
||||
def get_r_unit_vector(self):
|
||||
return self.calc_r_unit_vector()
|
||||
|
||||
def update_shared_attributes(self):
|
||||
# These 2 lines are used to make sure we set the internal properties correctly
|
||||
self.vector = self.calc_r_vector()
|
||||
self.length = self.calc_length()
|
||||
# END
|
||||
if self.shared_attributes is not None:
|
||||
self.shared_attributes.set_child_statespace(self.grounded_joint.get_statespace())
|
||||
self.shared_attributes.set_parent_statespace(self.parent_joint.get_statespace())
|
||||
self.shared_attributes.set_length(self.length)
|
||||
|
||||
def attach_shared_attributes(self, shared_attributes: 'SharedRigidActuator'):
|
||||
if not isinstance(shared_attributes._getvalue(), SharedRigidActuator):
|
||||
raise TypeError(f"shared_attributes for RigidActuator {self.name} is not a SharedRigidActuator")
|
||||
self.sharing_attributes = True
|
||||
self.shared_attributes = shared_attributes
|
||||
|
||||
self.update_shared_attributes()
|
||||
|
||||
def set_parent_pos(self, new_pos):
|
||||
'''Set the position of the parent joint's statestpace'''
|
||||
self.parent_joint.set_state_space_pos(new_pos)
|
||||
|
||||
def update_from_controller(self):
|
||||
if self.control_code == 0: # Controller controls pos
|
||||
self.set_pos_to_controlled_pos()
|
||||
elif self.control_code == 1: # Controller controls vel
|
||||
self.set_vel_to_controlled_vel()
|
||||
elif self.control_code == 2: # Controls controls accel
|
||||
pass
|
||||
|
||||
def set_vel_to_controlled_vel(self):
|
||||
controlled_vel = self.shared_attributes.get_vel()
|
||||
self.parent_joint.set_state_space_vel(controlled_vel)
|
||||
|
||||
def set_pos_to_controlled_pos(self):
|
||||
controlled_pos = self.shared_attributes.get_pos()
|
||||
self.parent_joint.set_state_space_pos(controlled_pos)
|
||||
|
||||
|
||||
class SharedPhysicsElement():
|
||||
def __init__(self) -> None:
|
||||
self.name: str = None
|
||||
self.statespace: StateSpace = None
|
||||
|
||||
self.connected_to_plotter: bool = False
|
||||
self.connected_to_visualization: bool = False
|
||||
self.connected_to_sensor: bool = False
|
||||
self.connected_to_controller: bool = False
|
||||
|
||||
def set_name(self, name:str) -> None:
|
||||
self.name = name
|
||||
def set_statespace(self, statespace:StateSpace) -> None:
|
||||
self.statespace = statespace
|
||||
def set_connected_to_plotter(self, state: bool) -> None:
|
||||
self.connected_to_plotter = state
|
||||
def set_connected_to_visualization(self, state: bool) -> None:
|
||||
self.connected_to_visualization = state
|
||||
def set_connected_to_sensor(self, state: bool) -> None:
|
||||
self.connected_to_sensor = state
|
||||
def set_connected_to_controller(self, state: bool) -> None:
|
||||
self.connected_to_controller = state
|
||||
|
||||
def get_name(self) -> str:
|
||||
return self.name
|
||||
def get_statespace(self) -> StateSpace:
|
||||
return self.statespace
|
||||
def get_pos(self) -> List[float]:
|
||||
return self.statespace.get_pos()
|
||||
def get_vel(self) -> List[float]:
|
||||
return self.statespace.get_vel()
|
||||
def get_accel(self) -> List[float]:
|
||||
return self.statespace.get_accel()
|
||||
def get_force(self) -> List[float]:
|
||||
return self.statespace.get_force()
|
||||
def get_connected_to_plotter(self) -> bool:
|
||||
return self.connected_to_plotter
|
||||
def get_connected_to_visualization(self) -> bool:
|
||||
return self.connected_to_visualization
|
||||
def get_connected_to_sensor(self) -> bool:
|
||||
return self.connected_to_sensor
|
||||
def get_connected_to_controller(self) -> bool:
|
||||
return self.connected_to_controller
|
||||
|
||||
class SharedJoint(SharedPhysicsElement):
|
||||
def _init__(self) -> None:
|
||||
super().__init__()
|
||||
|
||||
class SharedSpring(SharedPhysicsElement):
|
||||
'''self.statespace is the statespace of the parent'''
|
||||
def _init__(self) -> None:
|
||||
super().__init__()
|
||||
self.child_statespace: StateSpace = None
|
||||
|
||||
self.unstretched_length: float = None
|
||||
self.length: float = None
|
||||
self.stiffness: float = None
|
||||
|
||||
self.spring_force: float = None
|
||||
|
||||
def set_parent_statespace(self, statespace:StateSpace) -> None:
|
||||
self.statespace = statespace
|
||||
def set_child_statespace(self, statespace:StateSpace) -> None:
|
||||
self.child_statespace = statespace
|
||||
def set_unstretched_length(self, unstretched_length: float) -> None:
|
||||
self.unstretched_length = unstretched_length
|
||||
def set_length(self, length: float) -> None:
|
||||
self.length = length
|
||||
def set_stiffness(self, stiffness: float) -> None:
|
||||
self.stiffness = stiffness
|
||||
def set_spring_force(self, set_spring_force: float) -> None:
|
||||
self.spring_force = set_spring_force
|
||||
def set_spring_force_scalar(self, scalar_force: float) -> None:
|
||||
self.spring_force_scalar = scalar_force
|
||||
|
||||
def get_parent_statespace(self) -> StateSpace:
|
||||
return self.statespace
|
||||
def get_child_statespace(self) -> StateSpace:
|
||||
return self.child_statespace
|
||||
def get_unstretched_length(self) -> float:
|
||||
return self.unstretched_length
|
||||
def get_length(self) -> float:
|
||||
return self.length
|
||||
def get_stiffness(self) -> float:
|
||||
return self.stiffness
|
||||
def get_spring_force(self) -> list[float]:
|
||||
return self.spring_force
|
||||
def get_spring_force_scalar(self) -> float:
|
||||
return self.spring_force_scalar
|
||||
def get_axis_vector(self) -> List[float]:
|
||||
return self.child_statespace.get_pos() - self.statespace.get_pos()
|
||||
|
||||
|
||||
class SharedRigidActuator(SharedPhysicsElement):
|
||||
'''self.statespace is the statespace of the parent'''
|
||||
def _init__(self) -> None:
|
||||
super().__init__()
|
||||
self.child_statespace: StateSpace = None
|
||||
|
||||
self.length: float = None
|
||||
|
||||
def set_parent_statespace(self, statespace:StateSpace) -> None:
|
||||
self.statespace = statespace
|
||||
def set_parent_statespace_pos(self, new):
|
||||
self.statespace.set_pos(new)
|
||||
def set_parent_statespace_vel(self, new):
|
||||
self.statespace.set_vel(new)
|
||||
def set_child_statespace(self, statespace:StateSpace) -> None:
|
||||
self.child_statespace = statespace
|
||||
def set_length(self, length: float) -> None:
|
||||
self.length = length
|
||||
|
||||
def get_parent_statespace(self) -> StateSpace:
|
||||
return self.statespace
|
||||
def get_child_statespace(self) -> StateSpace:
|
||||
return self.child_statespace
|
||||
def get_length(self) -> float:
|
||||
return self.length
|
||||
def get_axis_vector(self) -> List[float]:
|
||||
return self.child_statespace.get_pos() - self.statespace.get_pos()
|
||||
def get_r_unit_vector(self):
|
||||
return self.calc_r_unit_vector()
|
||||
def calc_r_vector(self):
|
||||
return self.child_statespace.get_pos() - self.statespace.get_pos()
|
||||
def calc_length(self):
|
||||
return ((self.vector[0])**2 + (self.vector[1])**2 + (self.vector[2])**2) ** 0.5
|
||||
def calc_r_unit_vector(self):
|
||||
self.vector = self.calc_r_vector()
|
||||
self.length = self.calc_length()
|
||||
return self.vector / self.length
|
||||
@ -1,451 +0,0 @@
|
||||
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
|
||||
@ -1,236 +0,0 @@
|
||||
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}")
|
||||
@ -1,594 +0,0 @@
|
||||
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()
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -1,268 +0,0 @@
|
||||
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()
|
||||
@ -1,430 +0,0 @@
|
||||
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()
|
||||
@ -1,649 +0,0 @@
|
||||
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()
|
||||
@ -1,514 +0,0 @@
|
||||
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()
|
||||
|
||||
@ -1,112 +0,0 @@
|
||||
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()
|
||||
@ -1,436 +0,0 @@
|
||||
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()
|
||||
|
||||
@ -1,30 +0,0 @@
|
||||
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))
|
||||
@ -1,111 +0,0 @@
|
||||
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()
|
||||
@ -1,59 +0,0 @@
|
||||
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()
|
||||
@ -1,8 +0,0 @@
|
||||
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)
|
||||
@ -1,78 +0,0 @@
|
||||
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()
|
||||
@ -1,99 +0,0 @@
|
||||
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()
|
||||
@ -1,3 +0,0 @@
|
||||
{
|
||||
"python.REPL.enableREPLSmartSend": false
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -1,23 +0,0 @@
|
||||
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.")
|
||||
Binary file not shown.
@ -1,29 +0,0 @@
|
||||
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)
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -1,149 +0,0 @@
|
||||
<mxfile host="Electron" modified="2024-06-11T19:31:46.378Z" agent="Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/24.0.4 Chrome/120.0.6099.109 Electron/28.1.0 Safari/537.36" etag="YiW67u71xuPxX2CrrYUi" version="24.0.4" type="device">
|
||||
<diagram name="Page-1" id="e7e014a7-5840-1c2e-5031-d8a46d1fe8dd">
|
||||
<mxGraphModel dx="2074" dy="1204" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="1169" pageHeight="826" background="none" math="0" shadow="0">
|
||||
<root>
|
||||
<mxCell id="0" />
|
||||
<mxCell id="1" parent="0" />
|
||||
<mxCell id="2" value="Thread 1" style="swimlane;whiteSpace=wrap" parent="1" vertex="1">
|
||||
<mxGeometry x="164.5" y="128" width="280" height="570" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="5" value="" style="ellipse;shape=startState;fillColor=#000000;strokeColor=#ff0000;" parent="2" vertex="1">
|
||||
<mxGeometry x="100" y="40" width="30" height="30" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="6" value="" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;verticalAlign=bottom;endArrow=open;endSize=8;strokeColor=#FF0000;endFill=1;rounded=0" parent="2" source="5" target="7" edge="1">
|
||||
<mxGeometry x="100" y="40" as="geometry">
|
||||
<mxPoint x="115" y="110" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="7" value="idle" style="" parent="2" vertex="1">
|
||||
<mxGeometry x="60" y="110" width="110" height="50" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="8" value="user action" style="" parent="2" vertex="1">
|
||||
<mxGeometry x="60" y="220" width="110" height="50" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="9" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="2" source="7" target="8" edge="1">
|
||||
<mxGeometry relative="1" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="10" value="post command" style="" parent="2" vertex="1">
|
||||
<mxGeometry x="60" y="325" width="110" height="50" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="11" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="2" source="8" target="10" edge="1">
|
||||
<mxGeometry relative="1" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="12" value="" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;strokeColor=#FF0000;endArrow=open;endFill=1;rounded=0" parent="2" source="10" target="7" edge="1">
|
||||
<mxGeometry width="100" height="100" relative="1" as="geometry">
|
||||
<mxPoint x="160" y="290" as="sourcePoint" />
|
||||
<mxPoint x="260" y="190" as="targetPoint" />
|
||||
<Array as="points">
|
||||
<mxPoint x="30" y="250" />
|
||||
</Array>
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="3" value="Thread 2" style="swimlane;whiteSpace=wrap" parent="1" vertex="1">
|
||||
<mxGeometry x="444.5" y="128" width="280" height="570" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="13" value="" style="ellipse;shape=startState;fillColor=#000000;strokeColor=#ff0000;" parent="3" vertex="1">
|
||||
<mxGeometry x="60" y="40" width="30" height="30" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="14" value="" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;verticalAlign=bottom;endArrow=open;endSize=8;strokeColor=#FF0000;endFill=1;rounded=0" parent="3" source="13" target="15" edge="1">
|
||||
<mxGeometry x="40" y="20" as="geometry">
|
||||
<mxPoint x="55" y="90" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="15" value="idle" style="" parent="3" vertex="1">
|
||||
<mxGeometry x="20" y="110" width="110" height="50" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="16" value="check for 
new commands" style="" parent="3" vertex="1">
|
||||
<mxGeometry x="20" y="220" width="110" height="50" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="17" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="3" source="15" target="16" edge="1">
|
||||
<mxGeometry relative="1" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="18" value="command queue" style="" parent="3" vertex="1">
|
||||
<mxGeometry x="20" y="325" width="110" height="50" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="19" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="3" source="16" target="18" edge="1">
|
||||
<mxGeometry relative="1" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="21" value="queue empty" style="rhombus;fillColor=#ffffc0;strokeColor=#ff0000;" parent="3" vertex="1">
|
||||
<mxGeometry x="150" y="225" width="80" height="40" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="22" value="yes" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;align=left;verticalAlign=bottom;endArrow=open;endSize=8;strokeColor=#FF0000;exitX=0.5;exitY=0;endFill=1;rounded=0;entryX=0.75;entryY=0.5;entryPerimeter=0" parent="3" source="21" target="25" edge="1">
|
||||
<mxGeometry x="-1" relative="1" as="geometry">
|
||||
<mxPoint x="160" y="150" as="targetPoint" />
|
||||
<Array as="points">
|
||||
<mxPoint x="190" y="180" />
|
||||
</Array>
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="23" value="no" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;align=left;verticalAlign=top;endArrow=open;endSize=8;strokeColor=#FF0000;endFill=1;rounded=0" parent="3" source="21" target="30" edge="1">
|
||||
<mxGeometry x="-1" relative="1" as="geometry">
|
||||
<mxPoint x="190" y="305" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="24" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="3" source="16" target="21" edge="1">
|
||||
<mxGeometry relative="1" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="25" value="" style="shape=line;strokeWidth=6;strokeColor=#ff0000;rotation=90" parent="3" vertex="1">
|
||||
<mxGeometry x="130" y="127.5" width="50" height="15" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="26" value="" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;verticalAlign=bottom;endArrow=open;endSize=8;strokeColor=#FF0000;endFill=1;rounded=0" parent="3" source="25" target="15" edge="1">
|
||||
<mxGeometry x="130" y="90" as="geometry">
|
||||
<mxPoint x="230" y="140" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="30" value="dispatch
command
worker thread" style="" parent="3" vertex="1">
|
||||
<mxGeometry x="140" y="325" width="110" height="50" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="31" value="critical
section" style="shape=note;whiteSpace=wrap;size=17" parent="3" vertex="1">
|
||||
<mxGeometry x="105" y="490" width="100" height="50" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="32" value="" style="endArrow=none;strokeColor=#FF0000;endFill=0;rounded=0;dashed=1" parent="3" source="18" target="31" edge="1">
|
||||
<mxGeometry relative="1" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="34" value="" style="whiteSpace=wrap;strokeColor=#FF0000;fillColor=#FF0000" parent="3" vertex="1">
|
||||
<mxGeometry x="245" y="395" width="5" height="45" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="41" value="" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;entryX=0;entryY=0.5;strokeColor=#FF0000;endArrow=open;endFill=1;rounded=0" parent="3" source="30" target="34" edge="1">
|
||||
<mxGeometry width="100" height="100" relative="1" as="geometry">
|
||||
<mxPoint x="60" y="480" as="sourcePoint" />
|
||||
<mxPoint x="160" y="380" as="targetPoint" />
|
||||
<Array as="points">
|
||||
<mxPoint x="195" y="400" />
|
||||
</Array>
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="4" value="Thread 3" style="swimlane;whiteSpace=wrap" parent="1" vertex="1">
|
||||
<mxGeometry x="724.5" y="128" width="280" height="570" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="33" value="process
command" style="" parent="4" vertex="1">
|
||||
<mxGeometry x="90" y="405" width="110" height="50" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="35" value="" style="edgeStyle=elbowEdgeStyle;elbow=horizontal;strokeColor=#FF0000;endArrow=open;endFill=1;rounded=0;entryX=0.25;entryY=0.5;entryPerimeter=0" parent="4" target="25" edge="1">
|
||||
<mxGeometry width="100" height="100" relative="1" as="geometry">
|
||||
<mxPoint x="-30" y="410" as="sourcePoint" />
|
||||
<mxPoint x="-120" y="120" as="targetPoint" />
|
||||
<Array as="points">
|
||||
<mxPoint x="-10" y="135" />
|
||||
</Array>
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="37" value="" style="edgeStyle=none;strokeColor=#FF0000;endArrow=open;endFill=1;rounded=0" parent="4" target="33" edge="1">
|
||||
<mxGeometry width="100" height="100" relative="1" as="geometry">
|
||||
<mxPoint x="-30" y="429.5" as="sourcePoint" />
|
||||
<mxPoint x="90" y="429.5" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="38" value="" style="ellipse;shape=endState;fillColor=#000000;strokeColor=#ff0000" parent="4" vertex="1">
|
||||
<mxGeometry x="130" y="500" width="30" height="30" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="39" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="4" source="33" target="38" edge="1">
|
||||
<mxGeometry relative="1" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="20" value="" style="endArrow=open;strokeColor=#FF0000;endFill=1;rounded=0" parent="1" source="10" target="18" edge="1">
|
||||
<mxGeometry relative="1" as="geometry" />
|
||||
</mxCell>
|
||||
</root>
|
||||
</mxGraphModel>
|
||||
</diagram>
|
||||
</mxfile>
|
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
Before Width: | Height: | Size: 6.4 MiB |
Binary file not shown.
|
Before Width: | Height: | Size: 38 KiB |
BIN
code/setup/mccdaq.exe
Normal file
BIN
code/setup/mccdaq.exe
Normal file
Binary file not shown.
Loading…
Reference in New Issue
Block a user