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 10:39:17 +08:00
parent 66a00a8b5b
commit a2c820bce5
14 changed files with 239 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
gripper_control_02/README.md
+28 -10
View File
@@ -9,12 +9,13 @@
3. 展示时，把物体从某个侧边扫一下，触发单侧力变化。
4. 触发后夹爪开始闭合，你把物体移动到中间。
5. 闭合过程中检测到物体后，切到低力并保持当前位置。
6. 像 01 一样进入 `gripping`，慢慢加力到 `HOLD_FORCE`。
7. 进入 `hold_check` 后记录夹持参考力。
8. 后续法向力或切向力变化超过阈值，夹爪直接张开到 `OPEN_POS`。
9. 张开后进入 `open_recover`，保持张开并等待恢复条件。
10. 夹爪回到 `OPEN_POS` 初始点位，或法向/切向读数都低于恢复阈值并稳定后，回到 `open_wait`。
11. 回到 `open_wait` 后，程序必须先看到触发力清掉并稳定，才会重新武装；下一次扫过才允许闭合。
6. 像 01 一样进入 `gripping`，先慢慢加力到基础 `HOLD_FORCE`。
7. 进入 `hold_check` 后，根据切向力估计物体负载；切向力越大，目标夹紧力越高，并按小步进继续补力。
8. 自适应补力停止并稳定 `RELEASE_ARM_DELAY_SECONDS` 后，重新记录释放参考力，才开始检测人取物松开。
9. 后续法向力或切向力变化超过阈值，夹爪直接张开到 `OPEN_POS`。
10. 张开后进入 `open_recover`，保持张开并等待恢复条件。
11. 夹爪回到 `OPEN_POS` 初始点位，或法向/切向读数都低于恢复阈值并稳定后，回到 `open_wait`。
12. 回到 `open_wait` 后，如果是因为夹爪已回到 `OPEN_POS`，会直接重新武装；程序只响应下一次新的触发上升沿，残余力保持不变时不会自动闭合。
## 运行
@@ -67,6 +68,14 @@ gripper_control_02\config\gripper_demo_02.py
- `FORCE_RAMP_INTERVAL`: 慢慢加力时两次加力的间隔。
- `HOLD_FORCE`: 慢慢加力到这个值后进入 `hold_check`。
- `GRIP_SETTLE_SECONDS`: 夹住后等待稳定多久再记录参考力。
- `ADAPTIVE_GRIP_ENABLED`: 是否启用按切向力估计负载并自适应补夹紧力。
- `ADAPTIVE_SHEAR_START_FORCE`: 切向力低于这个值时，认为是轻物体，只使用最小目标力。
- `ADAPTIVE_SHEAR_FULL_FORCE`: 切向力达到这个值时，认为负载较大，目标力加到最大目标力。
- `ADAPTIVE_FORCE_MIN`: 自适应夹持的最小目标力。
- `ADAPTIVE_FORCE_MAX`: 自适应夹持的最大目标力；重物夹不住优先加这个，但不要超过 `FORCE_MAX`。
- `ADAPTIVE_FORCE_STEP`: 自适应补力时每次增加多少 `force_pct`。
- `ADAPTIVE_FORCE_INTERVAL`: 自适应补力两次之间的间隔，越大越柔和。
- `RELEASE_ARM_DELAY_SECONDS`: 自适应补力停止后，等待多久再启用“人取物松开”检测。
- `RELEASE_NORMAL_CHANGE`: 夹住后法向力变化超过它就松开。
- `RELEASE_SHEAR_CHANGE`: 夹住后切向力变化超过它就松开。
- `RELEASE_OPEN_SPEED`: 检测到松开后，张开到 `OPEN_POS` 使用的速度。
@@ -87,13 +96,15 @@ gripper_control_02\config\gripper_demo_02.py
- `maxN`: 左右法向力最大值，单位 N。
- `shear`: 左右切向力最大值，单位 N。
- `trigger`: 是否满足单侧触发条件。
- `armed`: 是否已经重新武装；只有 `trigger=1 armed=1` 才会闭合。
- `armed`: 是否已经重新武装；只有 `trigger=1 armed=1` 且是新的一次触发上升沿才会闭合。
- `grip_contact`: 是否满足夹住接触条件。
- `dN`: 夹住后当前法向力相对参考值的最大变化，单位 N。
- `dShear`: 夹住后当前切向力相对参考值的变化，单位 N。
- `state`: 当前状态。
- `hold_pos`: 夹住后保持的位置。
- `force_pct`: 当前夹爪目标力百分比。
- `target`: 当前根据切向力计算出来的自适应目标力。
- `release_armed`: 是否已经启用释放检测；`0` 表示还在补力/等待稳定，`1` 表示法向或切向变化会触发张开。
- `action`: 当前动作。
## 可视化展示
@@ -102,7 +113,9 @@ gripper_control_02\config\gripper_demo_02.py
- 左右两边的光流幅值图。
- 左右法向力、最大切向力、左右差值。
- 当前 `state`、`action`、`trigger/armed`、`grip_contact`。
- 当前 `force_pct`、速度 `speed_pct`、`hold_pos`。
- 当前 `force_pct`、自适应目标力、速度 `speed_pct`、`hold_pos`。
- 当前 `release_armed`，用于判断系统是在补夹紧力，还是已经开始检测人取物松开。
- 折线图带图例，默认固定 y 轴为 `0~3 N`，范围在 `visualizer.py` 顶部的 `PLOT_FORCE_Y_MIN_N / PLOT_FORCE_Y_MAX_N` 调整。
- 左右传感器 FPS、接触标志、光流均值/最大值。
## 状态说明
@@ -110,17 +123,22 @@ gripper_control_02\config\gripper_demo_02.py
- `open_wait`: 夹爪张开，等待任意单侧触发。
- `closing`: 单侧触发后正在闭合，等待检测到物体。
- `gripping`: 检测到物体后低力保持当前位置，并慢慢加力。
- `hold_check`: 已夹住并记录参考力，监测法向/切向变化；变化超过阈值会直接张开。
- `hold_check`: 已夹住；先根据切向力自适应补夹紧力，补力稳定后再监测法向/切向变化；变化超过阈值会直接张开。
- `open_recover`: 已经发出张开命令，正在等待回到初始点位或等待法向/切向残余力清空；这个状态不会闭合夹爪。
## 常见动作说明
- `force-change-release-open`: 检测到夹住后的法向或切向变化，已经发出张开命令。
- `adaptive-grip-ramp`: 切向力显示负载较大，正在按小步进增加夹紧力。
- `adaptive-grip-wait`: 已经需要更高目标力，但还没到下一次加力时间。
- `release-arm-delay`: 补力刚停止，正在等待力稳定，暂时不触发松开。
- `release-reference-armed`: 已重新记录释放参考力，后续法向/切向变化会触发张开。
- `open-recover-position`: 夹爪当前位置已经接近 `OPEN_POS`，正在等待到位稳定。
- `open-ready-position`: 夹爪已经回到初始点位，重新进入等待下一次扫过触发。
- `open-ready-position`: 夹爪已经回到初始点位，重新进入等待下一次扫过触发；此时会直接武装，但残余触发保持不变不会自动闭合。
- `open-wait-clear-trigger`: 已经回到等待状态，但当前力还超过触发阈值，暂时不会闭合。
- `open-wait-trigger-clearing`: 触发力已经低于阈值，正在等待清零稳定时间。
- `open-wait-armed`: 触发已经清掉并稳定，下一次扫过可以闭合。
- `open-wait-trigger-held`: 已经武装，但当前触发不是新上升沿，等待力先降下去再下一次扫过。
- `open-recover-wait-force-clear`: 夹爪保持张开，但传感器还有残余力，不允许重新闭合。
- `open-recover-stabilizing`: 力已经低于恢复阈值，正在等待稳定时间。
- `open-ready`: 力已经清空并稳定，重新进入等待下一次扫过触发。
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__/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__/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
+21 -2
View File
@@ -52,9 +52,9 @@ RELEASE_OPEN_DECEL = 120 # release 张开的减速度。
OPEN_FORCE = 15 # 张开时使用的力。
CLOSE_FORCE = 15 # 单侧触发后闭合时使用的小力。
GRIP_START_FORCE = 10 # 闭合检测到物体后，先切到这个低力保持。
HOLD_FORCE = 15 # 慢慢加力到这个值后进入 hold_check。
HOLD_FORCE = 15 # 基础夹持力；轻物体默认慢慢加到这个力。
FORCE_MIN = 10
FORCE_MAX = 30
FORCE_MAX = 60 # 允许自适应加力的最大总上限；重物夹不住时可以适当加大。
# 控制循环频率。越高检测释放越快。
@@ -86,6 +86,17 @@ FORCE_RAMP_STEP = 1
FORCE_RAMP_INTERVAL = 0.1
GRIP_SETTLE_SECONDS = 0.1 # 加到 HOLD_FORCE 后再稳定这么久，进入 hold_check。
# 根据切向力估计物体负载，并慢慢补夹紧力。
# 逻辑：切向力越大，说明物体越重或越容易下滑，目标夹紧力越高。
ADAPTIVE_GRIP_ENABLED = True # 是否启用切向力自适应加力。
ADAPTIVE_SHEAR_START_FORCE = 0.10 # 切向力低于这个值时，认为是轻物体，只用 ADAPTIVE_FORCE_MIN。
ADAPTIVE_SHEAR_FULL_FORCE = 0.60 # 切向力达到这个值时，认为负载较大，目标力加到 ADAPTIVE_FORCE_MAX。
ADAPTIVE_FORCE_MIN = 15 # 自适应夹持的最小目标力，通常等于 HOLD_FORCE。
ADAPTIVE_FORCE_MAX = 45 # 自适应夹持的最大目标力；重物夹不住可以加大，但不要超过 FORCE_MAX。
ADAPTIVE_FORCE_STEP = 1 # 自适应加力每次增加多少 force_pct。
ADAPTIVE_FORCE_INTERVAL = 0.15 # 自适应加力两次之间的间隔，单位秒；越大加力越柔和。
RELEASE_ARM_DELAY_SECONDS = 0.8 # 自适应加力停止后，等待这么久再启用“人取物松开”检测。
# 松开触发阈值。夹住后，法向或切向变化超过其中任意一个，就张开。
RELEASE_NORMAL_CHANGE = 0.2 # 左/右法向力相对夹住参考值的变化阈值，单位 N。
RELEASE_SHEAR_CHANGE = 0.2 # 左/右切向力相对夹住参考值的变化阈值，单位 N。
@@ -195,6 +206,14 @@ CONFIG = {
"force_ramp_step": FORCE_RAMP_STEP,
"force_ramp_interval": FORCE_RAMP_INTERVAL,
"grip_settle_seconds": GRIP_SETTLE_SECONDS,
"adaptive_grip_enabled": ADAPTIVE_GRIP_ENABLED,
"adaptive_shear_start_force": ADAPTIVE_SHEAR_START_FORCE,
"adaptive_shear_full_force": ADAPTIVE_SHEAR_FULL_FORCE,
"adaptive_force_min": ADAPTIVE_FORCE_MIN,
"adaptive_force_max": ADAPTIVE_FORCE_MAX,
"adaptive_force_step": ADAPTIVE_FORCE_STEP,
"adaptive_force_interval": ADAPTIVE_FORCE_INTERVAL,
"release_arm_delay_seconds": RELEASE_ARM_DELAY_SECONDS,
"release_normal_change": RELEASE_NORMAL_CHANGE,
"release_shear_change": RELEASE_SHEAR_CHANGE,
"rearm_seconds": REARM_SECONDS,
gripper_control_02/controller.py
+138 -28
View File
@@ -73,6 +73,16 @@ 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(
"Adaptive grip: "
f"enabled={int(self.config.adaptive_grip_enabled)}, "
f"shear_start={self.config.adaptive_shear_start_force:.3f}{self.shear_unit}, "
f"shear_full={self.config.adaptive_shear_full_force:.3f}{self.shear_unit}, "
f"force={self.config.adaptive_force_min}-{self.config.adaptive_force_max}, "
f"step={self.config.adaptive_force_step}, "
f"interval={self.config.adaptive_force_interval:.2f}s, "
f"release_arm_delay={self.config.release_arm_delay_seconds:.2f}s"
)
print(
"Release open: "
f"speed={self.config.release_open_speed}, "
@@ -102,7 +112,10 @@ class SideTriggerGripController:
recover_position_since = None
trigger_clear_since = None
trigger_armed = False
last_trigger = False
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
@@ -119,6 +132,9 @@ class SideTriggerGripController:
"state": state,
"action": "waiting-for-sensor-samples",
"force_pct": force_pct,
"adaptive_target_force": adaptive_target_force,
"adaptive_grip_enabled": config.adaptive_grip_enabled,
"release_armed": release_armed,
"speed_pct": config.speed,
"normal_unit": self.normal_unit,
"shear_unit": self.shear_unit,
@@ -134,6 +150,7 @@ class SideTriggerGripController:
action = "wait"
if state == "open_wait":
trigger_edge = trigger and not last_trigger
if not trigger:
if trigger_clear_since is None:
trigger_clear_since = now
@@ -146,6 +163,8 @@ class SideTriggerGripController:
elif not trigger_armed:
trigger_clear_since = None
action = "open-wait-clear-trigger"
elif not trigger_edge:
action = "open-wait-trigger-held"
else:
force_pct = int(clamp(
config.close_force,
@@ -163,6 +182,8 @@ class SideTriggerGripController:
recover_position_since = None
trigger_clear_since = None
trigger_armed = False
adaptive_target_force = self._adaptive_force_target(feedback)
release_armed = False
hold_pos = None
reference = None
action = "side-trigger-close"
@@ -179,6 +200,8 @@ class SideTriggerGripController:
state = "gripping"
state_start = now
last_force_ramp_time = now
adaptive_target_force = self._adaptive_force_target(feedback)
release_armed = False
reference = None
action = "object-detected-low-force"
elif now - state_start >= config.close_timeout_seconds:
@@ -196,6 +219,7 @@ class SideTriggerGripController:
last_open_position_ready = False
recover_quiet_since = None
recover_position_since = None
release_armed = False
hold_pos = None
reference = None
action = "close-timeout-open"
@@ -222,42 +246,81 @@ class SideTriggerGripController:
reference = self._make_reference(feedback)
state = "hold_check"
state_start = now
adaptive_target_force = self._adaptive_force_target(feedback)
release_armed = False
action = "hold-reference-armed"
else:
action = "grip-hold"
elif state == "hold_check":
normal_change, shear_change = self._release_changes(feedback, reference)
should_release = (
normal_change >= config.release_normal_change
or shear_change >= config.release_shear_change
adaptive_target_force = self._adaptive_force_target(feedback)
should_adapt_force = (
config.adaptive_grip_enabled
and not release_armed
and force_pct < adaptive_target_force
)
if should_release:
if (
should_adapt_force
and now - last_force_ramp_time >= config.adaptive_force_interval
):
force_pct = int(clamp(
config.open_force,
force_pct + config.adaptive_force_step,
config.force_min,
config.force_max,
adaptive_target_force,
))
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"
self.gripper.set_force(force_pct)
last_force_ramp_time = now
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"
reference = self._make_reference(feedback)
release_armed = False
action = "adaptive-grip-ramp"
elif should_adapt_force:
release_armed = False
action = "adaptive-grip-wait"
elif (
not release_armed
and now - max(state_start, last_force_ramp_time)
< config.release_arm_delay_seconds
):
action = "release-arm-delay"
elif not release_armed:
reference = self._make_reference(feedback)
release_armed = True
action = "release-reference-armed"
else:
action = "hold-check"
normal_change, shear_change = self._release_changes(feedback, reference)
should_release = (
normal_change >= config.release_normal_change
or shear_change >= config.release_shear_change
)
if should_release:
force_pct = int(clamp(
config.open_force,
config.force_min,
config.force_max,
))
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
release_armed = False
hold_pos = None
reference = None
action = "force-change-release-open"
else:
action = "hold-check"
elif state == "open_recover":
current_pos = last_open_position
@@ -287,7 +350,9 @@ class SideTriggerGripController:
recover_quiet_since = None
recover_position_since = None
trigger_clear_since = None
trigger_armed = False
trigger_armed = True
last_trigger = trigger
release_armed = False
hold_pos = None
reference = None
action = f"open-ready-position pos={current_pos}"
@@ -307,7 +372,9 @@ class SideTriggerGripController:
recover_quiet_since = None
recover_position_since = None
trigger_clear_since = None
trigger_armed = False
trigger_armed = True
last_trigger = trigger
release_armed = False
hold_pos = None
reference = None
action = "open-ready"
@@ -327,6 +394,8 @@ class SideTriggerGripController:
shear_change=shear_change,
hold_pos=hold_pos,
force_pct=force_pct,
adaptive_target_force=adaptive_target_force,
release_armed=release_armed,
action=action,
trigger_armed=trigger_armed,
)
@@ -340,10 +409,13 @@ class SideTriggerGripController:
shear_change=shear_change,
hold_pos=hold_pos,
force_pct=force_pct,
adaptive_target_force=adaptive_target_force,
release_armed=release_armed,
action=action,
trigger_armed=trigger_armed,
)
)
last_trigger = trigger
sleep_time = interval - (time.perf_counter() - tick_start)
if sleep_time > 0:
@@ -378,6 +450,36 @@ class SideTriggerGripController:
<= int(self.config.open_position_tolerance)
)
def _adaptive_force_target(self, feedback):
config = self.config
if not config.adaptive_grip_enabled:
return int(clamp(config.hold_force, config.force_min, config.force_max))
force_min = int(clamp(
config.adaptive_force_min,
config.force_min,
config.force_max,
))
force_max = int(clamp(
config.adaptive_force_max,
config.force_min,
config.force_max,
))
if force_max < force_min:
force_max = force_min
shear_start = max(0.0, float(config.adaptive_shear_start_force))
shear_full = max(shear_start + 1e-6, float(config.adaptive_shear_full_force))
shear = max(0.0, float(feedback.max_shear))
if shear <= shear_start:
return force_min
if shear >= shear_full:
return force_max
ratio = (shear - shear_start) / (shear_full - shear_start)
target = round(force_min + ratio * (force_max - force_min))
return int(clamp(target, force_min, force_max))
def _make_reference(self, feedback):
return {
"left_normal": feedback.left_normal,
@@ -412,6 +514,8 @@ class SideTriggerGripController:
shear_change,
hold_pos,
force_pct,
adaptive_target_force,
release_armed,
action,
trigger_armed,
):
@@ -425,7 +529,8 @@ class SideTriggerGripController:
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}"
f"force_pct={force_pct:3d} target={adaptive_target_force:3d} "
f"release_armed={int(release_armed)} action={action}"
)
def _make_status(
@@ -438,6 +543,8 @@ class SideTriggerGripController:
shear_change,
hold_pos,
force_pct,
adaptive_target_force,
release_armed,
action,
trigger_armed,
):
@@ -474,6 +581,9 @@ class SideTriggerGripController:
"shear_change": shear_change,
"hold_pos": hold_pos,
"force_pct": force_pct,
"adaptive_target_force": adaptive_target_force,
"adaptive_grip_enabled": self.config.adaptive_grip_enabled,
"release_armed": bool(release_armed),
"speed_pct": speed_pct,
"normal_unit": self.normal_unit,
"shear_unit": self.shear_unit,
gripper_control_02/settings.py
+18 -1
View File
@@ -53,7 +53,7 @@ class Demo02Config:
hold_force: int = 15
initial_force: int = 10
force_min: int = 10
force_max: int = 30
force_max: int = 60
control_hz: float = 30.0
baseline_seconds: float = 1.0
@@ -69,6 +69,14 @@ class Demo02Config:
force_ramp_step: int = 1
force_ramp_interval: float = 0.1
grip_settle_seconds: float = 0.1
adaptive_grip_enabled: bool = True
adaptive_shear_start_force: float = 0.10
adaptive_shear_full_force: float = 0.60
adaptive_force_min: int = 15
adaptive_force_max: int = 45
adaptive_force_step: int = 1
adaptive_force_interval: float = 0.15
release_arm_delay_seconds: float = 0.8
release_normal_change: float = 0.03
release_shear_change: float = 0.03
rearm_seconds: float = 0.5
@@ -150,6 +158,15 @@ class Demo02Config:
add("--force-ramp-step", type=int, default=get("force_ramp_step", cls.force_ramp_step))
add("--force-ramp-interval", type=float, default=get("force_ramp_interval", cls.force_ramp_interval))
add("--grip-settle-seconds", type=float, default=get("grip_settle_seconds", cls.grip_settle_seconds))
add("--enable-adaptive-grip", dest="adaptive_grip_enabled", action="store_true", default=get("adaptive_grip_enabled", cls.adaptive_grip_enabled))
add("--disable-adaptive-grip", dest="adaptive_grip_enabled", action="store_false")
add("--adaptive-shear-start-force", type=float, default=get("adaptive_shear_start_force", cls.adaptive_shear_start_force))
add("--adaptive-shear-full-force", type=float, default=get("adaptive_shear_full_force", cls.adaptive_shear_full_force))
add("--adaptive-force-min", type=int, default=get("adaptive_force_min", cls.adaptive_force_min))
add("--adaptive-force-max", type=int, default=get("adaptive_force_max", cls.adaptive_force_max))
add("--adaptive-force-step", type=int, default=get("adaptive_force_step", cls.adaptive_force_step))
add("--adaptive-force-interval", type=float, default=get("adaptive_force_interval", cls.adaptive_force_interval))
add("--release-arm-delay-seconds", type=float, default=get("release_arm_delay_seconds", cls.release_arm_delay_seconds))
add("--release-normal-change", type=float, default=get("release_normal_change", cls.release_normal_change))
add("--release-shear-change", type=float, default=get("release_shear_change", cls.release_shear_change))
add("--rearm-seconds", type=float, default=get("rearm_seconds", cls.rearm_seconds))
gripper_control_02/visualizer.py
+34 -4
View File
@@ -58,6 +58,8 @@ from .main import build_controller
PLOT_POINTS = 180
PLOT_FORCE_Y_MIN_N = 0.0
PLOT_FORCE_Y_MAX_N = 3.0
CURVE_COLOR = "#00E5FF"
@@ -250,7 +252,7 @@ class GripperDemo02Viewer(QMainWindow):
top = QHBoxLayout()
top.setSpacing(8)
title = QLabel("Gripper Demo 02")
title = QLabel("Orisys 夹爪")
title.setObjectName("Title")
self.status_chip = QLabel("state=--")
self.status_chip.setObjectName("StatusChip")
@@ -295,18 +297,22 @@ class GripperDemo02Viewer(QMainWindow):
self.shear_force = MetricBox("最大切向力", "--")
self.force_diff = MetricBox("左右差值", "--")
self.force_pct_box = MetricBox("force_pct", "--")
self.target_force_box = MetricBox("目标力", "--")
self.speed_box = MetricBox("速度", "--")
self.trigger_box = MetricBox("触发/武装", "--")
self.contact_box = MetricBox("接触", "--")
self.release_box = MetricBox("释放检测", "--")
boxes = [
self.left_force,
self.right_force,
self.shear_force,
self.force_diff,
self.force_pct_box,
self.target_force_box,
self.speed_box,
self.trigger_box,
self.contact_box,
self.release_box,
]
for idx, box in enumerate(boxes):
metrics_grid.addWidget(box, idx // 2, idx % 2)
@@ -343,9 +349,22 @@ class GripperDemo02Viewer(QMainWindow):
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")
self.plot.setYRange(PLOT_FORCE_Y_MIN_N, PLOT_FORCE_Y_MAX_N, padding=0)
self.plot.setLimits(yMin=PLOT_FORCE_Y_MIN_N, yMax=PLOT_FORCE_Y_MAX_N)
self.plot.enableAutoRange(axis=pg.ViewBox.YAxis, enable=False)
self.plot.addLegend(offset=(-12, 12), labelTextColor="#d0d0d0")
self.left_curve = self.plot.plot(
pen=pg.mkPen(CURVE_COLOR, width=2),
name="左法向 (N)",
)
self.right_curve = self.plot.plot(
pen=pg.mkPen("#ffb000", width=2),
name="右法向 (N)",
)
self.shear_curve = self.plot.plot(
pen=pg.mkPen("#7ddc8a", width=2),
name="最大切向 (N)",
)
def start_control(self):
if self.control_thread is not None and self.control_thread.is_alive():
@@ -422,6 +441,10 @@ class GripperDemo02Viewer(QMainWindow):
str(status.get("force_pct", "--")),
f"hold_pos={status.get('hold_pos')}",
)
self.target_force_box.set_value(
str(status.get("adaptive_target_force", "--")),
f"adaptive={int(bool(status.get('adaptive_grip_enabled')))}",
)
self.speed_box.set_value(
str(status.get("speed_pct", "--")),
"speed_pct",
@@ -434,6 +457,10 @@ class GripperDemo02Viewer(QMainWindow):
str(int(bool(status.get("grip_contact")))),
self._contact_text(status),
)
self.release_box.set_value(
str(int(bool(status.get("release_armed")))),
"release_armed",
)
left_sample = status.get("left_sample") or {}
right_sample = status.get("right_sample") or {}
@@ -462,6 +489,8 @@ class GripperDemo02Viewer(QMainWindow):
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"target={status.get('adaptive_target_force', '--')}",
f"release_armed={int(bool(status.get('release_armed')))}",
f"flow L/R={self._flow_text(left_sample)} / {self._flow_text(right_sample)}",
]
)
@@ -475,6 +504,7 @@ class GripperDemo02Viewer(QMainWindow):
self.left_curve.setData(x, data[:, 1])
self.right_curve.setData(x, data[:, 2])
self.shear_curve.setData(x, data[:, 3])
self.plot.setYRange(PLOT_FORCE_Y_MIN_N, PLOT_FORCE_Y_MAX_N, padding=0)
if len(x) >= 2:
self.plot.setXRange(max(0.0, x[-1] - 6.0), max(6.0, x[-1]), padding=0)