夹爪可视化界面部分更新，主要是3D模型
This commit is contained in:
cheng
+159
-10
@@ -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,
|
||||
|
||||
Reference in New Issue
Block a user