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): 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_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"]) return normal_change, shear_change 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}" )