夹爪可视化界面部分更新，主要是3D模型

gripper/control/controller.py

@@ -23,15 +23,41 @@ class SideTriggerGripController:
         self._stop_event = threading.Event()
         self._manual_lock = threading.Lock()
         self._manual_command = None
+        self._mode_lock = threading.Lock()
+        self._control_mode = "smart"
 
     def stop(self):
         self._stop_event.set()
 
-    def request_manual_command(self, command):
-        if command not in {"open", "close", "hold", "release"}:
+    def request_manual_command(self, command, speed_pct=None, force_pct=None, position=None):
+        if command not in {
+            "open",
+            "close",
+            "hold",
+            "release",
+            "move_position",
+            "restart_left_sensor",
+            "restart_right_sensor",
+        }:
             raise ValueError(f"unsupported manual command: {command}")
+        payload = {
+            "command": command,
+            "speed_pct": speed_pct,
+            "force_pct": force_pct,
+            "position": position,
+        }
         with self._manual_lock:
-            self._manual_command = command
+            self._manual_command = payload
+
+    def set_control_mode(self, mode):
+        if mode not in {"smart", "manual"}:
+            raise ValueError(f"unsupported control mode: {mode}")
+        with self._mode_lock:
+            self._control_mode = mode
+
+    def _get_control_mode(self):
+        with self._mode_lock:
+            return self._control_mode
 
     def _pop_manual_command(self):
         with self._manual_lock:
@@ -134,6 +160,45 @@ class SideTriggerGripController:
         release_armed = False
         hold_pos = None
         reference = None
+        current_position = None
+        last_position_status_time = 0.0
+        position_perf = {
+            "last_log": time.perf_counter(),
+            "count": 0,
+            "total": 0.0,
+            "max": 0.0,
+        }
+
+        def update_current_position(now, force=False):
+            nonlocal current_position, last_position_status_time, position_perf
+            if force or now - last_position_status_time >= 0.1:
+                last_position_status_time = now
+                t0 = time.perf_counter()
+                current_position = self.gripper.read_position()
+                dt = time.perf_counter() - t0
+                position_perf["count"] += 1
+                position_perf["total"] += dt
+                position_perf["max"] = max(position_perf["max"], dt)
+                if now - position_perf["last_log"] >= 1.0:
+                    avg_ms = (
+                        position_perf["total"] / position_perf["count"] * 1000.0
+                        if position_perf["count"]
+                        else 0.0
+                    )
+                    print(
+                        "[perf] position_read "
+                        f"count={position_perf['count']} "
+                        f"avg={avg_ms:.2f}ms "
+                        f"max={position_perf['max'] * 1000.0:.2f}ms "
+                        f"pos={current_position}"
+                    )
+                    position_perf = {
+                        "last_log": now,
+                        "count": 0,
+                        "total": 0.0,
+                        "max": 0.0,
+                    }
+            return current_position
 
         def reset_open_wait_stability():
             nonlocal open_wait_samples, open_wait_stable_values
@@ -144,15 +209,26 @@ class SideTriggerGripController:
             tick_start = time.perf_counter()
             now = time.perf_counter()
             manual_action = None
+            control_mode = self._get_control_mode()
 
-            manual_command = self._pop_manual_command()
+            manual_payload = self._pop_manual_command()
+            if isinstance(manual_payload, dict):
+                manual_command = manual_payload.get("command")
+                manual_speed_pct = manual_payload.get("speed_pct")
+                manual_force_pct = manual_payload.get("force_pct")
+                manual_position = manual_payload.get("position")
+            else:
+                manual_command = manual_payload
+                manual_speed_pct = None
+                manual_force_pct = None
+                manual_position = None
             if manual_command == "open":
                 force_pct = int(clamp(
-                    config.open_force,
+                    config.open_force if manual_force_pct is None else manual_force_pct,
                     config.force_min,
                     config.force_max,
                 ))
-                self.gripper.open(force_pct, "manual-open")
+                self.gripper.open(force_pct, "manual-open", speed_pct=manual_speed_pct)
                 self.feedback_processor.reset_filters()
                 state = "open_recover"
                 state_start = now
@@ -170,11 +246,11 @@ class SideTriggerGripController:
                 manual_action = "manual-open"
             elif manual_command == "close":
                 force_pct = int(clamp(
-                    config.close_force,
+                    config.close_force if manual_force_pct is None else manual_force_pct,
                     config.force_min,
                     config.force_max,
                 ))
-                self.gripper.close(force_pct, "manual-close")
+                self.gripper.close(force_pct, "manual-close", speed_pct=manual_speed_pct)
                 state = "manual_close"
                 state_start = now
                 last_force_ramp_time = now
@@ -192,12 +268,15 @@ class SideTriggerGripController:
                 manual_action = "manual-close"
             elif manual_command == "hold":
                 force_pct = int(clamp(
-                    config.hold_force,
+                    config.hold_force if manual_force_pct is None else manual_force_pct,
                     config.force_min,
                     config.force_max,
                 ))
                 self.gripper.set_force(force_pct)
