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 09:31:44 +08:00
parent cbd8683429
commit 66a00a8b5b
39 changed files with 1055 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
gripper_control_02/controller.py
+216 -8
View File
@@ -1,18 +1,32 @@
import time
import threading
from gripper_control.filters import clamp
class SideTriggerGripController:
def __init__(self, config, reader, gripper, feedback_processor):
def __init__(
self,
config,
reader,
gripper,
feedback_processor,
status_callback=None,
):
self.config = config
self.reader = reader
self.gripper = gripper
self.feedback_processor = feedback_processor
self.normal_unit = feedback_processor.normal_converter.unit
self.shear_unit = feedback_processor.shear_converter.unit
self.status_callback = status_callback
self._stop_event = threading.Event()
def stop(self):
self._stop_event.set()
def run(self):
self._stop_event.clear()
self.reader.start()
try:
@@ -59,6 +73,20 @@ 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(
"Release open: "
f"speed={self.config.release_open_speed}, "
f"accel={self.config.release_open_accel}, "
f"decel={self.config.release_open_decel}"
)
print(
"Recover: "
f"force_normal={self.config.recover_normal_force:.3f}{self.normal_unit}, "
f"force_shear={self.config.recover_shear_force:.3f}{self.shear_unit}, "
f"open_pos={self.config.open_pos}, "
f"pos_enabled={int(self.config.open_position_recover_enabled)}, "
f"pos_tol={self.config.open_position_tolerance}"
)
def _run_event_monitor(self):
config = self.config
@@ -67,18 +95,35 @@ class SideTriggerGripController:
state = "open_wait"
state_start = time.perf_counter()
last_force_ramp_time = state_start
last_open_position_check_time = 0.0
last_open_position = None
last_open_position_ready = False
recover_quiet_since = None
recover_position_since = None
trigger_clear_since = None
trigger_armed = False
force_pct = int(clamp(config.open_force, config.force_min, config.force_max))
hold_pos = None
reference = None
while True:
while not self._stop_event.is_set():
tick_start = time.perf_counter()
now = time.perf_counter()
feedback = self.feedback_processor.read(self.reader, timeout=0.3)
if feedback is None:
print(f"state={state} action=waiting-for-sensor-samples")
self._emit_status(
{
"timestamp": time.time(),
"state": state,
"action": "waiting-for-sensor-samples",
"force_pct": force_pct,
"speed_pct": config.speed,
"normal_unit": self.normal_unit,
"shear_unit": self.shear_unit,
}
)
time.sleep(0.05)
continue
@@ -89,7 +134,19 @@ class SideTriggerGripController:
action = "wait"
if state == "open_wait":
if trigger:
if not trigger:
if trigger_clear_since is None:
trigger_clear_since = now
action = "open-wait-trigger-clearing"
elif now - trigger_clear_since >= config.trigger_clear_stable_seconds:
trigger_armed = True
action = "open-wait-armed"
else:
action = "open-wait-trigger-clearing"
elif not trigger_armed:
trigger_clear_since = None
action = "open-wait-clear-trigger"
else:
force_pct = int(clamp(
config.close_force,
config.force_min,
@@ -99,12 +156,16 @@ class SideTriggerGripController:
state = "closing"
state_start = now
last_force_ramp_time = 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
trigger_clear_since = None
trigger_armed = False
hold_pos = None
reference = None
action = "side-trigger-close"
else:
action = "open-wait-touch"
elif state == "closing":
if grip_contact:
@@ -130,7 +191,11 @@ class SideTriggerGripController:
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
hold_pos = None
reference = None
action = "close-timeout-open"
@@ -173,11 +238,21 @@ class SideTriggerGripController:
config.force_min,
config.force_max,
))
self.gripper.open(force_pct, "force-change-release-open")
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
hold_pos = None
reference = None
action = "force-change-release-open"
@@ -185,7 +260,41 @@ class SideTriggerGripController:
action = "hold-check"
elif state == "open_recover":
if self._recover_quiet(feedback):
current_pos = last_open_position
position_ready = last_open_position_ready
if (
config.open_position_recover_enabled
and now - last_open_position_check_time
>= config.open_position_check_interval
):
last_open_position_check_time = now
current_pos = self.gripper.read_position()
position_ready = self._open_position_ready(current_pos)
last_open_position = current_pos
last_open_position_ready = position_ready
if position_ready:
if recover_position_since is None:
recover_position_since = now
action = f"open-recover-position pos={current_pos}"
elif (
now - recover_position_since
>= config.open_position_stable_seconds
and now - state_start >= config.rearm_seconds
):
state = "open_wait"
state_start = now
recover_quiet_since = None
recover_position_since = None
trigger_clear_since = None
trigger_armed = False
hold_pos = None
reference = None
action = f"open-ready-position pos={current_pos}"
else:
action = f"open-recover-position-stabilizing pos={current_pos}"
elif self._recover_quiet(feedback):
recover_position_since = None
if recover_quiet_since is None:
recover_quiet_since = now
action = "open-recover-quiet"
@@ -196,6 +305,9 @@ class SideTriggerGripController:
state = "open_wait"
state_start = now
recover_quiet_since = None
recover_position_since = None
trigger_clear_since = None
trigger_armed = False
hold_pos = None
reference = None
action = "open-ready"
@@ -203,6 +315,7 @@ class SideTriggerGripController:
action = "open-recover-stabilizing"
else:
recover_quiet_since = None
recover_position_since = None
action = "open-recover-wait-force-clear"
self._log_tick(
@@ -215,12 +328,29 @@ class SideTriggerGripController:
hold_pos=hold_pos,
force_pct=force_pct,
action=action,
trigger_armed=trigger_armed,
)
self._emit_status(
self._make_status(
feedback=feedback,
state=state,
trigger=trigger,
grip_contact=grip_contact,
normal_change=normal_change,
shear_change=shear_change,
hold_pos=hold_pos,
force_pct=force_pct,
action=action,
trigger_armed=trigger_armed,
)
)
sleep_time = interval - (time.perf_counter() - tick_start)
if sleep_time > 0:
time.sleep(sleep_time)
print("demo02 control loop stopped.")
def _side_triggered(self, feedback):
return (
feedback.max_normal >= self.config.trigger_normal_force
@@ -240,10 +370,20 @@ class SideTriggerGripController:
and feedback.max_shear <= self.config.recover_shear_force
)
def _open_position_ready(self, current_pos):
if current_pos is None:
return False
return (
abs(int(current_pos) - int(self.config.open_pos))
<= int(self.config.open_position_tolerance)
)
def _make_reference(self, feedback):
return {
"left_normal": feedback.left_normal,
"right_normal": feedback.right_normal,
"left_shear": feedback.left_shear,
"right_shear": feedback.right_shear,
"max_shear": feedback.max_shear,
}
@@ -255,6 +395,11 @@ class SideTriggerGripController:
abs(feedback.right_normal - reference["right_normal"]),
)
shear_change = abs(feedback.max_shear - reference["max_shear"])
shear_change = max(
shear_change,
abs(feedback.left_shear - reference["left_shear"]),
abs(feedback.right_shear - reference["right_shear"]),
)
return normal_change, shear_change
def _log_tick(
@@ -268,15 +413,78 @@ class SideTriggerGripController:
hold_pos,
force_pct,
action,
trigger_armed,
):
print(
f"L={feedback.left_normal:7.3f}{self.normal_unit} "
f"R={feedback.right_normal:7.3f}{self.normal_unit} "
f"maxN={feedback.max_normal:7.3f}{self.normal_unit} "
f"shear={feedback.max_shear:7.3f}{self.shear_unit} "
f"trigger={int(trigger)} grip_contact={int(grip_contact)} "
f"trigger={int(trigger)} armed={int(trigger_armed)} "
f"grip_contact={int(grip_contact)} "
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}"
)
def _make_status(
self,
feedback,
state,
trigger,
grip_contact,
normal_change,
shear_change,
hold_pos,
force_pct,
action,
trigger_armed,
):
speed_pct = self.config.speed
if action == "force-change-release-open":
speed_pct = self.config.release_open_speed
left_sample = None
right_sample = None
try:
if len(self.reader.latest) >= 2:
left_sample = self.reader.latest[0]
right_sample = self.reader.latest[1]
except Exception:
pass
return {
"timestamp": time.time(),
"state": state,
"action": action,
"trigger": bool(trigger),
"trigger_armed": bool(trigger_armed),
"grip_contact": bool(grip_contact),
"left_normal": feedback.left_normal,
"right_normal": feedback.right_normal,
"min_normal": feedback.min_normal,
"max_normal": feedback.max_normal,
"normal_diff": feedback.normal_diff,
"left_shear": feedback.left_shear,
"right_shear": feedback.right_shear,
"max_shear": feedback.max_shear,
"shear_diff": feedback.shear_diff,
"normal_change": normal_change,
"shear_change": shear_change,
"hold_pos": hold_pos,
"force_pct": force_pct,
"speed_pct": speed_pct,
"normal_unit": self.normal_unit,
"shear_unit": self.shear_unit,
"left_sample": left_sample,
"right_sample": right_sample,
}
def _emit_status(self, status):
if self.status_callback is None:
return
try:
self.status_callback(status)
except Exception as exc:
print(f"status callback failed: {exc}")