diff --git a/config/compound_eye.png b/config/compound_eye.png index 52cc158..0b7376c 100644 Binary files a/config/compound_eye.png and b/config/compound_eye.png differ diff --git a/gripper_control_02/__pycache__/controller.cpython-311.pyc b/gripper_control_02/__pycache__/controller.cpython-311.pyc index 230a339..29b1ce3 100644 Binary files a/gripper_control_02/__pycache__/controller.cpython-311.pyc and b/gripper_control_02/__pycache__/controller.cpython-311.pyc differ diff --git a/gripper_control_02/__pycache__/controller.cpython-313.pyc b/gripper_control_02/__pycache__/controller.cpython-313.pyc index c698d9f..60dc51a 100644 Binary files a/gripper_control_02/__pycache__/controller.cpython-313.pyc and b/gripper_control_02/__pycache__/controller.cpython-313.pyc differ diff --git a/gripper_control_02/__pycache__/settings.cpython-311.pyc b/gripper_control_02/__pycache__/settings.cpython-311.pyc index e58f8a0..571cee7 100644 Binary files a/gripper_control_02/__pycache__/settings.cpython-311.pyc and b/gripper_control_02/__pycache__/settings.cpython-311.pyc differ diff --git a/gripper_control_02/__pycache__/settings.cpython-313.pyc b/gripper_control_02/__pycache__/settings.cpython-313.pyc index 9b1ad90..96f7b10 100644 Binary files a/gripper_control_02/__pycache__/settings.cpython-313.pyc and b/gripper_control_02/__pycache__/settings.cpython-313.pyc differ diff --git a/gripper_control_02/__pycache__/visualizer.cpython-311.pyc b/gripper_control_02/__pycache__/visualizer.cpython-311.pyc index 0049dd5..6895877 100644 Binary files a/gripper_control_02/__pycache__/visualizer.cpython-311.pyc and b/gripper_control_02/__pycache__/visualizer.cpython-311.pyc differ diff --git a/gripper_control_02/__pycache__/visualizer.cpython-313.pyc b/gripper_control_02/__pycache__/visualizer.cpython-313.pyc index f428e56..f4d45fa 100644 Binary files a/gripper_control_02/__pycache__/visualizer.cpython-313.pyc and b/gripper_control_02/__pycache__/visualizer.cpython-313.pyc differ diff --git a/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-311.pyc b/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-311.pyc index d21cdd7..5a997f2 100644 Binary files a/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-311.pyc and b/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-311.pyc differ diff --git a/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-313.pyc b/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-313.pyc index b08cb11..1f459d7 100644 Binary files a/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-313.pyc and b/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-313.pyc differ diff --git a/gripper_control_02/config/gripper_demo_02.py b/gripper_control_02/config/gripper_demo_02.py index aa2a1a7..1abc5e3 100644 --- a/gripper_control_02/config/gripper_demo_02.py +++ b/gripper_control_02/config/gripper_demo_02.py @@ -73,6 +73,9 @@ SHEAR_FILTER_ALPHA = 0.65 TRIGGER_NORMAL_FORCE = 0.1 # 单侧法向力触发阈值，单位 N。 TRIGGER_SHEAR_FORCE = 0.1 # 单侧切向力触发阈值，单位 N。 TRIGGER_CLEAR_STABLE_SECONDS = 0.2 # 张开恢复后，必须先看到触发力清掉并稳定这么久，才允许下一次扫过触发。 +OPEN_WAIT_STABLE_SAMPLES = 12 # 张开等待时，用多少个近期样本判断当前稳定基准。 +OPEN_WAIT_STABLE_RANGE_N = 0.03 # 近期样本最大-最小值小于该值，认为该通道趋于稳定，单位 N。 +OPEN_WAIT_STABLE_TREND_N = 0.015 # 近期样本首尾变化小于该值，认为该通道无明显漂移，单位 N。 # 闭合夹取：物体移动到中间后，左右两边都超过该阈值就认为夹住。 @@ -202,6 +205,9 @@ CONFIG = { "trigger_normal_force": TRIGGER_NORMAL_FORCE, "trigger_shear_force": TRIGGER_SHEAR_FORCE, "trigger_clear_stable_seconds": TRIGGER_CLEAR_STABLE_SECONDS, + "open_wait_stable_samples": OPEN_WAIT_STABLE_SAMPLES, + "open_wait_stable_range_n": OPEN_WAIT_STABLE_RANGE_N, + "open_wait_stable_trend_n": OPEN_WAIT_STABLE_TREND_N, "grip_normal_force": GRIP_NORMAL_FORCE, "grip_requires_both": GRIP_REQUIRES_BOTH, "close_timeout_seconds": CLOSE_TIMEOUT_SECONDS, diff --git a/gripper_control_02/controller.py b/gripper_control_02/controller.py index 06b86a9..5c477c6 100644 --- a/gripper_control_02/controller.py +++ b/gripper_control_02/controller.py @@ -127,12 +127,19 @@ class SideTriggerGripController: 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() @@ -156,6 +163,7 @@ class SideTriggerGripController: recover_position_since = None trigger_clear_since = None trigger_armed = False + reset_open_wait_stability() release_armed = False hold_pos = None reference = None @@ -177,6 +185,7 @@ class SideTriggerGripController: recover_position_since = None trigger_clear_since = None trigger_armed = False + reset_open_wait_stability() release_armed = False hold_pos = None reference = None @@ -194,6 +203,7 @@ class SideTriggerGripController: 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" @@ -203,6 +213,7 @@ class SideTriggerGripController: state_start = now trigger_clear_since = None trigger_armed = False + reset_open_wait_stability() release_armed = False hold_pos = None reference = None @@ -239,21 +250,21 @@ class SideTriggerGripController: if manual_action is not None: pass elif state == "open_wait": - trigger_edge = trigger and not last_trigger + 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: - if trigger_clear_since is None: - trigger_clear_since = now - action = "open-wait-trigger-clearing" - elif now - trigger_clear_since >= config.trigger_clear_stable_seconds: - trigger_armed = True - action = "open-wait-armed" - else: - action = "open-wait-trigger-clearing" - elif not trigger_armed: - trigger_clear_since = None - action = "open-wait-clear-trigger" - elif not trigger_edge: - action = "open-wait-trigger-held" + 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, @@ -271,6 +282,7 @@ class SideTriggerGripController: 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 @@ -308,6 +320,7 @@ class SideTriggerGripController: 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 @@ -404,6 +417,7 @@ class SideTriggerGripController: 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 @@ -442,6 +456,7 @@ class SideTriggerGripController: recover_position_since = None trigger_clear_since = None trigger_armed = True + reset_open_wait_stability() last_trigger = trigger release_armed = False hold_pos = None @@ -464,6 +479,7 @@ class SideTriggerGripController: recover_position_since = None trigger_clear_since = None trigger_armed = True + reset_open_wait_stability() last_trigger = trigger release_armed = False hold_pos = None @@ -526,6 +542,51 @@ class SideTriggerGripController: 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 diff --git a/gripper_control_02/settings.py b/gripper_control_02/settings.py index 44ff382..b61e232 100644 --- a/gripper_control_02/settings.py +++ b/gripper_control_02/settings.py @@ -63,6 +63,9 @@ class Demo02Config: trigger_normal_force: float = 0.04 trigger_shear_force: float = 0.04 trigger_clear_stable_seconds: float = 0.2 + open_wait_stable_samples: int = 12 + open_wait_stable_range_n: float = 0.03 + open_wait_stable_trend_n: float = 0.015 grip_normal_force: float = 0.05 grip_requires_both: bool = True close_timeout_seconds: float = 5.0 @@ -151,6 +154,9 @@ class Demo02Config: add("--trigger-normal-force", type=float, default=get("trigger_normal_force", cls.trigger_normal_force)) add("--trigger-shear-force", type=float, default=get("trigger_shear_force", cls.trigger_shear_force)) add("--trigger-clear-stable-seconds", type=float, default=get("trigger_clear_stable_seconds", cls.trigger_clear_stable_seconds)) + add("--open-wait-stable-samples", type=int, default=get("open_wait_stable_samples", cls.open_wait_stable_samples)) + add("--open-wait-stable-range-n", type=float, default=get("open_wait_stable_range_n", cls.open_wait_stable_range_n)) + add("--open-wait-stable-trend-n", type=float, default=get("open_wait_stable_trend_n", cls.open_wait_stable_trend_n)) add("--grip-normal-force", type=float, default=get("grip_normal_force", cls.grip_normal_force)) add("--grip-requires-both", dest="grip_requires_both", action="store_true", default=get("grip_requires_both", cls.grip_requires_both)) add("--allow-single-side-grip", dest="grip_requires_both", action="store_false") diff --git a/gripper_control_02/visualizer.py b/gripper_control_02/visualizer.py index 796196a..41064fc 100644 --- a/gripper_control_02/visualizer.py +++ b/gripper_control_02/visualizer.py @@ -97,6 +97,62 @@ def fmt(value, unit="", width=0, precision=3): return f"{text}{unit}" +STATE_TEXT = { + "open_wait": "等待触发", + "closing": "闭合中", + "gripping": "夹持加力", + "hold_check": "夹持检测", + "open_recover": "张开恢复", + "manual_close": "手动闭合", + "manual_hold": "手动保持", + "error": "错误", +} + +ACTION_TEXT = { + "wait": "等待", + "waiting-for-sensor-samples": "等待传感器数据", + "open-wait-stabilizing": "等待稳定基准", + "open-wait-armed": "已就绪", + "side-trigger-close": "侧边触发闭合", + "closing-to-grip": "闭合寻找物体", + "object-detected-low-force": "检测到物体", + "close-timeout-open": "闭合超时张开", + "grip-ramp-up": "夹持加力", + "grip-hold": "夹持保持", + "hold-reference-armed": "记录夹持基准", + "adaptive-grip-ramp": "自适应加力", + "adaptive-grip-wait": "等待自适应加力", + "release-arm-delay": "释放检测延时", + "release-reference-armed": "释放检测就绪", + "hold-check": "保持检测", + "force-change-release-open": "力变化张开", + "open-recover-position": "张开位置恢复", + "open-recover-position-stabilizing": "张开位置稳定中", + "open-ready-position": "张开到位", + "open-recover-quiet": "力稳定恢复", + "open-recover-stabilizing": "恢复稳定中", + "open-recover-wait-force-clear": "等待力恢复", + "open-ready": "恢复完成", + "manual-open": "手动张开", + "manual-close": "手动闭合", + "manual-hold": "手动保持", + "manual-release": "手动释放", +} + + +def label_with_cn(value, mapping): + value = str(value or "--") + cn = mapping.get(value) + if cn is None: + for prefix, text in mapping.items(): + if value.startswith(f"{prefix} "): + cn = text + break + if cn is None: + return value + return f"{value}（{cn}）" + + STYLE = """ QMainWindow { background-color: #000000; @@ -140,6 +196,22 @@ QLabel#StatusChip { font-weight: 600; padding: 8px 12px; } +QFrame#StatusPanel { + background-color: #101010; + border: 1px solid #303030; + border-radius: 4px; +} +QLabel#StatusMain { + color: #00e5ff; + font-family: "Cascadia Code", "Consolas"; + font-size: 17px; + font-weight: 600; +} +QLabel#StatusSub { + color: #9a9a9a; + font-family: "Cascadia Code", "Consolas"; + font-size: 12px; +} QLabel#ActionChip { background-color: #0b0b0b; border: 1px solid #262626; @@ -332,8 +404,17 @@ class GripperDemo02Viewer(QMainWindow): top.setSpacing(8) title = QLabel("Orisys 夹爪") title.setObjectName("Title") + self.status_panel = QFrame() + self.status_panel.setObjectName("StatusPanel") + status_layout = QVBoxLayout(self.status_panel) + status_layout.setContentsMargins(10, 6, 10, 6) + status_layout.setSpacing(1) self.status_chip = QLabel("state=--") - self.status_chip.setObjectName("StatusChip") + self.status_chip.setObjectName("StatusMain") + self.action_label = QLabel("action=--") + self.action_label.setObjectName("StatusSub") + status_layout.addWidget(self.status_chip) + status_layout.addWidget(self.action_label) self.start_button = AnimatedButton("启动") self.start_button.setObjectName("StartButton") self.start_button.clicked.connect(self.start_control) @@ -355,7 +436,7 @@ class GripperDemo02Viewer(QMainWindow): self.manual_hold_button.clicked.connect(self.toggle_manual_hold) self._set_button_state("idle") top.addWidget(title) - top.addWidget(self.status_chip) + top.addWidget(self.status_panel) top.addStretch(1) top.addWidget(self.start_button) top.addWidget(self.stop_button) @@ -557,9 +638,11 @@ class GripperDemo02Viewer(QMainWindow): self._set_button_state("running") state = status.get("state", "--") + action = status.get("action", "--") unit = status.get("normal_unit", "N") shear_unit = status.get("shear_unit", "N") - self.status_chip.setText(f"state={state}") + self.status_chip.setText(f"state={label_with_cn(state, STATE_TEXT)}") + self.action_label.setText(f"action={label_with_cn(action, ACTION_TEXT)}") self.manual_hold_button.setText( "释放" if bool(status.get("manual_hold_active")) else "保持" )