451 lines
18 KiB
Python
451 lines
18 KiB
Python
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 |