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}")