diff --git a/config/compound_eye.png b/config/compound_eye.png index 019065f..74970b0 100644 Binary files a/config/compound_eye.png and b/config/compound_eye.png differ diff --git a/gripper_control_02/README.md b/gripper_control_02/README.md index b7eddb0..47b2b39 100644 --- a/gripper_control_02/README.md +++ b/gripper_control_02/README.md @@ -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`: 力已经清空并稳定，重新进入等待下一次扫过触发。 diff --git a/gripper_control_02/__pycache__/controller.cpython-311.pyc b/gripper_control_02/__pycache__/controller.cpython-311.pyc index e60fa35..4c0d7d4 100644 Binary files a/gripper_control_02/__pycache__/controller.cpython-311.pyc and b/gripper_control_02/__pycache__/controller.cpython-311.pyc differ diff --git a/gripper_control_02/__pycache__/controller.cpython-313.pyc b/gripper_control_02/__pycache__/controller.cpython-313.pyc index c055ba8..f30f5f5 100644 Binary files a/gripper_control_02/__pycache__/controller.cpython-313.pyc and b/gripper_control_02/__pycache__/controller.cpython-313.pyc differ diff --git a/gripper_control_02/__pycache__/settings.cpython-311.pyc b/gripper_control_02/__pycache__/settings.cpython-311.pyc index 721453e..966bc6c 100644 Binary files a/gripper_control_02/__pycache__/settings.cpython-311.pyc and b/gripper_control_02/__pycache__/settings.cpython-311.pyc differ diff --git a/gripper_control_02/__pycache__/settings.cpython-313.pyc b/gripper_control_02/__pycache__/settings.cpython-313.pyc index db32e39..63dbe21 100644 Binary files a/gripper_control_02/__pycache__/settings.cpython-313.pyc and b/gripper_control_02/__pycache__/settings.cpython-313.pyc differ diff --git a/gripper_control_02/__pycache__/visualizer.cpython-311.pyc b/gripper_control_02/__pycache__/visualizer.cpython-311.pyc index c2ec4f7..76182a5 100644 Binary files a/gripper_control_02/__pycache__/visualizer.cpython-311.pyc and b/gripper_control_02/__pycache__/visualizer.cpython-311.pyc differ diff --git a/gripper_control_02/__pycache__/visualizer.cpython-313.pyc b/gripper_control_02/__pycache__/visualizer.cpython-313.pyc index 1e9b90c..28510da 100644 Binary files a/gripper_control_02/__pycache__/visualizer.cpython-313.pyc and b/gripper_control_02/__pycache__/visualizer.cpython-313.pyc differ diff --git a/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-311.pyc b/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-311.pyc index 0127587..50a258d 100644 Binary files a/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-311.pyc and b/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-311.pyc differ diff --git a/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-313.pyc b/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-313.pyc index 750d858..237027a 100644 Binary files a/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-313.pyc and b/gripper_control_02/config/__pycache__/gripper_demo_02.cpython-313.pyc differ diff --git a/gripper_control_02/config/gripper_demo_02.py b/gripper_control_02/config/gripper_demo_02.py index d8dc062..551cb0e 100644 --- a/gripper_control_02/config/gripper_demo_02.py +++ b/gripper_control_02/config/gripper_demo_02.py @@ -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, diff --git a/gripper_control_02/controller.py b/gripper_control_02/controller.py index ef5161e..0715e6d 100644 --- a/gripper_control_02/controller.py +++ b/gripper_control_02/controller.py @@ -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, diff --git a/gripper_control_02/settings.py b/gripper_control_02/settings.py index 515ae85..44ff382 100644 --- a/gripper_control_02/settings.py +++ b/gripper_control_02/settings.py @@ -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)) diff --git a/gripper_control_02/visualizer.py b/gripper_control_02/visualizer.py index 2b10530..cec6112 100644 --- a/gripper_control_02/visualizer.py +++ b/gripper_control_02/visualizer.py @@ -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)