test/gripper_control_02/controller.py

import time
import threading

from gripper_control.filters import clamp


class SideTriggerGripController:
    def __init__(
        self,
        config,
        reader,
        gripper,
        feedback_processor,
        status_callback=None,
    ):
        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
        self.status_callback = status_callback
        self._stop_event = threading.Event()
        self._manual_lock = threading.Lock()
        self._manual_command = None

    def stop(self):
        self._stop_event.set()

    def request_manual_command(self, command):
        if command not in {"open", "close", "hold", "release"}:
            raise ValueError(f"unsupported manual command: {command}")
        with self._manual_lock:
            self._manual_command = command

    def _pop_manual_command(self):
        with self._manual_lock:
            command = self._manual_command
            self._manual_command = None
        return command

    def run(self):
        self._stop_event.clear()
        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}"
        )
        print(
            "Adaptive grip: "
            f"enabled={int(self.config.adaptive_grip_enabled)}, "
            f"shear_start={self.config.adaptive_shear_start_force:.3f}{self.shear_unit}, "
            f"shear_full={self.config.adaptive_shear_full_force:.3f}{self.shear_unit}, "
            f"force={self.config.adaptive_force_min}-{self.config.adaptive_force_max}, "
            f"step={self.config.adaptive_force_step}, "
            f"interval={self.config.adaptive_force_interval:.2f}s, "
            f"release_arm_delay={self.config.release_arm_delay_seconds:.2f}s"
        )
        print(
            "Release open: "
            f"speed={self.config.release_open_speed}, "
            f"accel={self.config.release_open_accel}, "
            f"decel={self.config.release_open_decel}"
        )
        print(
            "Recover: "
            f"force_normal={self.config.recover_normal_force:.3f}{self.normal_unit}, "
            f"force_shear={self.config.recover_shear_force:.3f}{self.shear_unit}, "
            f"open_pos={self.config.open_pos}, "
            f"pos_enabled={int(self.config.open_position_recover_enabled)}, "
            f"pos_tol={self.config.open_position_tolerance}"
        )

    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
        last_open_position_check_time = 0.0
        last_open_position = None
        last_open_position_ready = False
        recover_quiet_since = None
        recover_position_since = None
        trigger_clear_since = None
        trigger_armed = False
        last_trigger = False
        open_wait_samples = []
        open_wait_stable_values = {}
        force_pct = int(clamp(config.open_force, config.force_min, config.force_max))
        adaptive_target_force = int(clamp(config.hold_force, config.force_min, config.force_max))
        release_armed = False
        hold_pos = None
        reference = None

        def reset_open_wait_stability():
            nonlocal open_wait_samples, open_wait_stable_values
            open_wait_samples = []
            open_wait_stable_values = {}

        while not self._stop_event.is_set():
            tick_start = time.perf_counter()
            now = time.perf_counter()
            manual_action = None

            manual_command = self._pop_manual_command()
            if manual_command == "open":
                force_pct = int(clamp(
                    config.open_force,
                    config.force_min,
                    config.force_max,
                ))
                self.gripper.open(force_pct, "manual-open")
                self.feedback_processor.reset_filters()
                state = "open_recover"
                state_start = now
                last_open_position_check_time = 0.0
                last_open_position = None
                last_open_position_ready = False
                recover_quiet_since = None
                recover_position_since = None
                trigger_clear_since = None
                trigger_armed = False
                reset_open_wait_stability()
                release_armed = False
                hold_pos = None
                reference = None
                manual_action = "manual-open"
            elif manual_command == "close":
                force_pct = int(clamp(
                    config.close_force,
                    config.force_min,
                    config.force_max,
                ))
                self.gripper.close(force_pct, "manual-close")
                state = "manual_close"
                state_start = now
                last_force_ramp_time = now
                last_open_position_check_time = 0.0
                last_open_position = None
                last_open_position_ready = False
                recover_quiet_since = None
                recover_position_since = None
                trigger_clear_since = None
                trigger_armed = False
                reset_open_wait_stability()
                release_armed = False
                hold_pos = None
                reference = None
                manual_action = "manual-close"
            elif manual_command == "hold":
                force_pct = int(clamp(
                    config.hold_force,
                    config.force_min,
                    config.force_max,
                ))
                self.gripper.set_force(force_pct)
                hold_pos = self.gripper.hold_current_position(force_pct)
                state = "manual_hold"
                state_start = now
                last_force_ramp_time = now
                trigger_clear_since = None
                trigger_armed = False
                reset_open_wait_stability()
                release_armed = False
                reference = None
                manual_action = "manual-hold"
            elif manual_command == "release":
                if state == "manual_hold":
                    state = "open_wait"
                    state_start = now
                    trigger_clear_since = None
                    trigger_armed = False
                    reset_open_wait_stability()
                    release_armed = False
                    hold_pos = None
                    reference = None
                manual_action = "manual-release"

            feedback = self.feedback_processor.read(self.reader, timeout=0.3)
            if feedback is None:
                action = manual_action or "waiting-for-sensor-samples"
                print(f"state={state} action={action}")
                self._emit_status(
                    {
                        "timestamp": time.time(),
                        "state": state,
                        "action": action,
                        "force_pct": force_pct,
                        "adaptive_target_force": adaptive_target_force,
                        "adaptive_grip_enabled": config.adaptive_grip_enabled,
                        "release_armed": release_armed,
                        "manual_hold_active": state == "manual_hold",
                        "speed_pct": config.speed,
                        "normal_unit": self.normal_unit,
                        "shear_unit": self.shear_unit,
                    }
                )
                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 = manual_action or "wait"

            if manual_action is not None:
                pass
            elif state == "open_wait":
                open_wait_sample = self._open_wait_sample(feedback)
                self._append_open_wait_sample(open_wait_samples, open_wait_sample)
                stable_values = self._open_wait_stable_values(open_wait_samples)
                trigger = self._open_wait_delta_triggered(
                    open_wait_sample,
                    open_wait_stable_values,
                )
                if not trigger:
                    open_wait_stable_values.update(stable_values)

                trigger_armed = bool(open_wait_stable_values)
                if not trigger_armed:
                    action = "open-wait-stabilizing"
                elif not trigger:
                    action = "open-wait-armed"
                else:
                    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
                    last_open_position_check_time = 0.0
                    last_open_position = None
                    last_open_position_ready = False
                    recover_quiet_since = None
                    recover_position_since = None
                    trigger_clear_since = None
                    trigger_armed = False
                    reset_open_wait_stability()
                    adaptive_target_force = self._adaptive_force_target(feedback)
                    release_armed = False
                    hold_pos = None
                    reference = None
                    action = "side-trigger-close"

            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
                    adaptive_target_force = self._adaptive_force_target(feedback)
                    release_armed = False
                    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
                    last_open_position_check_time = 0.0
                    last_open_position = None
                    last_open_position_ready = False
                    recover_quiet_since = None
                    recover_position_since = None
                    reset_open_wait_stability()
                    release_armed = False
                    hold_pos = None
                    reference = None
                    action = "close-timeout-open"
                else:
                    if (
                        config.closing_command_interval > 0
                        and now - last_force_ramp_time >= config.closing_command_interval
                    ):
                        self.gripper.close(force_pct, "closing-continue")
                        last_force_ramp_time = now
                        action = "closing-continue"
                    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
                    adaptive_target_force = self._adaptive_force_target(feedback)
                    release_armed = False
                    action = "hold-reference-armed"
                else:
                    action = "grip-hold"

            elif state == "hold_check":
                adaptive_target_force = self._adaptive_force_target(feedback)
                should_adapt_force = (
                    config.adaptive_grip_enabled
                    and not release_armed
                    and force_pct < adaptive_target_force
                )

                if (
                    should_adapt_force
                    and now - last_force_ramp_time >= config.adaptive_force_interval
                ):
                    force_pct = int(clamp(
                        force_pct + config.adaptive_force_step,
                        config.force_min,
                        adaptive_target_force,
                    ))
                    self.gripper.set_force(force_pct)
                    last_force_ramp_time = now
                    state_start = now
                    reference = self._make_reference(feedback)
                    release_armed = False
                    action = "adaptive-grip-ramp"
                elif should_adapt_force:
                    release_armed = False
                    action = "adaptive-grip-wait"
                elif (
                    not release_armed
                    and now - max(state_start, last_force_ramp_time)
                    < config.release_arm_delay_seconds
                ):
                    action = "release-arm-delay"
                elif not release_armed:
                    reference = self._make_reference(feedback)
                    release_armed = True
                    action = "release-reference-armed"
                else:
                    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",
                            speed_pct=config.release_open_speed,
                            accel=config.release_open_accel,
                            decel=config.release_open_decel,
                        )
                        self.feedback_processor.reset_filters()
                        state = "open_recover"
                        state_start = now
                        last_open_position_check_time = 0.0
                        last_open_position = None
                        last_open_position_ready = False
                        recover_quiet_since = None
                        recover_position_since = None
                        reset_open_wait_stability()
                        release_armed = False
                        hold_pos = None
                        reference = None
                        action = "force-change-release-open"
                    else:
                        action = "hold-check"

            elif state == "open_recover":
                current_pos = last_open_position
                position_ready = last_open_position_ready
                if (
                    config.open_position_recover_enabled
                    and now - last_open_position_check_time
                    >= config.open_position_check_interval
                ):
                    last_open_position_check_time = now
                    current_pos = self.gripper.read_position()
                    position_ready = self._open_position_ready(current_pos)
                    last_open_position = current_pos
                    last_open_position_ready = position_ready

                if position_ready:
                    if recover_position_since is None:
                        recover_position_since = now
                        action = f"open-recover-position pos={current_pos}"
                    elif (
                        now - recover_position_since
                        >= max(
                            config.open_position_stable_seconds,
                            config.rearm_seconds,
                        )
                    ):
                        state = "open_wait"
                        state_start = now
                        recover_quiet_since = None
                        recover_position_since = None
                        trigger_clear_since = None
                        trigger_armed = True
                        reset_open_wait_stability()
                        last_trigger = trigger
                        release_armed = False
                        hold_pos = None
                        reference = None
                        action = f"open-ready-position pos={current_pos}"
                    else:
                        action = f"open-recover-position-stabilizing pos={current_pos}"
                elif self._recover_quiet(feedback):
                    recover_position_since = None
                    if recover_quiet_since is None:
                        recover_quiet_since = now
                        action = "open-recover-quiet"
                    elif (
                        now - recover_quiet_since
                        >= max(config.recover_stable_seconds, config.rearm_seconds)
                    ):
                        state = "open_wait"
                        state_start = now
                        recover_quiet_since = None
                        recover_position_since = None
                        trigger_clear_since = None
                        trigger_armed = True
                        reset_open_wait_stability()
                        last_trigger = trigger
                        release_armed = False
                        hold_pos = None
                        reference = None
                        action = "open-ready"
                    else:
                        action = "open-recover-stabilizing"
                else:
                    recover_quiet_since = None
                    recover_position_since = None
                    action = "open-recover-wait-force-clear"

            elif state == "manual_hold":
                action = "manual-hold"

            elif state == "manual_close":
                action = "manual-close"

            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,
                adaptive_target_force=adaptive_target_force,
                release_armed=release_armed,
                action=action,
                trigger_armed=trigger_armed,
            )
            self._emit_status(
                self._make_status(
                    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,
                    adaptive_target_force=adaptive_target_force,
                    release_armed=release_armed,
                    action=action,
                    trigger_armed=trigger_armed,
                )
            )
            last_trigger = trigger

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

        print("demo02 control loop stopped.")

    def _side_triggered(self, feedback):
        return (
            feedback.max_normal >= self.config.trigger_normal_force
            or feedback.max_shear >= self.config.trigger_shear_force
        )

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

    def _append_open_wait_sample(self, samples, sample):
        samples.append(sample)
        stable_samples = max(2, int(self.config.open_wait_stable_samples))
        if len(samples) > stable_samples:
            del samples[0]

    def _open_wait_stable_values(self, samples):
        stable_samples = max(2, int(self.config.open_wait_stable_samples))
        if len(samples) < stable_samples:
            return {}

        stable = {}
        first = samples[0]
        last = samples[-1]
        for key in first:
            values = [sample[key] for sample in samples]
            value_range = max(values) - min(values)
            trend = abs(last[key] - first[key])
            if (
                value_range <= self.config.open_wait_stable_range_n
                and trend <= self.config.open_wait_stable_trend_n
            ):
                stable[key] = sum(values) / len(values)
        return stable

    def _open_wait_delta_triggered(self, sample, stable_values):
        thresholds = {
            "left_normal": self.config.trigger_normal_force,
            "right_normal": self.config.trigger_normal_force,
            "left_shear": self.config.trigger_shear_force,
            "right_shear": self.config.trigger_shear_force,
        }
        for key, stable_value in stable_values.items():
            if sample[key] - stable_value >= thresholds[key]:
                return True
        return False

    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 _open_position_ready(self, current_pos):
        if current_pos is None:
            return False
        return (
            abs(int(current_pos) - int(self.config.open_pos))
            <= int(self.config.open_position_tolerance)
        )

    def _adaptive_force_target(self, feedback):
        config = self.config
        if not config.adaptive_grip_enabled:
            return int(clamp(config.hold_force, config.force_min, config.force_max))

        force_min = int(clamp(
            config.adaptive_force_min,
            config.force_min,
            config.force_max,
        ))
        force_max = int(clamp(
            config.adaptive_force_max,
            config.force_min,
            config.force_max,
        ))
        if force_max < force_min:
            force_max = force_min

        shear_start = max(0.0, float(config.adaptive_shear_start_force))
        shear_full = max(shear_start + 1e-6, float(config.adaptive_shear_full_force))
        shear = max(0.0, float(feedback.max_shear))
        if shear <= shear_start:
            return force_min
        if shear >= shear_full:
            return force_max

        ratio = (shear - shear_start) / (shear_full - shear_start)
        target = round(force_min + ratio * (force_max - force_min))
        return int(clamp(target, force_min, force_max))

    def _make_reference(self, feedback):
        return {
            "left_normal": feedback.left_normal,
            "right_normal": feedback.right_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"]),
        )
        shear_change = abs(feedback.max_shear - reference["max_shear"])
        shear_change = max(
            shear_change,
            abs(feedback.left_shear - reference["left_shear"]),
            abs(feedback.right_shear - reference["right_shear"]),
        )
        return normal_change, shear_change

    def _log_tick(
        self,
        feedback,
        state,
        trigger,
        grip_contact,
        normal_change,
        shear_change,
        hold_pos,
        force_pct,
        adaptive_target_force,
        release_armed,
        action,
        trigger_armed,
    ):
        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)} armed={int(trigger_armed)} "
            f"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} target={adaptive_target_force:3d} "
            f"release_armed={int(release_armed)} action={action}"
        )

    def _make_status(
        self,
        feedback,
        state,
        trigger,
        grip_contact,
        normal_change,
        shear_change,
        hold_pos,
        force_pct,
        adaptive_target_force,
        release_armed,
        action,
        trigger_armed,
    ):
        speed_pct = self.config.speed
        if action == "force-change-release-open":
            speed_pct = self.config.release_open_speed

        left_sample = None
        right_sample = None
        try:
            if len(self.reader.latest) >= 2:
                left_sample = self.reader.latest[0]
                right_sample = self.reader.latest[1]
        except Exception:
            pass

        return {
            "timestamp": time.time(),
            "state": state,
            "action": action,
            "trigger": bool(trigger),
            "trigger_armed": bool(trigger_armed),
            "grip_contact": bool(grip_contact),
            "left_normal": feedback.left_normal,
            "right_normal": feedback.right_normal,
            "min_normal": feedback.min_normal,
            "max_normal": feedback.max_normal,
            "normal_diff": feedback.normal_diff,
            "left_shear": feedback.left_shear,
            "right_shear": feedback.right_shear,
            "left_shear_x": feedback.left_shear_x,
            "left_shear_y": feedback.left_shear_y,
            "right_shear_x": feedback.right_shear_x,
            "right_shear_y": feedback.right_shear_y,
            "max_shear": feedback.max_shear,
            "shear_diff": feedback.shear_diff,
            "normal_change": normal_change,
            "shear_change": shear_change,
            "hold_pos": hold_pos,
            "force_pct": force_pct,
            "adaptive_target_force": adaptive_target_force,
            "adaptive_grip_enabled": self.config.adaptive_grip_enabled,
            "release_armed": bool(release_armed),
            "manual_hold_active": state == "manual_hold",
            "speed_pct": speed_pct,
            "normal_unit": self.normal_unit,
            "shear_unit": self.shear_unit,
            "left_sample": left_sample,
            "right_sample": right_sample,
        }

    def _emit_status(self, status):
        if self.status_callback is None:
            return
        try:
            self.status_callback(status)
        except Exception as exc:
            print(f"status callback failed: {exc}")