import time from .filters import clamp class TimedCycleGripperController: 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): config = self.config self.reader.start() try: self._print_thresholds() print( f"Calibrating baseline for {config.baseline_seconds:.2f}s. " "Keep sensors unloaded." ) baseline = self.feedback_processor.calibrate_baseline(self.reader) print( f"baseline normal: left={baseline.left_normal:.4f}{self.normal_unit}, " f"right={baseline.right_normal:.4f}{self.normal_unit}; " f"shear: left={baseline.left_shear:.4f}{self.shear_unit}, " f"right={baseline.right_shear:.4f}{self.shear_unit}" ) self.gripper.connect() cycle_index = 1 while config.cycles <= 0 or cycle_index <= config.cycles: open_force = int(clamp( config.initial_force, config.force_min, config.force_max, )) self._run_open_phase(open_force, cycle_index) result = self._run_close_phase(cycle_index) if result in ("emergency", "released"): time.sleep(config.open_seconds) if result == "gripped": print("object is gripped; timed open/close loop stopped") break cycle_index += 1 except KeyboardInterrupt: print("\nStopping timed cycle control.") finally: self.gripper.final_open_if_needed() self.reader.stop() def _print_thresholds(self): config = self.config print( "Normal force thresholds: " f"both_contact={config.both_contact_force:.3f}{self.normal_unit}, " f"object={config.object_force:.3f}{self.normal_unit}, " f"emergency={config.emergency_touch_force:.3f}{self.normal_unit}, " f"requires_both={int(config.object_requires_both_contact)}, " f"empty_release={config.empty_release_seconds:.2f}s" ) print( "Shear force thresholds: " f"hold_min={config.shear_hold_force:.3f}{self.shear_unit}, " f"stable_delta={config.shear_stable_delta:.3f}{self.shear_unit}, " f"stable_seconds={config.shear_stable_seconds:.2f}s, " f"release_change={config.release_shear_change:.3f}{self.shear_unit}" ) def _run_open_phase(self, force_pct, cycle_index): print(f"\ncycle {cycle_index}: open phase") self.gripper.open(force_pct, "open") deadline = time.perf_counter() + self.config.open_seconds while time.perf_counter() < deadline: self.reader.update() time.sleep(0.02) def _run_close_phase(self, cycle_index): config = self.config print(f"cycle {cycle_index}: close phase") self.feedback_processor.reset_filters() force_pct = int(clamp(config.initial_force, config.force_min, config.force_max)) self.gripper.close(force_pct, "close") interval = 1.0 / config.control_hz if config.control_hz > 0 else 0.1 phase_start = time.perf_counter() last_close_command_time = phase_start last_force_ramp_time = phase_start state = "closing" hold_pos = None previous_shear = None hold_start_time = None hold_shear_reference = None release_monitor_armed = False shear_stable_since = None empty_since = None gripped = False deadline = time.perf_counter() + config.close_seconds while True: tick_start = time.perf_counter() now = time.perf_counter() if state == "closing" and now >= deadline: print("close phase finished without object; continue timed open/close cycle") return "no_object" feedback = self.feedback_processor.read(self.reader, timeout=0.3) if feedback is None: phase_time = time.perf_counter() - phase_start print(f"t={phase_time:5.2f}s action=timed-close waiting for sensor samples") time.sleep(0.05) continue has_previous_shear = previous_shear is not None shear_delta = 0.0 if not has_previous_shear else feedback.max_shear - previous_shear shear_abs_delta = abs(shear_delta) previous_shear = feedback.max_shear both_contact = ( feedback.left_normal >= config.both_contact_force and feedback.right_normal >= config.both_contact_force ) normal_force_detected = feedback.max_normal >= config.object_force contact_confirmed = ( normal_force_detected and (both_contact or not config.object_requires_both_contact) ) object_detected = contact_confirmed grip_contact = ( both_contact if config.object_requires_both_contact else feedback.max_normal >= config.both_contact_force ) if state in ("gripping", "hold_check") and not grip_contact: if empty_since is None: empty_since = now elif now - empty_since >= config.empty_release_seconds: force_pct = self.gripper.set_force(config.force_min) print( f"L={feedback.left_normal:8.3f}{self.normal_unit} " f"R={feedback.right_normal:8.3f}{self.normal_unit} " f"min={feedback.min_normal:8.3f}{self.normal_unit} " f"max={feedback.max_normal:8.3f}{self.normal_unit} " f"force_pct={force_pct:3d} action=empty-grip-resume-cycle" ) return "no_object" else: empty_since = None if feedback.max_normal >= config.emergency_touch_force: if config.enable_emergency_open: force_pct = self.gripper.set_force(config.force_min) self.gripper.open(force_pct, "emergency-open") print( f"L={feedback.left_normal:8.3f}{self.normal_unit} " f"R={feedback.right_normal:8.3f}{self.normal_unit} " f"max={feedback.max_normal:8.3f}{self.normal_unit} " f"force_pct={force_pct:3d} action=emergency-open" ) return "emergency" hold_pos = self.gripper.hold_current_position(force_pct) state = "hold_check" gripped = True if hold_start_time is None: hold_start_time = now action = "overforce-hold" if state == "closing": if contact_confirmed: state = "gripping" gripped = True force_pct = int(clamp( config.grip_start_force, config.force_min, config.force_max, )) force_pct = self.gripper.set_force(force_pct) hold_pos = self.gripper.hold_current_position(force_pct) last_force_ramp_time = now shear_stable_since = None action = "object-detected-low-force" print( "object detected; timed open/close cycle stopped, " f"grip force reset to {force_pct}" ) else: action = "closing" if ( config.close_command_interval > 0 and now - last_close_command_time >= config.close_command_interval ): self.gripper.close(force_pct, "close-continue") last_close_command_time = now elif state == "gripping": shear_is_high_enough = feedback.max_shear >= config.shear_hold_force shear_is_stable = ( shear_is_high_enough and has_previous_shear and shear_abs_delta <= config.shear_stable_delta ) if shear_is_stable: if shear_stable_since is None: shear_stable_since = now else: shear_stable_since = None if ( shear_stable_since is not None and now - shear_stable_since >= config.shear_stable_seconds ): state = "hold_check" hold_start_time = now hold_shear_reference = None release_monitor_armed = False hold_pos = self.gripper.hold_current_position(force_pct) action = "shear-stable-hold" elif shear_is_high_enough: action = "wait-shear-stable" elif ( now - last_force_ramp_time >= config.force_ramp_interval and force_pct < config.force_max ): force_pct = int(clamp( force_pct + config.force_ramp_step, config.force_min, config.force_max, )) force_pct = self.gripper.set_force(force_pct) last_force_ramp_time = now action = "grip-ramp-up" else: action = "grip-hold" elif state == "hold_check": hold_elapsed = now - hold_start_time if not release_monitor_armed and hold_elapsed >= config.human_hold_seconds: hold_shear_reference = feedback.max_shear release_monitor_armed = True action = "hold-check-armed" elif release_monitor_armed: hold_shear_change = abs(feedback.max_shear - hold_shear_reference) if hold_shear_change >= config.release_shear_change: if config.enable_human_release: force_pct = self.gripper.set_force(config.force_min) self.gripper.open(force_pct, "human-release-open") print( f"L={feedback.left_normal:8.3f}{self.normal_unit} " f"R={feedback.right_normal:8.3f}{self.normal_unit} " f"shear={feedback.max_shear:8.3f}{self.shear_unit} " f"hold_change={hold_shear_change:8.3f}{self.shear_unit} " f"force_pct={force_pct:3d} action=human-release-open" ) return "released" hold_shear_reference = feedback.max_shear action = "hold-check-change-ignored" else: action = "hold-check" else: action = "hold-check" self._log_tick( phase_start, feedback, shear_delta, both_contact, object_detected, grip_contact, empty_since, shear_stable_since, state, gripped, hold_pos, force_pct, action, ) sleep_time = interval - (time.perf_counter() - tick_start) if sleep_time > 0: time.sleep(sleep_time) def _log_tick( self, phase_start, feedback, shear_delta, both_contact, object_detected, grip_contact, empty_since, shear_stable_since, state, gripped, hold_pos, force_pct, action, ): now = time.perf_counter() phase_time = now - phase_start print( f"t={phase_time:5.2f}s " f"L={feedback.left_normal:8.3f}{self.normal_unit} " f"R={feedback.right_normal:8.3f}{self.normal_unit} " f"min={feedback.min_normal:8.3f}{self.normal_unit} " f"max={feedback.max_normal:8.3f}{self.normal_unit} " f"diff={feedback.normal_diff:8.3f}{self.normal_unit} " f"both={int(both_contact)} object={int(object_detected)} " f"grip_contact={int(grip_contact)} " f"empty_for={0.0 if empty_since is None else now - empty_since:4.1f}s " f"shear={feedback.max_shear:8.3f}{self.shear_unit} " f"d_shear={shear_delta:8.3f}{self.shear_unit} " f"shear_stable={int(shear_stable_since is not None)} " f"state={state} gripped={int(gripped)} hold_pos={hold_pos} " f"force_pct={force_pct:3d} action={action}" )