qt第一版可视化

gripper_control_02/config/gripper_demo_02.py

@@ -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,