def __init__(self,
data_container: Type[exoboot.Exo.DataContainer],
acc_threshold_x: float = 0.2,
time_out: float = 5,
max_acc_y: float = 0.1,
max_acc_z: float = 0.1,
do_filter_accels=True,
required_seconds_of_stillness=0,
return_did_slip=False,
start_active=False):
'''Last working with 0.5, 0.2, 0.2, no filter.'''
self.data_container = data_container
self.acc_threshold_x = acc_threshold_x
self.max_acc_y = max_acc_y
self.max_acc_z = max_acc_z
self.refractory_timer = util.DelayTimer(time_out, true_until=True)
self.do_filter_accels = do_filter_accels
self.return_did_slip = return_did_slip
self.accel_x_filter = filters.Butterworth(
N=2, Wn=0.01, btype='high')
self.accel_y_filter = filters.Butterworth(
N=2, Wn=0.01, btype='high')
self.accel_z_filter = filters.Butterworth(
N=2, Wn=0.01, btype='high')
self.slip_detect_active = start_active
print('slip_detect_active: ', self.slip_detect_active)
self.shuffling_timer = util.DelayTimer(
delay_time=required_seconds_of_stillness)
def __init__(self, target_freq, threshold: float = 4, min_phase: float = 0.5):
'''Uses peak plantarflexion angle to determine toe-off. Warning--does not work well when actuated!'''
self.threshold = threshold
self.min_phase = min_phase
self.ankle_angle_history = deque([0, 0, 0], maxlen=3)
self.ankle_angle_filter = filters.Butterworth(
N=2, Wn=10/(target_freq/2))
def __init__(self,
exo_1: Type[exoboot.Exo],
exo_2: Type[exoboot.Exo],
acc_threshold_x: float = 0.2, # 0.2
time_out: float = 5,
max_acc_y: float = 0.1, # 0.1
max_acc_z: float = 0.1, # 0.1
do_filter_accels=True,
required_seconds_of_stillness=0,
return_did_slip=False,
start_active=False):
self.exo_list = [exo_1, exo_2]
self.acc_threshold_x = acc_threshold_x
self.max_acc_y = max_acc_y
self.max_acc_z = max_acc_z
self.refractory_timer = util.DelayTimer(time_out, true_until=True)
self.do_filter_accels = do_filter_accels
self.return_did_slip = return_did_slip
self.filter_list = [[
filters.Butterworth(N=2, Wn=0.01, btype='high'),
filters.Butterworth(N=2, Wn=0.01, btype='high'),
filters.Butterworth(N=2, Wn=0.01, btype='high')],
[filters.Butterworth(N=2, Wn=0.01, btype='high'),
filters.Butterworth(N=2, Wn=0.01, btype='high'),
filters.Butterworth(N=2, Wn=0.01, btype='high')]]
self.slip_detect_active = start_active
print('slip_detect_active: ', self.slip_detect_active)
self.shuffling_timer = util.DelayTimer(
delay_time=required_seconds_of_stillness,
true_until=True)
self.print_counter = 0
def __init__(self,
exo: Exo,
k_val: int,
b_val: int = 0,
Kp: int = constants.DEFAULT_KP,
Ki: int = constants.DEFAULT_KI,
Kd: int = constants.DEFAULT_KD,
ff: int = constants.DEFAULT_FF):
self.exo = exo
self.k_val = k_val
self.b_val = b_val
super().update_controller_gains(Kp=Kp, Ki=Ki, Kd=Kd, ff=ff)
self.ankle_angles = deque(maxlen=3) # looking for peak in pf
self.ankle_angle_filter = filters.Butterworth(N=2, Wn=0.2)
def test_Butterworth(self):
# Ensure the realtime filter behaves like scipy.sosfilt
x = (np.arange(250) < 100).astype(int).tolist()
sos = signal.butter(4, 0.1, output='sos')
zi = signal.sosfilt_zi(sos)
y, _ = signal.sosfilt(sos, x, zi=zi)
test_filter = filters.Butterworth(N=4, Wn=0.1)
# Simulate data coming in real time
y_real_time_filter = []
for new_val in x:
y_real_time_filter.append(test_filter.filter(new_val))
self.assertListEqual(y.tolist(), y_real_time_filter)
def test_StrideAverageGaitPhaseEstimator(self):
'''Simultaneously runs roughly simulated gyro through heel strike detector,
toe off detector, gait phase estimator. Requires looking at the plot to
confirm.'''
data = Exo.DataContainer()
sampling_freq = 100
time_nows = 1 / sampling_freq * np.arange(0, 1300)
# about 1 heel strike per second
gyro_values = 0.5 * np.sin(6 * time_nows +
0.3 * np.sin(10 * time_nows))
gyro_values = gyro_values[:len(gyro_values) - 300]
gyro_values = np.append(gyro_values, [0] * 300)
heel_strike_detector = gait_state_estimators.GyroHeelStrikeDetector(
height=0.3, gyro_filter=filters.Butterworth(N=2, Wn=0.4))
gait_phase_estimator = gait_state_estimators.StrideAverageGaitPhaseEstimator(
num_strides_required=4,
min_allowable_stride_duration=0.6,
max_allowable_stride_duration=1.3)
toe_off_detector = gait_state_estimators.GaitPhaseBasedToeOffDetector(
fraction_of_gait=0.6)
gait_event_detector = gait_state_estimators.GaitStateEstimator(
data_container=data,
heel_strike_detector=heel_strike_detector,
gait_phase_estimator=gait_phase_estimator,
toe_off_detector=toe_off_detector)
did_heel_strikes = []
gait_phases = []
did_toe_offs = []
for time_now, gyro_value in zip(time_nows, gyro_values):
data.gyro_z = gyro_value
gait_event_detector.detect()
did_heel_strikes.append(data.did_heel_strike)
gait_phases.append(data.gait_phase)
did_toe_offs.append(data.did_toe_off)
plt.plot(did_heel_strikes, label="heel strike")
plt.plot(gait_phases, label='gait phase', linestyle='--')
plt.plot(did_toe_offs, label='toe off')
plt.plot(gyro_values, label='gyro')
plt.legend()
plt.show()
def __init__(self,
dev_id: int,
max_allowable_current: int,
file_ID: str = None,
target_freq: float = 200,
do_read_fsrs: bool = False):
'''Exo object is the primary interface with the Dephy ankle exos, and corresponds to a single physical exoboot.
Args:
dev_id: int. Unique integer to identify the exo in flexsea's library. Returned by connect_to_exo
file_ID: str. Unique string added to filename. If None, no file will be saved.
do_read_dsrs: bool indicating whether to read FSRs '''
self.dev_id = dev_id
self.max_allowable_current = max_allowable_current
self.file_ID = file_ID
self.do_read_fsrs = do_read_fsrs
if self.dev_id is None:
print(
'Exo obj created but no exoboot connected. Some methods available'
)
elif self.dev_id in constants.LEFT_EXO_DEV_IDS:
self.side = constants.Side.LEFT
self.motor_sign = -1
self.ankle_to_motor_angle_polynomial = constants.LEFT_ANKLE_TO_MOTOR
self.ankle_angle_offset = constants.LEFT_ANKLE_ANGLE_OFFSET
elif self.dev_id in constants.RIGHT_EXO_DEV_IDS:
self.side = constants.Side.RIGHT
self.motor_sign = 1
self.ankle_to_motor_angle_polynomial = constants.RIGHT_ANKLE_TO_MOTOR
self.ankle_angle_offset = constants.RIGHT_ANKLE_ANGLE_OFFSET
else:
raise ValueError(
'dev_id: ', self.dev_id,
'not found in constants.LEFT_EXO_DEV_IDS or constants.RIGHT_EXO_DEV_IDS'
)
self.motor_offset = 0
# ankle velocity filter is hardcoded for simplicity, but can be factored out if necessary
self.ankle_velocity_filter = filters.Butterworth(N=2,
Wn=10,
fs=target_freq)
if self.do_read_fsrs:
print('do_read_fsrs: True. Checking if rpi')
if fxu.is_pi() or fxu.is_pi64():
import gpiozero # pylint: disable=import-error
if self.side == constants.Side.LEFT:
self.heel_fsr_detector = gpiozero.InputDevice(
pin=constants.LEFT_HEEL_FSR_PIN, pull_up=True)
self.toe_fsr_detector = gpiozero.InputDevice(
pin=constants.LEFT_TOE_FSR_PIN, pull_up=True)
else:
self.heel_fsr_detector = gpiozero.InputDevice(
pin=constants.RIGHT_HEEL_FSR_PIN, pull_up=True)
self.toe_fsr_detector = gpiozero.InputDevice(
pin=constants.RIGHT_TOE_FSR_PIN, pull_up=True)
else:
raise Exception('Can only use FSRs with rapberry pi!')
self.data = self.DataContainer(do_include_FSRs=do_read_fsrs)
self.has_calibrated = False
self.is_clipping = False
if self.file_ID is not None:
self.setup_data_writer(file_ID=file_ID)
if self.dev_id is not None:
self.update_gains(Kp=constants.DEFAULT_KP,
Ki=constants.DEFAULT_KI,
Kd=constants.DEFAULT_KD,
k_val=0,
b_val=0,
ff=constants.DEFAULT_FF)
self.TR_from_ankle_angle = interpolate.PchipInterpolator(
constants.ANKLE_PTS, self.motor_sign * constants.TR_PTS)
def get_gse_and_sm_lists(exo_list,
config: Type[config_util.ConfigurableConstants]):
'''depending on config, uses exo list to create gait state estimator and state machine lists.'''
gait_state_estimator_list = []
state_machine_list = []
if config.TASK == config_util.Task.WALKING:
for exo in exo_list:
heel_strike_detector = gait_state_estimators.GyroHeelStrikeDetector(
height=config.HS_GYRO_THRESHOLD,
gyro_filter=filters.Butterworth(N=config.HS_GYRO_FILTER_N,
Wn=config.HS_GYRO_FILTER_WN,
fs=config.TARGET_FREQ),
delay=config.HS_GYRO_DELAY)
gait_phase_estimator = gait_state_estimators.StrideAverageGaitPhaseEstimator(
)
toe_off_detector = gait_state_estimators.GaitPhaseBasedToeOffDetector(
fraction_of_gait=config.TOE_OFF_FRACTION)
gait_state_estimator = gait_state_estimators.GaitStateEstimator(
side=exo.side,
data_container=exo.data,
heel_strike_detector=heel_strike_detector,
gait_phase_estimator=gait_phase_estimator,
toe_off_detector=toe_off_detector,
do_print_heel_strikes=config.PRINT_HS)
gait_state_estimator_list.append(gait_state_estimator)
# Define State Machine
reel_in_controller = controllers.SmoothReelInController(
exo=exo,
reel_in_mV=config.REEL_IN_MV,
slack_cutoff=config.REEL_IN_SLACK_CUTOFF,
time_out=config.REEL_IN_TIMEOUT)
swing_controller = controllers.StalkController(
exo=exo, desired_slack=config.SWING_SLACK)
reel_out_controller = controllers.SoftReelOutController(
exo=exo, desired_slack=config.SWING_SLACK)
if config.STANCE_CONTROL_STYLE == config_util.StanceCtrlStyle.FOURPOINTSPLINE:
stance_controller = controllers.FourPointSplineController(
exo=exo,
rise_fraction=config.RISE_FRACTION,
peak_torque=config.PEAK_TORQUE,
peak_fraction=config.PEAK_FRACTION,
fall_fraction=config.FALL_FRACTION,
bias_torque=config.SPLINE_BIAS)
elif config.STANCE_CONTROL_STYLE == config_util.StanceCtrlStyle.SAWICKIWICKI:
stance_controller = controllers.SawickiWickiController(
exo=exo, k_val=config.K_VAL, b_val=config.B_VAL)
state_machine = state_machines.StanceSwingReeloutReelinStateMachine(
exo=exo,
stance_controller=stance_controller,
swing_controller=swing_controller,
reel_in_controller=reel_in_controller,
reel_out_controller=reel_out_controller)
state_machine_list.append(state_machine)
elif config.TASK == config_util.Task.STANDINGPERTURBATION:
for exo in exo_list:
gait_state_estimator = gait_state_estimators.SlipDetectorAP(
data_container=exo.data)
gait_state_estimator_list.append(gait_state_estimator)
standing_controller = controllers.GenericImpedanceController(
exo=exo, setpoint=10, k_val=100)
if config.STANCE_CONTROL_STYLE == config_util.StanceCtrlStyle.GENERICIMPEDANCE:
slip_controller = controllers.GenericImpedanceController(
exo=exo, setpoint=config.SET_POINT, k_val=config.K_VAL)
slip_recovery_time = 1.01 # TODO(maxshep)
elif config.STANCE_CONTROL_STYLE == config_util.StanceCtrlStyle.FOURPOINTSPLINE:
print("using a spline based controller!")
slip_controller = controllers.FourPointSplineController(
exo=exo,
rise_fraction=config.RISE_FRACTION,
peak_torque=config.PEAK_TORQUE,
peak_fraction=config.PEAK_FRACTION,
fall_fraction=config.FALL_FRACTION,
bias_torque=config.SPLINE_BIAS,
use_gait_phase=False)
# slip_recovery_time = config.FALL_FRACTION-0.01
slip_recovery_time = 0.99
state_machine = state_machines.StandingPerturbationResponse(
exo=exo,
standing_controller=standing_controller,
slip_controller=slip_controller,
slip_recovery_time=slip_recovery_time)
state_machine_list.append(state_machine)
elif config.TASK == config_util.Task.BILATERALSTANDINGPERTURBATION:
if len(exo_list) != 2:
raise ValueError(
'Must have two exos connected for task=BILATERALSTANDINGPERTURBATION'
)
gait_phase_estimator = gait_state_estimators.BilateralSlipDetector(
exo_1=exo_list[0], exo_2=exo_list[1])
gait_state_estimator_list.append(gait_phase_estimator)
for exo in exo_list:
standing_controller = controllers.GenericImpedanceController(
exo=exo, setpoint=10, k_val=100)
if config.STANCE_CONTROL_STYLE == config_util.StanceCtrlStyle.GENERICIMPEDANCE:
slip_controller = controllers.GenericImpedanceController(
exo=exo, setpoint=config.SET_POINT, k_val=config.K_VAL)
slip_recovery_time = 1.01 # TODO(maxshep)
elif config.STANCE_CONTROL_STYLE == config_util.StanceCtrlStyle.FOURPOINTSPLINE:
print("using a spline based controller!")
slip_controller = controllers.FourPointSplineController(
exo=exo,
rise_fraction=config.RISE_FRACTION,
peak_torque=config.PEAK_TORQUE,
peak_fraction=config.PEAK_FRACTION,
fall_fraction=config.FALL_FRACTION,
bias_torque=config.SPLINE_BIAS,
use_gait_phase=False)
# slip_recovery_time = config.FALL_FRACTION-0.01
slip_recovery_time = 0.99
state_machine = state_machines.StandingPerturbationResponse(
exo=exo,
standing_controller=standing_controller,
slip_controller=slip_controller,
slip_recovery_time=slip_recovery_time)
state_machine_list.append(state_machine)
return gait_state_estimator_list, state_machine_list