test/gripper_control_02/controller.py

import time

from gripper_control.filters import clamp


class SideTriggerGripController:
    def __init__(self, config, reader, gripper, feedback_processor):
        self.config = config
        self.reader = reader
        self.gripper = gripper
        self.feedback_processor = feedback_processor
        self.normal_unit = feedback_processor.normal_converter.unit
        self.shear_unit = feedback_processor.shear_converter.unit

    def run(self):
        self.reader.start()

        try:
            self.gripper.connect()
            open_force = int(clamp(
                self.config.open_force,
                self.config.force_min,
                self.config.force_max,
            ))
            print("demo02 start: open gripper and wait for side touch")
            self.gripper.open(open_force, "startup-open")

            self._print_thresholds()
            print(
                f"Calibrating baseline for {self.config.baseline_seconds:.2f}s. "
                "Keep sensors unloaded."
            )
            baseline = self.feedback_processor.calibrate_baseline(self.reader)
            print(
                f"baseline normal: L={baseline.left_normal:.4f}{self.normal_unit} "
                f"R={baseline.right_normal:.4f}{self.normal_unit}; "
                f"shear: L={baseline.left_shear:.4f}{self.shear_unit} "
                f"R={baseline.right_shear:.4f}{self.shear_unit}"
            )

            self._run_event_monitor()

        except KeyboardInterrupt:
            print("\nStopping demo02.")
        finally:
            self.gripper.final_open_if_needed()
            self.reader.stop()

    def _print_thresholds(self):
        print(
            "Trigger thresholds: "
            f"normal={self.config.trigger_normal_force:.3f}{self.normal_unit}, "
            f"shear={self.config.trigger_shear_force:.3f}{self.shear_unit}"
        )
        print(
            "Grip/release thresholds: "
            f"grip_normal={self.config.grip_normal_force:.3f}{self.normal_unit}, "
            f"requires_both={int(self.config.grip_requires_both)}, "
            f"release_normal_change={self.config.release_normal_change:.3f}{self.normal_unit}, "
            f"release_shear_change={self.config.release_shear_change:.3f}{self.shear_unit}"
        )

    def _run_event_monitor(self):
        config = self.config
        interval = 1.0 / config.control_hz if config.control_hz > 0 else 0.05

        state = "open_wait"
        state_start = time.perf_counter()
        last_force_ramp_time = state_start
        recover_quiet_since = None
        force_pct = int(clamp(config.open_force, config.force_min, config.force_max))
        hold_pos = None
        reference = None

        while True:
            tick_start = time.perf_counter()
            now = time.perf_counter()

            feedback = self.feedback_processor.read(self.reader, timeout=0.3)
            if feedback is None:
                print(f"state={state} action=waiting-for-sensor-samples")
                time.sleep(0.05)
                continue

            trigger = self._side_triggered(feedback)
            grip_contact = self._grip_contact(feedback)
            normal_change = 0.0
            shear_change = 0.0
            action = "wait"

            if state == "open_wait":
                if trigger:
                    force_pct = int(clamp(
                        config.close_force,
                        config.force_min,
                        config.force_max,
                    ))
                    self.gripper.close(force_pct, "side-trigger-close")
                    state = "closing"
                    state_start = now
                    last_force_ramp_time = now
                    recover_quiet_since = None
                    hold_pos = None
                    reference = None
                    action = "side-trigger-close"
                else:
                    action = "open-wait-touch"

            elif state == "closing":
                if grip_contact:
                    force_pct = int(clamp(
                        config.grip_start_force,
                        config.force_min,
                        config.force_max,
                    ))
                    self.gripper.set_force(force_pct)
                    hold_pos = self.gripper.hold_current_position(force_pct)
                    state = "gripping"
                    state_start = now
                    last_force_ramp_time = now
                    reference = None
                    action = "object-detected-low-force"
                elif now - state_start >= config.close_timeout_seconds:
                    force_pct = int(clamp(
                        config.open_force,
                        config.force_min,
                        config.force_max,
                    ))
                    self.gripper.open(force_pct, "close-timeout-open")
                    self.feedback_processor.reset_filters()
                    state = "open_recover"
                    state_start = now
                    recover_quiet_since = None
                    hold_pos = None
                    reference = None
                    action = "close-timeout-open"
                else:
                    action = "closing-to-grip"

            elif state == "gripping":
                if (
                    now - last_force_ramp_time >= config.force_ramp_interval
                    and force_pct < config.hold_force
                ):
                    force_pct = int(clamp(
                        force_pct + config.force_ramp_step,
                        config.force_min,
                        config.hold_force,
                    ))
                    self.gripper.set_force(force_pct)
                    last_force_ramp_time = now
                    action = "grip-ramp-up"
                elif (
                    force_pct >= config.hold_force
                    and now - state_start >= config.grip_settle_seconds
                ):
                    reference = self._make_reference(feedback)
                    state = "hold_check"
                    state_start = now
                    action = "hold-reference-armed"
                else:
                    action = "grip-hold"

            elif state == "hold_check":
                normal_change, shear_change = self._release_changes(feedback, reference)
                should_release = (
                    normal_change >= config.release_normal_change
                    or shear_change >= config.release_shear_change
                )
                if should_release:
                    force_pct = int(clamp(
                        config.open_force,
                        config.force_min,
                        config.force_max,
                    ))
                    self.gripper.open(force_pct, "force-change-release-open")
                    self.feedback_processor.reset_filters()
                    state = "open_recover"
                    state_start = now
                    recover_quiet_since = None
                    hold_pos = None
                    reference = None
                    action = "force-change-release-open"
                else:
                    action = "hold-check"

            elif state == "open_recover":
                if self._recover_quiet(feedback):
                    if recover_quiet_since is None:
                        recover_quiet_since = now
                        action = "open-recover-quiet"
                    elif (
                        now - recover_quiet_since >= config.recover_stable_seconds
                        and now - state_start >= config.rearm_seconds
                    ):
                        state = "open_wait"
                        state_start = now
                        recover_quiet_since = None
                        hold_pos = None
                        reference = None
                        action = "open-ready"
                    else:
                        action = "open-recover-stabilizing"
                else:
                    recover_quiet_since = None
                    action = "open-recover-wait-force-clear"

            self._log_tick(
                feedback=feedback,
                state=state,
                trigger=trigger,
                grip_contact=grip_contact,
                normal_change=normal_change,
                shear_change=shear_change,
                hold_pos=hold_pos,
                force_pct=force_pct,
                action=action,
            )

            sleep_time = interval - (time.perf_counter() - tick_start)
            if sleep_time > 0:
                time.sleep(sleep_time)

    def _side_triggered(self, feedback, reference=None):
        if reference is not None:
            normal_change, shear_change = self._release_changes(feedback, reference)
            if (
                normal_change >= self.config.trigger_normal_change
                or shear_change >= self.config.trigger_shear_change
            ):
                return True

        return (
            feedback.max_normal >= self.config.trigger_normal_force
            or feedback.max_shear >= self.config.trigger_shear_force
        )

    def _grip_contact(self, feedback):
        left_contact = feedback.left_normal >= self.config.grip_normal_force
        right_contact = feedback.right_normal >= self.config.grip_normal_force
        if self.config.grip_requires_both:
            return left_contact and right_contact
        return left_contact or right_contact

    def _recover_quiet(self, feedback):
        return (
            feedback.max_normal <= self.config.recover_normal_force
            and feedback.max_shear <= self.config.recover_shear_force
        )

    def _make_reference(self, feedback):
        return {
            "left_normal": feedback.left_normal,
            "right_normal": feedback.right_normal,
            "max_normal": feedback.max_normal,
            "left_shear": feedback.left_shear,
            "right_shear": feedback.right_shear,
            "max_shear": feedback.max_shear,
        }

    def _release_changes(self, feedback, reference):
        if reference is None:
            return 0.0, 0.0
        normal_change = max(
            abs(feedback.left_normal - reference["left_normal"]),
            abs(feedback.right_normal - reference["right_normal"]),
            abs(feedback.max_normal - reference["max_normal"]),
        )
        shear_change = max(
            abs(feedback.left_shear - reference["left_shear"]),
            abs(feedback.right_shear - reference["right_shear"]),
            abs(feedback.max_shear - reference["max_shear"]),
        )
        return normal_change, shear_change

    def _release_contact_lost(self, feedback):
        return (
            feedback.max_normal <= self.config.release_contact_lost_normal_force
            and feedback.max_shear <= self.config.release_contact_lost_shear_force
        )

    def _log_tick(
        self,
        feedback,
        state,
        trigger,
        grip_contact,
        normal_change,
        shear_change,
        hold_pos,
        force_pct,
        action,
    ):
        print(
            f"L={feedback.left_normal:7.3f}{self.normal_unit} "
            f"R={feedback.right_normal:7.3f}{self.normal_unit} "
            f"maxN={feedback.max_normal:7.3f}{self.normal_unit} "
            f"shear={feedback.max_shear:7.3f}{self.shear_unit} "
            f"trigger={int(trigger)} grip_contact={int(grip_contact)} "
            f"dN={normal_change:7.3f}{self.normal_unit} "
            f"dShear={shear_change:7.3f}{self.shear_unit} "
            f"state={state} hold_pos={hold_pos} "
            f"force_pct={force_pct:3d} action={action}"
        )