qt折线图修改

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
Binary file not shown.

Before

Width:  |  Height:  |  Size: 2.3 MiB

After

Width:  |  Height:  |  Size: 2.3 MiB

+28 -10
View File
@@ -9,12 +9,13 @@
3. 展示时,把物体从某个侧边扫一下,触发单侧力变化。 3. 展示时,把物体从某个侧边扫一下,触发单侧力变化。
4. 触发后夹爪开始闭合,你把物体移动到中间。 4. 触发后夹爪开始闭合,你把物体移动到中间。
5. 闭合过程中检测到物体后,切到低力并保持当前位置。 5. 闭合过程中检测到物体后,切到低力并保持当前位置。
6. 像 01 一样进入 `gripping`,慢慢加力到 `HOLD_FORCE` 6. 像 01 一样进入 `gripping`慢慢加力到基础 `HOLD_FORCE`
7. 进入 `hold_check`记录夹持参考力。 7. 进入 `hold_check`,根据切向力估计物体负载;切向力越大,目标夹紧力越高,并按小步进继续补力。
8. 后续法向力或切向力变化超过阈值,夹爪直接张开到 `OPEN_POS` 8. 自适应补力停止并稳定 `RELEASE_ARM_DELAY_SECONDS` 后,重新记录释放参考力,才开始检测人取物松开
9. 张开后进入 `open_recover`,保持张开并等待恢复条件 9. 后续法向力或切向力变化超过阈值,夹爪直接张开到 `OPEN_POS`
10. 夹爪回到 `OPEN_POS` 初始点位,或法向/切向读数都低于恢复阈值并稳定后,回到 `open_wait` 10. 张开后进入 `open_recover`,保持张开并等待恢复条件
11. 回到 `open_wait` 后,程序必须先看到触发力清掉并稳定,才会重新武装;下一次扫过才允许闭合 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`: 慢慢加力时两次加力的间隔。 - `FORCE_RAMP_INTERVAL`: 慢慢加力时两次加力的间隔。
- `HOLD_FORCE`: 慢慢加力到这个值后进入 `hold_check` - `HOLD_FORCE`: 慢慢加力到这个值后进入 `hold_check`
- `GRIP_SETTLE_SECONDS`: 夹住后等待稳定多久再记录参考力。 - `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_NORMAL_CHANGE`: 夹住后法向力变化超过它就松开。
- `RELEASE_SHEAR_CHANGE`: 夹住后切向力变化超过它就松开。 - `RELEASE_SHEAR_CHANGE`: 夹住后切向力变化超过它就松开。
- `RELEASE_OPEN_SPEED`: 检测到松开后,张开到 `OPEN_POS` 使用的速度。 - `RELEASE_OPEN_SPEED`: 检测到松开后,张开到 `OPEN_POS` 使用的速度。
@@ -87,13 +96,15 @@ gripper_control_02\config\gripper_demo_02.py
- `maxN`: 左右法向力最大值,单位 N。 - `maxN`: 左右法向力最大值,单位 N。
- `shear`: 左右切向力最大值,单位 N。 - `shear`: 左右切向力最大值,单位 N。
- `trigger`: 是否满足单侧触发条件。 - `trigger`: 是否满足单侧触发条件。
- `armed`: 是否已经重新武装;只有 `trigger=1 armed=1` 才会闭合。 - `armed`: 是否已经重新武装;只有 `trigger=1 armed=1` 且是新的一次触发上升沿才会闭合。
- `grip_contact`: 是否满足夹住接触条件。 - `grip_contact`: 是否满足夹住接触条件。
- `dN`: 夹住后当前法向力相对参考值的最大变化,单位 N。 - `dN`: 夹住后当前法向力相对参考值的最大变化,单位 N。
- `dShear`: 夹住后当前切向力相对参考值的变化,单位 N。 - `dShear`: 夹住后当前切向力相对参考值的变化,单位 N。
- `state`: 当前状态。 - `state`: 当前状态。
- `hold_pos`: 夹住后保持的位置。 - `hold_pos`: 夹住后保持的位置。
- `force_pct`: 当前夹爪目标力百分比。 - `force_pct`: 当前夹爪目标力百分比。
- `target`: 当前根据切向力计算出来的自适应目标力。
- `release_armed`: 是否已经启用释放检测;`0` 表示还在补力/等待稳定,`1` 表示法向或切向变化会触发张开。
- `action`: 当前动作。 - `action`: 当前动作。
## 可视化展示 ## 可视化展示
@@ -102,7 +113,9 @@ gripper_control_02\config\gripper_demo_02.py
- 左右两边的光流幅值图。 - 左右两边的光流幅值图。
- 左右法向力、最大切向力、左右差值。 - 左右法向力、最大切向力、左右差值。
- 当前 `state``action``trigger/armed``grip_contact` - 当前 `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、接触标志、光流均值/最大值。 - 左右传感器 FPS、接触标志、光流均值/最大值。
## 状态说明 ## 状态说明
@@ -110,17 +123,22 @@ gripper_control_02\config\gripper_demo_02.py
- `open_wait`: 夹爪张开,等待任意单侧触发。 - `open_wait`: 夹爪张开,等待任意单侧触发。
- `closing`: 单侧触发后正在闭合,等待检测到物体。 - `closing`: 单侧触发后正在闭合,等待检测到物体。
- `gripping`: 检测到物体后低力保持当前位置,并慢慢加力。 - `gripping`: 检测到物体后低力保持当前位置,并慢慢加力。
- `hold_check`: 已夹住并记录参考力,监测法向/切向变化;变化超过阈值会直接张开。 - `hold_check`: 已夹住;先根据切向力自适应补夹紧力,补力稳定后再监测法向/切向变化;变化超过阈值会直接张开。
- `open_recover`: 已经发出张开命令,正在等待回到初始点位或等待法向/切向残余力清空;这个状态不会闭合夹爪。 - `open_recover`: 已经发出张开命令,正在等待回到初始点位或等待法向/切向残余力清空;这个状态不会闭合夹爪。
## 常见动作说明 ## 常见动作说明
- `force-change-release-open`: 检测到夹住后的法向或切向变化,已经发出张开命令。 - `force-change-release-open`: 检测到夹住后的法向或切向变化,已经发出张开命令。
- `adaptive-grip-ramp`: 切向力显示负载较大,正在按小步进增加夹紧力。
- `adaptive-grip-wait`: 已经需要更高目标力,但还没到下一次加力时间。
- `release-arm-delay`: 补力刚停止,正在等待力稳定,暂时不触发松开。
- `release-reference-armed`: 已重新记录释放参考力,后续法向/切向变化会触发张开。
- `open-recover-position`: 夹爪当前位置已经接近 `OPEN_POS`,正在等待到位稳定。 - `open-recover-position`: 夹爪当前位置已经接近 `OPEN_POS`,正在等待到位稳定。
- `open-ready-position`: 夹爪已经回到初始点位,重新进入等待下一次扫过触发。 - `open-ready-position`: 夹爪已经回到初始点位,重新进入等待下一次扫过触发;此时会直接武装,但残余触发保持不变不会自动闭合
- `open-wait-clear-trigger`: 已经回到等待状态,但当前力还超过触发阈值,暂时不会闭合。 - `open-wait-clear-trigger`: 已经回到等待状态,但当前力还超过触发阈值,暂时不会闭合。
- `open-wait-trigger-clearing`: 触发力已经低于阈值,正在等待清零稳定时间。 - `open-wait-trigger-clearing`: 触发力已经低于阈值,正在等待清零稳定时间。
- `open-wait-armed`: 触发已经清掉并稳定,下一次扫过可以闭合。 - `open-wait-armed`: 触发已经清掉并稳定,下一次扫过可以闭合。
- `open-wait-trigger-held`: 已经武装,但当前触发不是新上升沿,等待力先降下去再下一次扫过。
- `open-recover-wait-force-clear`: 夹爪保持张开,但传感器还有残余力,不允许重新闭合。 - `open-recover-wait-force-clear`: 夹爪保持张开,但传感器还有残余力,不允许重新闭合。
- `open-recover-stabilizing`: 力已经低于恢复阈值,正在等待稳定时间。 - `open-recover-stabilizing`: 力已经低于恢复阈值,正在等待稳定时间。
- `open-ready`: 力已经清空并稳定,重新进入等待下一次扫过触发。 - `open-ready`: 力已经清空并稳定,重新进入等待下一次扫过触发。
+21 -2
View File
@@ -52,9 +52,9 @@ RELEASE_OPEN_DECEL = 120 # release 张开的减速度。
OPEN_FORCE = 15 # 张开时使用的力。 OPEN_FORCE = 15 # 张开时使用的力。
CLOSE_FORCE = 15 # 单侧触发后闭合时使用的小力。 CLOSE_FORCE = 15 # 单侧触发后闭合时使用的小力。
GRIP_START_FORCE = 10 # 闭合检测到物体后,先切到这个低力保持。 GRIP_START_FORCE = 10 # 闭合检测到物体后,先切到这个低力保持。
HOLD_FORCE = 15 # 慢慢加到这个值后进入 hold_check HOLD_FORCE = 15 # 基础夹持力;轻物体默认慢慢加到这个
FORCE_MIN = 10 FORCE_MIN = 10
FORCE_MAX = 30 FORCE_MAX = 60 # 允许自适应加力的最大总上限;重物夹不住时可以适当加大。
# 控制循环频率。越高检测释放越快。 # 控制循环频率。越高检测释放越快。
@@ -86,6 +86,17 @@ FORCE_RAMP_STEP = 1
FORCE_RAMP_INTERVAL = 0.1 FORCE_RAMP_INTERVAL = 0.1
GRIP_SETTLE_SECONDS = 0.1 # 加到 HOLD_FORCE 后再稳定这么久,进入 hold_check。 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_NORMAL_CHANGE = 0.2 # 左/右法向力相对夹住参考值的变化阈值,单位 N。
RELEASE_SHEAR_CHANGE = 0.2 # 左/右切向力相对夹住参考值的变化阈值,单位 N。 RELEASE_SHEAR_CHANGE = 0.2 # 左/右切向力相对夹住参考值的变化阈值,单位 N。
@@ -195,6 +206,14 @@ CONFIG = {
"force_ramp_step": FORCE_RAMP_STEP, "force_ramp_step": FORCE_RAMP_STEP,
"force_ramp_interval": FORCE_RAMP_INTERVAL, "force_ramp_interval": FORCE_RAMP_INTERVAL,
"grip_settle_seconds": GRIP_SETTLE_SECONDS, "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_normal_change": RELEASE_NORMAL_CHANGE,
"release_shear_change": RELEASE_SHEAR_CHANGE, "release_shear_change": RELEASE_SHEAR_CHANGE,
"rearm_seconds": REARM_SECONDS, "rearm_seconds": REARM_SECONDS,
+113 -3
View File
@@ -73,6 +73,16 @@ class SideTriggerGripController:
f"release_normal_change={self.config.release_normal_change:.3f}{self.normal_unit}, " 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}" 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( print(
"Release open: " "Release open: "
f"speed={self.config.release_open_speed}, " f"speed={self.config.release_open_speed}, "
@@ -102,7 +112,10 @@ class SideTriggerGripController:
recover_position_since = None recover_position_since = None
trigger_clear_since = None trigger_clear_since = None
trigger_armed = False trigger_armed = False
last_trigger = False
force_pct = int(clamp(config.open_force, config.force_min, config.force_max)) 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 hold_pos = None
reference = None reference = None
@@ -119,6 +132,9 @@ class SideTriggerGripController:
"state": state, "state": state,
"action": "waiting-for-sensor-samples", "action": "waiting-for-sensor-samples",
"force_pct": force_pct, "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, "speed_pct": config.speed,
"normal_unit": self.normal_unit, "normal_unit": self.normal_unit,
"shear_unit": self.shear_unit, "shear_unit": self.shear_unit,
@@ -134,6 +150,7 @@ class SideTriggerGripController:
action = "wait" action = "wait"
if state == "open_wait": if state == "open_wait":
trigger_edge = trigger and not last_trigger
if not trigger: if not trigger:
if trigger_clear_since is None: if trigger_clear_since is None:
trigger_clear_since = now trigger_clear_since = now
@@ -146,6 +163,8 @@ class SideTriggerGripController:
elif not trigger_armed: elif not trigger_armed:
trigger_clear_since = None trigger_clear_since = None
action = "open-wait-clear-trigger" action = "open-wait-clear-trigger"
elif not trigger_edge:
action = "open-wait-trigger-held"
else: else:
force_pct = int(clamp( force_pct = int(clamp(
config.close_force, config.close_force,
@@ -163,6 +182,8 @@ class SideTriggerGripController:
recover_position_since = None recover_position_since = None
trigger_clear_since = None trigger_clear_since = None
trigger_armed = False trigger_armed = False
adaptive_target_force = self._adaptive_force_target(feedback)
release_armed = False
hold_pos = None hold_pos = None
reference = None reference = None
action = "side-trigger-close" action = "side-trigger-close"
@@ -179,6 +200,8 @@ class SideTriggerGripController:
state = "gripping" state = "gripping"
state_start = now state_start = now
last_force_ramp_time = now last_force_ramp_time = now
adaptive_target_force = self._adaptive_force_target(feedback)
release_armed = False
reference = None reference = None
action = "object-detected-low-force" action = "object-detected-low-force"
elif now - state_start >= config.close_timeout_seconds: elif now - state_start >= config.close_timeout_seconds:
@@ -196,6 +219,7 @@ class SideTriggerGripController:
last_open_position_ready = False last_open_position_ready = False
recover_quiet_since = None recover_quiet_since = None
recover_position_since = None recover_position_since = None
release_armed = False
hold_pos = None hold_pos = None
reference = None reference = None
action = "close-timeout-open" action = "close-timeout-open"
@@ -222,11 +246,49 @@ class SideTriggerGripController:
reference = self._make_reference(feedback) reference = self._make_reference(feedback)
state = "hold_check" state = "hold_check"
state_start = now state_start = now
adaptive_target_force = self._adaptive_force_target(feedback)
release_armed = False
action = "hold-reference-armed" action = "hold-reference-armed"
else: else:
action = "grip-hold" action = "grip-hold"
elif state == "hold_check": elif state == "hold_check":
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_adapt_force
and now - last_force_ramp_time >= config.adaptive_force_interval
):
force_pct = int(clamp(
force_pct + config.adaptive_force_step,
config.force_min,
adaptive_target_force,
))
self.gripper.set_force(force_pct)
last_force_ramp_time = now
state_start = now
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:
normal_change, shear_change = self._release_changes(feedback, reference) normal_change, shear_change = self._release_changes(feedback, reference)
should_release = ( should_release = (
normal_change >= config.release_normal_change normal_change >= config.release_normal_change
@@ -253,6 +315,7 @@ class SideTriggerGripController:
last_open_position_ready = False last_open_position_ready = False
recover_quiet_since = None recover_quiet_since = None
recover_position_since = None recover_position_since = None
release_armed = False
hold_pos = None hold_pos = None
reference = None reference = None
action = "force-change-release-open" action = "force-change-release-open"
@@ -287,7 +350,9 @@ class SideTriggerGripController:
recover_quiet_since = None recover_quiet_since = None
recover_position_since = None recover_position_since = None
trigger_clear_since = None trigger_clear_since = None
trigger_armed = False trigger_armed = True
last_trigger = trigger
release_armed = False
hold_pos = None hold_pos = None
reference = None reference = None
action = f"open-ready-position pos={current_pos}" action = f"open-ready-position pos={current_pos}"
@@ -307,7 +372,9 @@ class SideTriggerGripController:
recover_quiet_since = None recover_quiet_since = None
recover_position_since = None recover_position_since = None
trigger_clear_since = None trigger_clear_since = None
trigger_armed = False trigger_armed = True
last_trigger = trigger
release_armed = False
hold_pos = None hold_pos = None
reference = None reference = None
action = "open-ready" action = "open-ready"
@@ -327,6 +394,8 @@ class SideTriggerGripController:
shear_change=shear_change, shear_change=shear_change,
hold_pos=hold_pos, hold_pos=hold_pos,
force_pct=force_pct, force_pct=force_pct,
adaptive_target_force=adaptive_target_force,
release_armed=release_armed,
action=action, action=action,
trigger_armed=trigger_armed, trigger_armed=trigger_armed,
) )
@@ -340,10 +409,13 @@ class SideTriggerGripController:
shear_change=shear_change, shear_change=shear_change,
hold_pos=hold_pos, hold_pos=hold_pos,
force_pct=force_pct, force_pct=force_pct,
adaptive_target_force=adaptive_target_force,
release_armed=release_armed,
action=action, action=action,
trigger_armed=trigger_armed, trigger_armed=trigger_armed,
) )
) )
last_trigger = trigger
sleep_time = interval - (time.perf_counter() - tick_start) sleep_time = interval - (time.perf_counter() - tick_start)
if sleep_time > 0: if sleep_time > 0:
@@ -378,6 +450,36 @@ class SideTriggerGripController:
<= int(self.config.open_position_tolerance) <= 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): def _make_reference(self, feedback):
return { return {
"left_normal": feedback.left_normal, "left_normal": feedback.left_normal,
@@ -412,6 +514,8 @@ class SideTriggerGripController:
shear_change, shear_change,
hold_pos, hold_pos,
force_pct, force_pct,
adaptive_target_force,
release_armed,
action, action,
trigger_armed, trigger_armed,
): ):
@@ -425,7 +529,8 @@ class SideTriggerGripController:
f"dN={normal_change:7.3f}{self.normal_unit} " f"dN={normal_change:7.3f}{self.normal_unit} "
f"dShear={shear_change:7.3f}{self.shear_unit} " f"dShear={shear_change:7.3f}{self.shear_unit} "
f"state={state} hold_pos={hold_pos} " 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( def _make_status(
@@ -438,6 +543,8 @@ class SideTriggerGripController:
shear_change, shear_change,
hold_pos, hold_pos,
force_pct, force_pct,
adaptive_target_force,
release_armed,
action, action,
trigger_armed, trigger_armed,
): ):
@@ -474,6 +581,9 @@ class SideTriggerGripController:
"shear_change": shear_change, "shear_change": shear_change,
"hold_pos": hold_pos, "hold_pos": hold_pos,
"force_pct": force_pct, "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, "speed_pct": speed_pct,
"normal_unit": self.normal_unit, "normal_unit": self.normal_unit,
"shear_unit": self.shear_unit, "shear_unit": self.shear_unit,
+18 -1
View File
@@ -53,7 +53,7 @@ class Demo02Config:
hold_force: int = 15 hold_force: int = 15
initial_force: int = 10 initial_force: int = 10
force_min: int = 10 force_min: int = 10
force_max: int = 30 force_max: int = 60
control_hz: float = 30.0 control_hz: float = 30.0
baseline_seconds: float = 1.0 baseline_seconds: float = 1.0
@@ -69,6 +69,14 @@ class Demo02Config:
force_ramp_step: int = 1 force_ramp_step: int = 1
force_ramp_interval: float = 0.1 force_ramp_interval: float = 0.1
grip_settle_seconds: 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_normal_change: float = 0.03
release_shear_change: float = 0.03 release_shear_change: float = 0.03
rearm_seconds: float = 0.5 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-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("--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("--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-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("--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)) add("--rearm-seconds", type=float, default=get("rearm_seconds", cls.rearm_seconds))
+34 -4
View File
@@ -58,6 +58,8 @@ from .main import build_controller
PLOT_POINTS = 180 PLOT_POINTS = 180
PLOT_FORCE_Y_MIN_N = 0.0
PLOT_FORCE_Y_MAX_N = 3.0
CURVE_COLOR = "#00E5FF" CURVE_COLOR = "#00E5FF"
@@ -250,7 +252,7 @@ class GripperDemo02Viewer(QMainWindow):
top = QHBoxLayout() top = QHBoxLayout()
top.setSpacing(8) top.setSpacing(8)
title = QLabel("Gripper Demo 02") title = QLabel("Orisys 夹爪")
title.setObjectName("Title") title.setObjectName("Title")
self.status_chip = QLabel("state=--") self.status_chip = QLabel("state=--")
self.status_chip.setObjectName("StatusChip") self.status_chip.setObjectName("StatusChip")
@@ -295,18 +297,22 @@ class GripperDemo02Viewer(QMainWindow):
self.shear_force = MetricBox("最大切向力", "--") self.shear_force = MetricBox("最大切向力", "--")
self.force_diff = MetricBox("左右差值", "--") self.force_diff = MetricBox("左右差值", "--")
self.force_pct_box = MetricBox("force_pct", "--") self.force_pct_box = MetricBox("force_pct", "--")
self.target_force_box = MetricBox("目标力", "--")
self.speed_box = MetricBox("速度", "--") self.speed_box = MetricBox("速度", "--")
self.trigger_box = MetricBox("触发/武装", "--") self.trigger_box = MetricBox("触发/武装", "--")
self.contact_box = MetricBox("接触", "--") self.contact_box = MetricBox("接触", "--")
self.release_box = MetricBox("释放检测", "--")
boxes = [ boxes = [
self.left_force, self.left_force,
self.right_force, self.right_force,
self.shear_force, self.shear_force,
self.force_diff, self.force_diff,
self.force_pct_box, self.force_pct_box,
self.target_force_box,
self.speed_box, self.speed_box,
self.trigger_box, self.trigger_box,
self.contact_box, self.contact_box,
self.release_box,
] ]
for idx, box in enumerate(boxes): for idx, box in enumerate(boxes):
metrics_grid.addWidget(box, idx // 2, idx % 2) 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.showGrid(x=False, y=True, alpha=0.12)
self.plot.setLabel("bottom", "时间", units="s") self.plot.setLabel("bottom", "时间", units="s")
self.plot.setLabel("left", "", units="N") self.plot.setLabel("left", "", units="N")
self.left_curve = self.plot.plot(pen=pg.mkPen(CURVE_COLOR, width=2), name="L") self.plot.setYRange(PLOT_FORCE_Y_MIN_N, PLOT_FORCE_Y_MAX_N, padding=0)
self.right_curve = self.plot.plot(pen=pg.mkPen("#ffb000", width=2), name="R") self.plot.setLimits(yMin=PLOT_FORCE_Y_MIN_N, yMax=PLOT_FORCE_Y_MAX_N)
self.shear_curve = self.plot.plot(pen=pg.mkPen("#7ddc8a", width=2), name="Shear") 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): def start_control(self):
if self.control_thread is not None and self.control_thread.is_alive(): 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", "--")), str(status.get("force_pct", "--")),
f"hold_pos={status.get('hold_pos')}", 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( self.speed_box.set_value(
str(status.get("speed_pct", "--")), str(status.get("speed_pct", "--")),
"speed_pct", "speed_pct",
@@ -434,6 +457,10 @@ class GripperDemo02Viewer(QMainWindow):
str(int(bool(status.get("grip_contact")))), str(int(bool(status.get("grip_contact")))),
self._contact_text(status), 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 {} left_sample = status.get("left_sample") or {}
right_sample = status.get("right_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"max={fmt(status.get('max_normal'), unit)}",
f"L_shear={fmt(status.get('left_shear'), shear_unit)}", f"L_shear={fmt(status.get('left_shear'), shear_unit)}",
f"R_shear={fmt(status.get('right_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)}", 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.left_curve.setData(x, data[:, 1])
self.right_curve.setData(x, data[:, 2]) self.right_curve.setData(x, data[:, 2])
self.shear_curve.setData(x, data[:, 3]) 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: if len(x) >= 2:
self.plot.setXRange(max(0.0, x[-1] - 6.0), max(6.0, x[-1]), padding=0) self.plot.setXRange(max(0.0, x[-1] - 6.0), max(6.0, x[-1]), padding=0)