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