orisys/test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

qt第一版可视化

Browse Source
This commit is contained in:
cheng
2026-06-09 09:31:44 +08:00
parent cbd8683429
commit 66a00a8b5b
39 changed files with 1055 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
gripper_control_02/README.md
+45 -3
View File
@@ -12,8 +12,9 @@
6. 像 01 一样进入 `gripping`，慢慢加力到 `HOLD_FORCE`。
7. 进入 `hold_check` 后记录夹持参考力。
8. 后续法向力或切向力变化超过阈值，夹爪直接张开到 `OPEN_POS`。
9. 张开后进入 `open_recover`，保持张开并等待传感器力清空。
10. 法向和切向读数都低于恢复阈值并稳定后，才回到 `open_wait` 等待下一次单侧扫过触发。
9. 张开后进入 `open_recover`，保持张开并等待恢复条件。
10. 夹爪回到 `OPEN_POS` 初始点位，或法向/切向读数都低于恢复阈值并稳定后，回到 `open_wait`。
11. 回到 `open_wait` 后，程序必须先看到触发力清掉并稳定，才会重新武装；下一次扫过才允许闭合。
## 运行
@@ -30,6 +31,24 @@ python -m gripper_control_02.main
python -m gripper_control_02.main --dry-run
```
PyQt 可视化展示：
```powershell
python -m gripper_control_02.visualizer
```
窗口模式运行：
```powershell
python -m gripper_control_02.visualizer --windowed
```
兼容入口：
```powershell
python examples\gripper_demo_02_viewer.py
```
默认配置文件：
```text
@@ -40,6 +59,7 @@ gripper_control_02\config\gripper_demo_02.py
- `TRIGGER_NORMAL_FORCE`: 单侧法向力触发阈值。
- `TRIGGER_SHEAR_FORCE`: 单侧切向力触发阈值。
- `TRIGGER_CLEAR_STABLE_SECONDS`: 张开恢复后，触发力必须清掉并稳定多久，才允许下一次扫过触发。
- `GRIP_NORMAL_FORCE`: 夹住时左右接触阈值。
- `GRIP_REQUIRES_BOTH`: 是否必须左右都接触才算夹住。
- `GRIP_START_FORCE`: 检测到物体后先切到的低力。
@@ -49,10 +69,17 @@ gripper_control_02\config\gripper_demo_02.py
- `GRIP_SETTLE_SECONDS`: 夹住后等待稳定多久再记录参考力。
- `RELEASE_NORMAL_CHANGE`: 夹住后法向力变化超过它就松开。
- `RELEASE_SHEAR_CHANGE`: 夹住后切向力变化超过它就松开。
- `RELEASE_OPEN_SPEED`: 检测到松开后，张开到 `OPEN_POS` 使用的速度。
- `RELEASE_OPEN_ACCEL`: release 张开的加速度。
- `RELEASE_OPEN_DECEL`: release 张开的减速度。
- `REARM_SECONDS`: 张开后最短等待时间，防止刚松开马上又误触发。
- `RECOVER_NORMAL_FORCE`: 张开恢复时允许重新触发的最大法向残余力。
- `RECOVER_SHEAR_FORCE`: 张开恢复时允许重新触发的最大切向残余力。
- `RECOVER_STABLE_SECONDS`: 力清空后需要连续稳定多久，才重新等待触发。
- `OPEN_POSITION_RECOVER_ENABLED`: 是否允许夹爪回到 `OPEN_POS` 后直接恢复等待。
- `OPEN_POSITION_TOLERANCE`: 当前点位和 `OPEN_POS` 差值小于它，就认为已经回到初始点位。
- `OPEN_POSITION_STABLE_SECONDS`: 回到初始点位后需要稳定多久，才重新等待触发。
- `OPEN_POSITION_CHECK_INTERVAL`: `open_recover` 中读取夹爪当前位置的间隔。
## 日志字段
@@ -60,6 +87,7 @@ gripper_control_02\config\gripper_demo_02.py
- `maxN`: 左右法向力最大值，单位 N。
- `shear`: 左右切向力最大值，单位 N。
- `trigger`: 是否满足单侧触发条件。
- `armed`: 是否已经重新武装；只有 `trigger=1 armed=1` 才会闭合。
- `grip_contact`: 是否满足夹住接触条件。
- `dN`: 夹住后当前法向力相对参考值的最大变化，单位 N。
- `dShear`: 夹住后当前切向力相对参考值的变化，单位 N。
@@ -68,17 +96,31 @@ gripper_control_02\config\gripper_demo_02.py
- `force_pct`: 当前夹爪目标力百分比。
- `action`: 当前动作。
## 可视化展示
- 左右两边的光流箭头图。
- 左右两边的光流幅值图。
- 左右法向力、最大切向力、左右差值。
- 当前 `state`、`action`、`trigger/armed`、`grip_contact`。
- 当前 `force_pct`、速度 `speed_pct`、`hold_pos`。
- 左右传感器 FPS、接触标志、光流均值/最大值。
## 状态说明
- `open_wait`: 夹爪张开，等待任意单侧触发。
- `closing`: 单侧触发后正在闭合，等待检测到物体。
- `gripping`: 检测到物体后低力保持当前位置，并慢慢加力。
- `hold_check`: 已夹住并记录参考力，监测法向/切向变化；变化超过阈值会直接张开。
- `open_recover`: 已经张开到初始位置，正在等待法向/切向残余力清空；这个状态不会闭合夹爪。
- `open_recover`: 已经发出张开命令，正在等待回到初始点位或等待法向/切向残余力清空；这个状态不会闭合夹爪。
## 常见动作说明
- `force-change-release-open`: 检测到夹住后的法向或切向变化，已经发出张开命令。
- `open-recover-position`: 夹爪当前位置已经接近 `OPEN_POS`，正在等待到位稳定。
- `open-ready-position`: 夹爪已经回到初始点位，重新进入等待下一次扫过触发。
- `open-wait-clear-trigger`: 已经回到等待状态，但当前力还超过触发阈值，暂时不会闭合。
- `open-wait-trigger-clearing`: 触发力已经低于阈值，正在等待清零稳定时间。
- `open-wait-armed`: 触发已经清掉并稳定，下一次扫过可以闭合。
- `open-recover-wait-force-clear`: 夹爪保持张开，但传感器还有残余力，不允许重新闭合。
- `open-recover-stabilizing`: 力已经低于恢复阈值，正在等待稳定时间。
- `open-ready`: 力已经清空并稳定，重新进入等待下一次扫过触发。
gripper_control_02/__pycache__/__init__.cpython-313.pyc
BIN
View File
Binary file not shown.
gripper_control_02/__pycache__/controller.cpython-311.pyc
BIN
View File
Binary file not shown.
gripper_control_02/__pycache__/controller.cpython-313.pyc
BIN
View File
Binary file not shown.
gripper_control_02/__pycache__/main.cpython-311.pyc
BIN
View File
Binary file not shown.
gripper_control_02/__pycache__/main.cpython-313.pyc
BIN
View File
Binary file not shown.
gripper_control_02/__pycache__/settings.cpython-311.pyc
BIN
View File
Binary file not shown.
gripper_control_02/__pycache__/settings.cpython-313.pyc
BIN
View File
Binary file not shown.
gripper_control_02/__pycache__/visualizer.cpython-311.pyc
BIN
View File
Binary file not shown.
gripper_control_02/__pycache__/visualizer.cpython-313.pyc
BIN
View File
Binary file not shown.
gripper_control_02/config/__pycache__/__init__.cpython-313.pyc
BIN
View File
Binary file not shown.
gripper_control_02/config/__pycache__/gripper_demo_02.cpython-311.pyc
BIN
View File
Binary file not shown.
gripper_control_02/config/__pycache__/gripper_demo_02.cpython-313.pyc
BIN
View File
Binary file not shown.
gripper_control_02/config/gripper_demo_02.py
+29 -10
View File
@@ -43,6 +43,9 @@ CLOSE_POS = 9000
SPEED = 25
ACCEL = 80
DECEL = 80
RELEASE_OPEN_SPEED = 50 # 检测到人取物/力变化后，张开使用的速度。
RELEASE_OPEN_ACCEL = 120 # release 张开的加速度。
RELEASE_OPEN_DECEL = 120 # release 张开的减速度。
# 夹爪力度百分比。
@@ -54,21 +57,22 @@ FORCE_MIN = 10
FORCE_MAX = 30
# 控制循环频率。
CONTROL_HZ = 20.0
# 控制循环频率。越高检测释放越快。
CONTROL_HZ = 30.0
# 启动时采集空载基线的时间。
BASELINE_SECONDS = 1.0
# 滤波系数，越大响应越快，越小越平滑。
FILTER_ALPHA = 0.35
SHEAR_FILTER_ALPHA = 0.45
FILTER_ALPHA = 0.55
SHEAR_FILTER_ALPHA = 0.65
# 等待触发：任意一侧超过这些阈值，就开始闭合。
TRIGGER_NORMAL_FORCE = 0.04 # 单侧法向力触发阈值，单位 N。
TRIGGER_SHEAR_FORCE = 0.04 # 单侧切向力触发阈值，单位 N。
TRIGGER_NORMAL_FORCE = 0.1 # 单侧法向力触发阈值，单位 N。
TRIGGER_SHEAR_FORCE = 0.1 # 单侧切向力触发阈值，单位 N。
TRIGGER_CLEAR_STABLE_SECONDS = 0.2 # 张开恢复后，必须先看到触发力清掉并稳定这么久，才允许下一次扫过触发。
# 闭合夹取：物体移动到中间后，左右两边都超过该阈值就认为夹住。
@@ -79,12 +83,12 @@ CLOSE_TIMEOUT_SECONDS = 5.0 # 触发闭合后这么久还没夹住，就重新
# 检测到物体后慢慢加力，类似 01 的 gripping 状态。
FORCE_RAMP_STEP = 1
FORCE_RAMP_INTERVAL = 0.2
GRIP_SETTLE_SECONDS = 0.3 # 加到 HOLD_FORCE 后再稳定这么久，进入 hold_check。
FORCE_RAMP_INTERVAL = 0.1
GRIP_SETTLE_SECONDS = 0.1 # 加到 HOLD_FORCE 后再稳定这么久，进入 hold_check。
# 松开触发阈值。夹住后，法向或切向变化超过其中任意一个，就张开。
RELEASE_NORMAL_CHANGE = 0.05 # 左/右法向力相对夹住参考值的变化阈值，单位 N。
RELEASE_SHEAR_CHANGE = 0.05 # 切向力相对夹住参考值的变化阈值，单位 N。
RELEASE_NORMAL_CHANGE = 0.2 # 左/右法向力相对夹住参考值的变化阈值，单位 N。
RELEASE_SHEAR_CHANGE = 0.2 # 左/右切向力相对夹住参考值的变化阈值，单位 N。
# 松开后至少等待多久重新进入等待触发，避免刚张开时马上再次触发。
REARM_SECONDS = 0.5
@@ -95,6 +99,13 @@ RECOVER_NORMAL_FORCE = 0.02 # 张开恢复时允许重新触发的最大法向
RECOVER_SHEAR_FORCE = 0.02 # 张开恢复时允许重新触发的最大切向残余力，单位 N。
RECOVER_STABLE_SECONDS = 0.4 # 力清空后需要连续稳定这么久，才回到 open_wait。
# 松开/超时张开后，如果夹爪已经回到 OPEN_POS 初始点位，也允许重新触发。
# 这样传感器上还有一点残余力时，不会一直卡在 open_recover。
OPEN_POSITION_RECOVER_ENABLED = True
OPEN_POSITION_TOLERANCE = 100 # 当前位置和 OPEN_POS 的差值小于它，就认为回到初始点位。
OPEN_POSITION_STABLE_SECONDS = 0.1 # 回到初始点位后稳定这么久，才回到 open_wait。
OPEN_POSITION_CHECK_INTERVAL = 0.1 # open_recover 状态下读取夹爪位置的间隔，单位秒。
# 如果读取当前位置失败，保持当前位置时使用这个备用位置；None 表示不使用。
HOLD_FALLBACK_POS = None
@@ -158,6 +169,9 @@ CONFIG = {
"speed": SPEED,
"accel": ACCEL,
"decel": DECEL,
"release_open_speed": RELEASE_OPEN_SPEED,
"release_open_accel": RELEASE_OPEN_ACCEL,
"release_open_decel": RELEASE_OPEN_DECEL,
"open_force": OPEN_FORCE,
"close_force": CLOSE_FORCE,
@@ -174,6 +188,7 @@ CONFIG = {
"trigger_normal_force": TRIGGER_NORMAL_FORCE,
"trigger_shear_force": TRIGGER_SHEAR_FORCE,
"trigger_clear_stable_seconds": TRIGGER_CLEAR_STABLE_SECONDS,
"grip_normal_force": GRIP_NORMAL_FORCE,
"grip_requires_both": GRIP_REQUIRES_BOTH,
"close_timeout_seconds": CLOSE_TIMEOUT_SECONDS,
@@ -186,6 +201,10 @@ CONFIG = {
"recover_normal_force": RECOVER_NORMAL_FORCE,
"recover_shear_force": RECOVER_SHEAR_FORCE,
"recover_stable_seconds": RECOVER_STABLE_SECONDS,
"open_position_recover_enabled": OPEN_POSITION_RECOVER_ENABLED,
"open_position_tolerance": OPEN_POSITION_TOLERANCE,
"open_position_stable_seconds": OPEN_POSITION_STABLE_SECONDS,
"open_position_check_interval": OPEN_POSITION_CHECK_INTERVAL,
"hold_fallback_pos": HOLD_FALLBACK_POS,
"trigger_force_update": TRIGGER_FORCE_UPDATE,
gripper_control_02/controller.py
+216 -8
View File
@@ -1,18 +1,32 @@
import time
import threading
from gripper_control.filters import clamp
class SideTriggerGripController:
def __init__(self, config, reader, gripper, feedback_processor):
def __init__(
self,
config,
reader,
gripper,
feedback_processor,
status_callback=None,
):
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
self.status_callback = status_callback
self._stop_event = threading.Event()
def stop(self):
self._stop_event.set()
def run(self):
self._stop_event.clear()
self.reader.start()
try:
@@ -59,6 +73,20 @@ class SideTriggerGripController:
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}"
)
print(
"Release open: "
f"speed={self.config.release_open_speed}, "
f"accel={self.config.release_open_accel}, "
f"decel={self.config.release_open_decel}"
)
print(
"Recover: "
f"force_normal={self.config.recover_normal_force:.3f}{self.normal_unit}, "
f"force_shear={self.config.recover_shear_force:.3f}{self.shear_unit}, "
f"open_pos={self.config.open_pos}, "
f"pos_enabled={int(self.config.open_position_recover_enabled)}, "
f"pos_tol={self.config.open_position_tolerance}"
)
def _run_event_monitor(self):
config = self.config
@@ -67,18 +95,35 @@ class SideTriggerGripController:
state = "open_wait"
state_start = time.perf_counter()
last_force_ramp_time = state_start
last_open_position_check_time = 0.0
last_open_position = None
last_open_position_ready = False
recover_quiet_since = None
recover_position_since = None
trigger_clear_since = None
trigger_armed = False
force_pct = int(clamp(config.open_force, config.force_min, config.force_max))
hold_pos = None
reference = None
while True:
while not self._stop_event.is_set():
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")
self._emit_status(
{
"timestamp": time.time(),
"state": state,
"action": "waiting-for-sensor-samples",
"force_pct": force_pct,
"speed_pct": config.speed,
"normal_unit": self.normal_unit,
"shear_unit": self.shear_unit,
}
)
time.sleep(0.05)
continue
@@ -89,7 +134,19 @@ class SideTriggerGripController:
action = "wait"
if state == "open_wait":
if trigger:
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"
else:
force_pct = int(clamp(
config.close_force,
config.force_min,
@@ -99,12 +156,16 @@ class SideTriggerGripController:
state = "closing"
state_start = now
last_force_ramp_time = now
last_open_position_check_time = 0.0
last_open_position = None
last_open_position_ready = False
recover_quiet_since = None
recover_position_since = None
trigger_clear_since = None
trigger_armed = False
hold_pos = None
reference = None
action = "side-trigger-close"
else:
action = "open-wait-touch"
elif state == "closing":
if grip_contact:
@@ -130,7 +191,11 @@ class SideTriggerGripController:
self.feedback_processor.reset_filters()
state = "open_recover"
state_start = now
last_open_position_check_time = 0.0
last_open_position = None
last_open_position_ready = False
recover_quiet_since = None
recover_position_since = None
hold_pos = None
reference = None
action = "close-timeout-open"
@@ -173,11 +238,21 @@ class SideTriggerGripController:
config.force_min,
config.force_max,
))
self.gripper.open(force_pct, "force-change-release-open")
self.gripper.open(
force_pct,
"force-change-release-open",
speed_pct=config.release_open_speed,
accel=config.release_open_accel,
decel=config.release_open_decel,
)
self.feedback_processor.reset_filters()
state = "open_recover"
state_start = now
last_open_position_check_time = 0.0
last_open_position = None
last_open_position_ready = False
recover_quiet_since = None
recover_position_since = None
hold_pos = None
reference = None
action = "force-change-release-open"
@@ -185,7 +260,41 @@ class SideTriggerGripController:
action = "hold-check"
elif state == "open_recover":
if self._recover_quiet(feedback):
current_pos = last_open_position
position_ready = last_open_position_ready
if (
config.open_position_recover_enabled
and now - last_open_position_check_time
>= config.open_position_check_interval
):
last_open_position_check_time = now
current_pos = self.gripper.read_position()
position_ready = self._open_position_ready(current_pos)
last_open_position = current_pos
last_open_position_ready = position_ready
if position_ready:
if recover_position_since is None:
recover_position_since = now
action = f"open-recover-position pos={current_pos}"
elif (
now - recover_position_since
>= config.open_position_stable_seconds
and now - state_start >= config.rearm_seconds
):
state = "open_wait"
state_start = now
recover_quiet_since = None
recover_position_since = None
trigger_clear_since = None
trigger_armed = False
hold_pos = None
reference = None
action = f"open-ready-position pos={current_pos}"
else:
action = f"open-recover-position-stabilizing pos={current_pos}"
elif self._recover_quiet(feedback):
recover_position_since = None
if recover_quiet_since is None:
recover_quiet_since = now
action = "open-recover-quiet"
@@ -196,6 +305,9 @@ class SideTriggerGripController:
state = "open_wait"
state_start = now
recover_quiet_since = None
recover_position_since = None
trigger_clear_since = None
trigger_armed = False
hold_pos = None
reference = None
action = "open-ready"
@@ -203,6 +315,7 @@ class SideTriggerGripController:
action = "open-recover-stabilizing"
else:
recover_quiet_since = None
recover_position_since = None
action = "open-recover-wait-force-clear"
self._log_tick(
@@ -215,12 +328,29 @@ class SideTriggerGripController:
hold_pos=hold_pos,
force_pct=force_pct,
action=action,
trigger_armed=trigger_armed,
)
self._emit_status(
self._make_status(
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,
trigger_armed=trigger_armed,
)
)
sleep_time = interval - (time.perf_counter() - tick_start)
if sleep_time > 0:
time.sleep(sleep_time)
print("demo02 control loop stopped.")
def _side_triggered(self, feedback):
return (
feedback.max_normal >= self.config.trigger_normal_force
@@ -240,10 +370,20 @@ class SideTriggerGripController:
and feedback.max_shear <= self.config.recover_shear_force
)
def _open_position_ready(self, current_pos):
if current_pos is None:
return False
return (
abs(int(current_pos) - int(self.config.open_pos))
<= int(self.config.open_position_tolerance)
)
def _make_reference(self, feedback):
return {
"left_normal": feedback.left_normal,
"right_normal": feedback.right_normal,
"left_shear": feedback.left_shear,
"right_shear": feedback.right_shear,
"max_shear": feedback.max_shear,
}
@@ -255,6 +395,11 @@ class SideTriggerGripController:
abs(feedback.right_normal - reference["right_normal"]),
)
shear_change = abs(feedback.max_shear - reference["max_shear"])
shear_change = max(
shear_change,
abs(feedback.left_shear - reference["left_shear"]),
abs(feedback.right_shear - reference["right_shear"]),
)
return normal_change, shear_change
def _log_tick(
@@ -268,15 +413,78 @@ class SideTriggerGripController:
hold_pos,
force_pct,
action,
trigger_armed,
):
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"trigger={int(trigger)} armed={int(trigger_armed)} "
f"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}"
)
def _make_status(
self,
feedback,
state,
trigger,
grip_contact,
normal_change,
shear_change,
hold_pos,
force_pct,
action,
trigger_armed,
):
speed_pct = self.config.speed
if action == "force-change-release-open":
speed_pct = self.config.release_open_speed
left_sample = None
right_sample = None
try:
if len(self.reader.latest) >= 2:
left_sample = self.reader.latest[0]
right_sample = self.reader.latest[1]
except Exception:
pass
return {
"timestamp": time.time(),
"state": state,
"action": action,
"trigger": bool(trigger),
"trigger_armed": bool(trigger_armed),
"grip_contact": bool(grip_contact),
"left_normal": feedback.left_normal,
"right_normal": feedback.right_normal,
"min_normal": feedback.min_normal,
"max_normal": feedback.max_normal,
"normal_diff": feedback.normal_diff,
"left_shear": feedback.left_shear,
"right_shear": feedback.right_shear,
"max_shear": feedback.max_shear,
"shear_diff": feedback.shear_diff,
"normal_change": normal_change,
"shear_change": shear_change,
"hold_pos": hold_pos,
"force_pct": force_pct,
"speed_pct": speed_pct,
"normal_unit": self.normal_unit,
"shear_unit": self.shear_unit,
"left_sample": left_sample,
"right_sample": right_sample,
}
def _emit_status(self, status):
if self.status_callback is None:
return
try:
self.status_callback(status)
except Exception as exc:
print(f"status callback failed: {exc}")
gripper_control_02/main.py
+15 -2
View File
@@ -10,7 +10,12 @@ from .controller import SideTriggerGripController
from .settings import Demo02Config
def build_controller(argv=None):
def build_controller(
argv=None,
status_callback=None,
include_visuals=False,
visual_size=320,
):
ensure_project_paths()
config = Demo02Config.from_args(argv)
@@ -33,10 +38,18 @@ def build_controller(argv=None):
target_fps=config.sensor_fps,
cuda=not config.sensor_cpu,
motion_threshold=config.motion_threshold,
include_visuals=include_visuals,
visual_size=visual_size,
)
gripper = GripperClient(config)
feedback = TactileFeedbackProcessor(config, normal_converter, shear_converter)
return SideTriggerGripController(config, reader, gripper, feedback)
return SideTriggerGripController(
config,
reader,
gripper,
feedback,
status_callback=status_callback,
)
def main(argv=None):
gripper_control_02/settings.py
+24 -7
View File
@@ -43,6 +43,9 @@ class Demo02Config:
speed: int = 25
accel: int = 80
decel: int = 80
release_open_speed: int = 50
release_open_accel: int = 120
release_open_decel: int = 120
open_force: int = 10
close_force: int = 10
@@ -52,25 +55,30 @@ class Demo02Config:
force_min: int = 10
force_max: int = 30
control_hz: float = 20.0
control_hz: float = 30.0
baseline_seconds: float = 1.0
filter_alpha: float = 0.35
shear_filter_alpha: float = 0.45
filter_alpha: float = 0.55
shear_filter_alpha: float = 0.65
trigger_normal_force: float = 0.04
trigger_shear_force: float = 0.04
trigger_clear_stable_seconds: float = 0.2
grip_normal_force: float = 0.05
grip_requires_both: bool = True
close_timeout_seconds: float = 5.0
force_ramp_step: int = 1
force_ramp_interval: float = 0.2
grip_settle_seconds: float = 0.3
release_normal_change: float = 0.05
release_shear_change: float = 0.05
force_ramp_interval: float = 0.1
grip_settle_seconds: float = 0.1
release_normal_change: float = 0.03
release_shear_change: float = 0.03
rearm_seconds: float = 0.5
recover_normal_force: float = 0.02
recover_shear_force: float = 0.02
recover_stable_seconds: float = 0.4
open_position_recover_enabled: bool = True
open_position_tolerance: int = 100
open_position_stable_seconds: float = 0.1
open_position_check_interval: float = 0.1
hold_fallback_pos: int | None = None
trigger_force_update: bool = False
@@ -116,6 +124,9 @@ class Demo02Config:
add("--speed", type=int, default=get("speed", cls.speed))
add("--accel", type=int, default=get("accel", cls.accel))
add("--decel", type=int, default=get("decel", cls.decel))
add("--release-open-speed", type=int, default=get("release_open_speed", cls.release_open_speed))
add("--release-open-accel", type=int, default=get("release_open_accel", cls.release_open_accel))
add("--release-open-decel", type=int, default=get("release_open_decel", cls.release_open_decel))
add("--open-force", type=int, default=get("open_force", cls.open_force))
add("--close-force", type=int, default=get("close_force", cls.close_force))
@@ -131,6 +142,7 @@ 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("--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")
@@ -144,6 +156,11 @@ class Demo02Config:
add("--recover-normal-force", type=float, default=get("recover_normal_force", cls.recover_normal_force))
add("--recover-shear-force", type=float, default=get("recover_shear_force", cls.recover_shear_force))
add("--recover-stable-seconds", type=float, default=get("recover_stable_seconds", cls.recover_stable_seconds))
add("--open-position-recover-enabled", action="store_true", default=get("open_position_recover_enabled", cls.open_position_recover_enabled))
add("--disable-open-position-recover", dest="open_position_recover_enabled", action="store_false")
add("--open-position-tolerance", type=int, default=get("open_position_tolerance", cls.open_position_tolerance))
add("--open-position-stable-seconds", type=float, default=get("open_position_stable_seconds", cls.open_position_stable_seconds))
add("--open-position-check-interval", type=float, default=get("open_position_check_interval", cls.open_position_check_interval))
add("--hold-fallback-pos", type=int, default=get("hold_fallback_pos", cls.hold_fallback_pos))
add("--trigger-force-update", action="store_true", default=get("trigger_force_update", cls.trigger_force_update))
gripper_control_02/visualizer.py
+562
View File
@@ -0,0 +1,562 @@
import argparse
import multiprocessing as mp
import sys
import threading
import time
import cv2
import numpy as np
def _fix_qt_plugin_path():
try:
from PyQt5.QtCore import QLibraryInfo
import os
os.environ["QT_QPA_PLATFORM_PLUGIN_PATH"] = QLibraryInfo.location(
QLibraryInfo.PluginsPath
)
except Exception:
pass
_fix_qt_plugin_path()
_QT_IMPORT_ERROR = None
try:
from PyQt5.QtCore import Qt, QTimer
from PyQt5.QtGui import QFont, QImage, QPixmap
from PyQt5.QtWidgets import (
QApplication,
QFrame,
QGridLayout,
QHBoxLayout,
QLabel,
QMainWindow,
QPushButton,
QSizePolicy,
QVBoxLayout,
QWidget,
)
except Exception as exc:
_QT_IMPORT_ERROR = exc
Qt = QTimer = QFont = QImage = QPixmap = QApplication = None
QGridLayout = QHBoxLayout = QLabel = QPushButton = QSizePolicy = None
QVBoxLayout = QWidget = None
QFrame = QMainWindow = object
try:
import pyqtgraph as pg
except Exception as exc:
pg = None
_PYQTGRAPH_IMPORT_ERROR = exc
else:
_PYQTGRAPH_IMPORT_ERROR = None
from .main import build_controller
PLOT_POINTS = 180
CURVE_COLOR = "#00E5FF"
def cv2_to_pixmap(img_bgr):
if img_bgr is None:
return QPixmap()
if len(img_bgr.shape) == 2:
img_bgr = cv2.cvtColor(img_bgr, cv2.COLOR_GRAY2BGR)
rgb = cv2.cvtColor(np.ascontiguousarray(img_bgr), cv2.COLOR_BGR2RGB)
h, w, ch = rgb.shape
qimg = QImage(rgb.data, w, h, ch * w, QImage.Format_RGB888).copy()
return QPixmap.fromImage(qimg)
def fmt(value, unit="", width=0, precision=3):
if value is None:
text = "--"
else:
text = f"{float(value):.{precision}f}"
if width:
text = text.rjust(width)
return f"{text}{unit}"
STYLE = """
QMainWindow {
background-color: #000000;
}
QWidget {
background-color: #000000;
color: #9a9a9a;
font-family: "Microsoft YaHei", "SimHei";
}
QLabel#Title {
color: #d7d7d7;
font-size: 18px;
font-weight: 600;
}
QLabel#Subtle {
color: #777777;
font-size: 12px;
}
QLabel#MetricLabel {
color: #888888;
font-size: 12px;
}
QLabel#MetricValue {
color: #f2f2f2;
font-family: "Cascadia Code", "Consolas";
font-size: 22px;
font-weight: 600;
}
QLabel#SmallValue {
color: #d8d8d8;
font-family: "Cascadia Code", "Consolas";
font-size: 15px;
}
QLabel#StatusChip {
background-color: #101010;
border: 1px solid #303030;
border-radius: 4px;
color: #00e5ff;
font-family: "Cascadia Code", "Consolas";
font-size: 18px;
font-weight: 600;
padding: 8px 12px;
}
QLabel#ActionChip {
background-color: #0b0b0b;
border: 1px solid #262626;
border-radius: 4px;
color: #c8c8c8;
font-family: "Cascadia Code", "Consolas";
font-size: 13px;
padding: 7px 10px;
}
QLabel#ImagePanel {
background-color: #0b0b0b;
border: 1px solid #262626;
border-radius: 4px;
}
QFrame#Panel {
background-color: #050505;
border: 1px solid #222222;
border-radius: 4px;
}
QPushButton {
background-color: #1a1a1a;
color: #a8a8a8;
border: 1px solid #333333;
border-radius: 4px;
padding: 7px 16px;
font-size: 13px;
}
QPushButton:hover {
background-color: #262626;
color: #d4d4d4;
border: 1px solid #555555;
}
QPushButton#StartButton {
background-color: #0d2818;
color: #67d79a;
border: 1px solid #1a4d30;
}
QPushButton#StopButton {
background-color: #2b1010;
color: #f07070;
border: 1px solid #4d1a1a;
}
"""
class MetricBox(QFrame):
def __init__(self, title, value="--", sub=""):
super().__init__()
self.setObjectName("Panel")
layout = QVBoxLayout(self)
layout.setContentsMargins(10, 8, 10, 8)
layout.setSpacing(3)
self.title_label = QLabel(title)
self.title_label.setObjectName("MetricLabel")
self.value_label = QLabel(value)
self.value_label.setObjectName("MetricValue")
self.sub_label = QLabel(sub)
self.sub_label.setObjectName("Subtle")
layout.addWidget(self.title_label)
layout.addWidget(self.value_label)
layout.addWidget(self.sub_label)
def set_value(self, value, sub=None):
self.value_label.setText(value)
if sub is not None:
self.sub_label.setText(sub)
class ImageBox(QFrame):
def __init__(self, title):
super().__init__()
self.setObjectName("Panel")
layout = QVBoxLayout(self)
layout.setContentsMargins(8, 8, 8, 8)
layout.setSpacing(6)
self.title_label = QLabel(title)
self.title_label.setObjectName("MetricLabel")
self.image_label = QLabel()
self.image_label.setObjectName("ImagePanel")
self.image_label.setAlignment(Qt.AlignCenter)
self.image_label.setMinimumSize(180, 180)
self.image_label.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
self.image_label.setScaledContents(True)
layout.addWidget(self.title_label)
layout.addWidget(self.image_label, 1)
def set_image(self, img):
self.image_label.setPixmap(cv2_to_pixmap(img))
class GripperDemo02Viewer(QMainWindow):
def __init__(self, argv=None):
super().__init__()
if _QT_IMPORT_ERROR is not None:
raise RuntimeError(f"PyQt5 import failed: {_QT_IMPORT_ERROR}")
if pg is None:
raise RuntimeError(f"pyqtgraph import failed: {_PYQTGRAPH_IMPORT_ERROR}")
self.setWindowTitle("Gripper Demo 02")
self.setStyleSheet(STYLE)
self.controller = None
self.control_thread = None
self.latest_status = None
self.status_lock = threading.Lock()
self.controller_finished = False
self.start_time = time.time()
self.force_history = []
self.argv = argv
self._build_ui()
self._configure_plot()
self.timer = QTimer(self)
self.timer.timeout.connect(self._refresh)
self.timer.start(33)
def _build_ui(self):
root = QWidget()
self.setCentralWidget(root)
outer = QVBoxLayout(root)
outer.setContentsMargins(8, 8, 8, 8)
outer.setSpacing(8)
top = QHBoxLayout()
top.setSpacing(8)
title = QLabel("Gripper Demo 02")
title.setObjectName("Title")
self.status_chip = QLabel("state=--")
self.status_chip.setObjectName("StatusChip")
self.action_chip = QLabel("action=--")
self.action_chip.setObjectName("ActionChip")
self.start_button = QPushButton("启动")
self.start_button.setObjectName("StartButton")
self.start_button.clicked.connect(self.start_control)
self.stop_button = QPushButton("停止")
self.stop_button.setObjectName("StopButton")
self.stop_button.clicked.connect(self.stop_control)
self.stop_button.setEnabled(False)
top.addWidget(title)
top.addWidget(self.status_chip)
top.addWidget(self.action_chip, 1)
top.addWidget(self.start_button)
top.addWidget(self.stop_button)
outer.addLayout(top)
main = QGridLayout()
main.setSpacing(8)
outer.addLayout(main, 1)
self.left_flow = ImageBox("左侧光流")
self.left_mag = ImageBox("左侧幅值图")
self.right_flow = ImageBox("右侧光流")
self.right_mag = ImageBox("右侧幅值图")
main.addWidget(self.left_flow, 0, 0)
main.addWidget(self.left_mag, 0, 1)
main.addWidget(self.right_flow, 1, 0)
main.addWidget(self.right_mag, 1, 1)
side = QVBoxLayout()
side.setSpacing(8)
main.addLayout(side, 0, 2, 2, 1)
metrics_grid = QGridLayout()
metrics_grid.setSpacing(8)
side.addLayout(metrics_grid)
self.left_force = MetricBox("左法向力", "--")
self.right_force = MetricBox("右法向力", "--")
self.shear_force = MetricBox("最大切向力", "--")
self.force_diff = MetricBox("左右差值", "--")
self.force_pct_box = MetricBox("force_pct", "--")
self.speed_box = MetricBox("速度", "--")
self.trigger_box = MetricBox("触发/武装", "--")
self.contact_box = MetricBox("接触", "--")
boxes = [
self.left_force,
self.right_force,
self.shear_force,
self.force_diff,
self.force_pct_box,
self.speed_box,
self.trigger_box,
self.contact_box,
]
for idx, box in enumerate(boxes):
metrics_grid.addWidget(box, idx // 2, idx % 2)
pg.setConfigOption("background", "#000000")
pg.setConfigOption("foreground", "#777777")
pg.setConfigOptions(antialias=True)
self.plot_widget = pg.GraphicsLayoutWidget()
self.plot_widget.setMinimumHeight(240)
side.addWidget(self.plot_widget, 1)
self.info_box = QFrame()
self.info_box.setObjectName("Panel")
info_layout = QVBoxLayout(self.info_box)
info_layout.setContentsMargins(10, 8, 10, 8)
self.info_label = QLabel("等待启动")
self.info_label.setObjectName("SmallValue")
self.info_label.setWordWrap(True)
info_layout.addWidget(self.info_label)
side.addWidget(self.info_box)
main.setColumnStretch(0, 2)
main.setColumnStretch(1, 2)
main.setColumnStretch(2, 3)
main.setRowStretch(0, 1)
main.setRowStretch(1, 1)
def _configure_plot(self):
self.plot = self.plot_widget.addPlot()
self.plot.setTitle("左右法向力与最大切向力", color="#999999", size="13pt")
self.plot.titleLabel.setFont(QFont("SimHei", 13))
self.plot.showAxis("top", False)
self.plot.showAxis("right", False)
self.plot.showGrid(x=False, y=True, alpha=0.12)
self.plot.setLabel("bottom", "时间", units="s")
self.plot.setLabel("left", "力", units="N")
self.left_curve = self.plot.plot(pen=pg.mkPen(CURVE_COLOR, width=2), name="L")
self.right_curve = self.plot.plot(pen=pg.mkPen("#ffb000", width=2), name="R")
self.shear_curve = self.plot.plot(pen=pg.mkPen("#7ddc8a", width=2), name="Shear")
def start_control(self):
if self.control_thread is not None and self.control_thread.is_alive():
return
self.force_history.clear()
self.start_time = time.time()
self.start_button.setEnabled(False)
self.stop_button.setEnabled(True)
self.controller_finished = False
self.info_label.setText("启动中：正在连接夹爪与两个触觉传感器")
self.controller = build_controller(
self.argv,
status_callback=self._on_status,
include_visuals=True,
visual_size=320,
)
self.control_thread = threading.Thread(
target=self._run_controller,
daemon=True,
)
self.control_thread.start()
def stop_control(self):
if self.controller is not None:
self.controller.stop()
self.stop_button.setEnabled(False)
self.info_label.setText("停止中：正在释放相机与夹爪")
def _run_controller(self):
try:
self.controller.run()
except Exception as exc:
self._on_status({"state": "error", "action": str(exc), "timestamp": time.time()})
finally:
self.controller_finished = True
def _on_status(self, status):
with self.status_lock:
self.latest_status = status
def _refresh(self):
with self.status_lock:
status = self.latest_status
if not status:
self._update_thread_buttons()
return
self._update_thread_buttons()
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.action_chip.setText(f"action={action}")
self.left_force.set_value(
fmt(status.get("left_normal"), unit),
f"raw FPS {self._sample_fps(status, 'left_sample')}",
)
self.right_force.set_value(
fmt(status.get("right_normal"), unit),
f"raw FPS {self._sample_fps(status, 'right_sample')}",
)
self.shear_force.set_value(
fmt(status.get("max_shear"), shear_unit),
f"dShear {fmt(status.get('shear_change'), shear_unit)}",
)
self.force_diff.set_value(
fmt(status.get("normal_diff"), unit),
f"dN {fmt(status.get('normal_change'), unit)}",
)
self.force_pct_box.set_value(
str(status.get("force_pct", "--")),
f"hold_pos={status.get('hold_pos')}",
)
self.speed_box.set_value(
str(status.get("speed_pct", "--")),
"speed_pct",
)
self.trigger_box.set_value(
f"{int(bool(status.get('trigger')))} / {int(bool(status.get('trigger_armed')))}",
"trigger / armed",
)
self.contact_box.set_value(
str(int(bool(status.get("grip_contact")))),
self._contact_text(status),
)
left_sample = status.get("left_sample") or {}
right_sample = status.get("right_sample") or {}
self.left_flow.set_image(left_sample.get("flow_view"))
self.left_mag.set_image(left_sample.get("magnitude_view"))
self.right_flow.set_image(right_sample.get("flow_view"))
self.right_mag.set_image(right_sample.get("magnitude_view"))
elapsed = time.time() - self.start_time
self.force_history.append(
(
elapsed,
float(status.get("left_normal", 0.0) or 0.0),
float(status.get("right_normal", 0.0) or 0.0),
float(status.get("max_shear", 0.0) or 0.0),
)
)
if len(self.force_history) > PLOT_POINTS:
self.force_history = self.force_history[-PLOT_POINTS:]
self._update_plot()
self.info_label.setText(
" | ".join(
[
f"min={fmt(status.get('min_normal'), unit)}",
f"max={fmt(status.get('max_normal'), unit)}",
f"L_shear={fmt(status.get('left_shear'), shear_unit)}",
f"R_shear={fmt(status.get('right_shear'), shear_unit)}",
f"flow L/R={self._flow_text(left_sample)} / {self._flow_text(right_sample)}",
]
)
)
def _update_plot(self):
if not self.force_history:
return
data = np.asarray(self.force_history, dtype=float)
x = data[:, 0]
self.left_curve.setData(x, data[:, 1])
self.right_curve.setData(x, data[:, 2])
self.shear_curve.setData(x, data[:, 3])
if len(x) >= 2:
self.plot.setXRange(max(0.0, x[-1] - 6.0), max(6.0, x[-1]), padding=0)
def _update_thread_buttons(self):
if not self.controller_finished:
return
self.controller_finished = False
self.start_button.setEnabled(True)
self.stop_button.setEnabled(False)
if self.controller is not None:
self.info_label.setText("已停止")
def _sample_fps(self, status, key):
sample = status.get(key) or {}
fps = sample.get("fps")
if fps is None:
return "--"
return f"{fps:.1f}"
def _contact_text(self, status):
left = status.get("left_sample") or {}
right = status.get("right_sample") or {}
return f"L={int(bool(left.get('is_contact')))} R={int(bool(right.get('is_contact')))}"
def _flow_text(self, sample):
if not sample:
return "--"
return f"{sample.get('flow_mean', 0.0):.2f}/{sample.get('flow_max', 0.0):.2f}"
def keyPressEvent(self, event):
if event.key() == Qt.Key_Q:
self.close()
return
if event.key() == Qt.Key_Escape:
if self.isFullScreen():
self.showNormal()
else:
self.showFullScreen()
return
super().keyPressEvent(event)
def closeEvent(self, event):
self.timer.stop()
self.stop_control()
if self.control_thread is not None:
self.control_thread.join(timeout=3.0)
event.accept()
def main(argv=None):
parser = argparse.ArgumentParser()
parser.add_argument("--windowed", action="store_true")
ui_args, controller_argv = parser.parse_known_args(argv)
if _QT_IMPORT_ERROR is not None:
print(
"PyQt5 is required for the visualizer. "
"Install it in the active environment, for example: pip install PyQt5 pyqtgraph"
)
print(f"Import error: {_QT_IMPORT_ERROR}")
return 1
if pg is None:
print(
"pyqtgraph is required for the visualizer. "
"Install it in the active environment, for example: pip install pyqtgraph"
)
print(f"Import error: {_PYQTGRAPH_IMPORT_ERROR}")
return 1
QApplication.setAttribute(Qt.AA_EnableHighDpiScaling, True)
QApplication.setAttribute(Qt.AA_UseHighDpiPixmaps, True)
app = QApplication(sys.argv[:1])
window = GripperDemo02Viewer(controller_argv)
if ui_args.windowed:
window.resize(1500, 900)
window.show()
else:
window.showFullScreen()
window.start_control()
return app.exec_()
if __name__ == "__main__":
mp.freeze_support()
raise SystemExit(main(sys.argv[1:]))