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
Exemple #4
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)
Exemple #5
0
    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)
Exemple #6
0
    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()
Exemple #7
0
    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