-                hold_pos = self.gripper.hold_current_position(force_pct)
+                hold_pos = self.gripper.hold_current_position(
+                    force_pct,
+                    speed_pct=manual_speed_pct,
+                )
                 state = "manual_hold"
                 state_start = now
                 last_force_ramp_time = now
@@ -218,10 +297,62 @@ class SideTriggerGripController:
                     hold_pos = None
                     reference = None
                 manual_action = "manual-release"
+            elif manual_command == "move_position":
+                target_min = min(int(config.open_pos), int(config.close_pos))
+                target_max = max(int(config.open_pos), int(config.close_pos))
+                target_position = int(clamp(
+                    int(manual_position if manual_position is not None else config.open_pos),
+                    target_min,
+                    target_max,
+                ))
+                force_pct = int(clamp(
+                    config.close_force if manual_force_pct is None else manual_force_pct,
+                    config.force_min,
+                    config.force_max,
+                ))
+                self.gripper.move(
+                    target_position,
+                    force_pct,
+                    "manual-move-position",
+                    speed_pct=manual_speed_pct,
+                )
+                state = "manual_position"
+                state_start = now
+                last_force_ramp_time = now
+                last_open_position_check_time = 0.0
+                last_open_position = target_position
+                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
+                current_position = target_position
+                manual_action = "manual-move-position"
+            elif manual_command == "restart_left_sensor":
+                if self.reader.restart_sensor(0):
+                    reset_open_wait_stability()
+                    trigger_clear_since = None
+                    trigger_armed = False
+                    manual_action = "left-sensor-restart"
+                else:
+                    manual_action = "left-sensor-restart-failed"
+            elif manual_command == "restart_right_sensor":
+                if self.reader.restart_sensor(1):
+                    reset_open_wait_stability()
+                    trigger_clear_since = None
+                    trigger_armed = False
+                    manual_action = "right-sensor-restart"
+                else:
+                    manual_action = "right-sensor-restart-failed"
 
             feedback = self.feedback_processor.read(self.reader, timeout=0.3)
             if feedback is None:
                 action = manual_action or "waiting-for-sensor-samples"
+                position_for_status = update_current_position(now)
                 print(f"state={state} action={action}")
                 self._emit_status(
                     {
@@ -233,7 +364,11 @@ class SideTriggerGripController:
                         "adaptive_grip_enabled": config.adaptive_grip_enabled,
                         "release_armed": release_armed,
                         "manual_hold_active": state == "manual_hold",
+                        "control_mode": control_mode,
                         "speed_pct": config.speed,
+                        "gripper_position": position_for_status,
+                        "open_pos": config.open_pos,
+                        "close_pos": config.close_pos,
                         "normal_unit": self.normal_unit,
                         "shear_unit": self.shear_unit,
                     }
@@ -249,6 +384,8 @@ class SideTriggerGripController:
 
             if manual_action is not None:
                 pass
+            elif control_mode == "manual" and state not in {"manual_hold", "manual_close", "manual_position"}:
+                action = "manual-standby"
             elif state == "open_wait":
                 open_wait_sample = self._open_wait_sample(feedback)
                 self._append_open_wait_sample(open_wait_samples, open_wait_sample)
@@ -506,6 +643,9 @@ class SideTriggerGripController:
             elif state == "manual_close":
                 action = "manual-close"
 
+            elif state == "manual_position":
+                action = "manual-position"
+
             self._log_tick(
                 feedback=feedback,
                 state=state,
@@ -520,6 +660,7 @@ class SideTriggerGripController:
                 action=action,
                 trigger_armed=trigger_armed,
             )
+            position_for_status = update_current_position(now)
             self._emit_status(
                 self._make_status(
                     feedback=feedback,
@@ -534,6 +675,8 @@ class SideTriggerGripController:
                     release_armed=release_armed,
                     action=action,
                     trigger_armed=trigger_armed,
+                    control_mode=control_mode,
+                    gripper_position=position_for_status,
                 )
             )
             last_trigger = trigger
@@ -713,6 +856,8 @@ class SideTriggerGripController:
         release_armed,
         action,
         trigger_armed,
+        control_mode="smart",
+        gripper_position=None,
     ):
         speed_pct = self.config.speed
         if action == "force-change-release-open":
@@ -755,7 +900,11 @@ class SideTriggerGripController:
             "adaptive_grip_enabled": self.config.adaptive_grip_enabled,
             "release_armed": bool(release_armed),
             "manual_hold_active": state == "manual_hold",
+            "control_mode": control_mode,
             "speed_pct": speed_pct,
+            "gripper_position": gripper_position,
+            "open_pos": self.config.open_pos,
+            "close_pos": self.config.close_pos,
             "normal_unit": self.normal_unit,
             "shear_unit": self.shear_unit,
             "left_sample": left_sample,

