198 lines
8.9 KiB
Python
198 lines
8.9 KiB
Python
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
|