gripper/hardware/gripper_client.py

@@ -6,6 +6,7 @@ class GripperClient:
     def __init__(self, config):
         self.config = config
         self.motor = None
+        self.last_commanded_position = None
 
     def connect(self):
         if self.config.dry_run:
@@ -24,6 +25,7 @@ class GripperClient:
 
     def move(self, position, force_pct, label, speed_pct=None, accel=None, decel=None):
         config = self.config
+        self.last_commanded_position = int(position)
         speed_pct = config.speed if speed_pct is None else speed_pct
         accel = config.accel if accel is None else accel
         decel = config.decel if decel is None else decel
@@ -60,8 +62,15 @@ class GripperClient:
             decel=decel,
         )
 
-    def close(self, force_pct, label="close"):
-        self.move(self.config.close_pos, force_pct, label)
+    def close(self, force_pct, label="close", speed_pct=None, accel=None, decel=None):
+        self.move(
+            self.config.close_pos,
+            force_pct,
+            label,
+            speed_pct=speed_pct,
+            accel=accel,
+            decel=decel,
+        )
 
     def set_force(self, force_pct):
         config = self.config
@@ -77,14 +86,17 @@ class GripperClient:
 
     def read_position(self):
         if self.config.dry_run:
-            return None
+            return self.last_commanded_position
         try:
-            return self.motor.read_real_position()
+            position = self.motor.read_real_position()
+            if position is not None:
+                return int(position)
+            return self.last_commanded_position
         except Exception as exc:
             print(f"read_real_position failed: {exc}")
-            return None
+            return self.last_commanded_position
 
-    def hold_current_position(self, force_pct):
+    def hold_current_position(self, force_pct, speed_pct=None, accel=None, decel=None):
         current_pos = self.read_position()
         if current_pos is None:
             current_pos = self.config.hold_fallback_pos
@@ -94,7 +106,14 @@ class GripperClient:
             self._stop_by_speed()
             return None
 
-        self.move(int(current_pos), force_pct, "clamp-hold")
+        self.move(
+            int(current_pos),
+            force_pct,
+            "clamp-hold",
+            speed_pct=speed_pct,
+            accel=accel,
+            decel=decel,
+        )
         return int(current_pos)
 
     def final_open_if_needed(self):

gripper/sensors/force_reader.py

@@ -316,29 +316,55 @@ class TwoSensorForceReader:
 
         self.result_queues = [Queue(maxsize=self.queue_size) for _ in self.sensor_configs]
         self.stop_events = [mp.Event() for _ in self.sensor_configs]
+        self.processes = [None] * len(self.sensor_configs)
         self.latest = [None] * len(self.sensor_configs)
 
-        for idx, config in enumerate(self.sensor_configs):
-            process = Process(
-                target=_sensor_force_worker,
-                args=(
-                    config["sensor_id"],
-                    config["vid_src"],
-                    config["config_name"],
-                    config["rotation_config"],
-                    self.result_queues[idx],
-                    self.stop_events[idx],
-                    self.target_fps,
-                    self.cuda,
-                    self.isstitch,
-                    self.backend,
-                    self.motion_threshold,
-                    self.include_visuals,
-                    self.visual_size,
-                ),
-            )
-            process.start()
-            self.processes.append(process)
+        for idx in range(len(self.sensor_configs)):
+            self._start_sensor_process(idx)
+
+    def _start_sensor_process(self, idx):
+        config = self.sensor_configs[idx]
+        process = Process(
+            target=_sensor_force_worker,
+            args=(
+                config["sensor_id"],
+                config["vid_src"],
+                config["config_name"],
+                config["rotation_config"],
+                self.result_queues[idx],
+                self.stop_events[idx],
+                self.target_fps,
+                self.cuda,
+                self.isstitch,
+                self.backend,
+                self.motion_threshold,
+                self.include_visuals,
+                self.visual_size,
+            ),
+        )
+        process.start()
+        self.processes[idx] = process
+
+    def restart_sensor(self, idx):
+        if idx < 0 or idx >= len(self.sensor_configs):
+            raise ValueError(f"invalid sensor index: {idx}")
+        if not self.processes:
+            return False
+
+        old_event = self.stop_events[idx]
+        old_process = self.processes[idx]
+        old_event.set()
+        if old_process is not None:
+            old_process.join(timeout=2.0)
+            if old_process.is_alive():
+                old_process.terminate()
+                old_process.join(timeout=1.0)
+
+        self.result_queues[idx] = Queue(maxsize=self.queue_size)
+        self.stop_events[idx] = mp.Event()
+        self.latest[idx] = None
+        self._start_sensor_process(idx)
+        return True
 
     def update(self):
         for idx, result_queue in enumerate(self.result_queues):
@@ -381,6 +407,8 @@ class TwoSensorForceReader:
             event.set()
 
         for process in self.processes:
+            if process is None:
+                continue
             process.join(timeout=2.0)
             if process.is_alive():
                 process.terminate()