This commit is contained in:
cheng
2026-06-09 14:29:44 +08:00
parent 74d2bc4853
commit 1f1ee6d8fe
16 changed files with 277 additions and 53 deletions
BIN
View File
Binary file not shown.

After

Width:  |  Height:  |  Size: 17 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 2.3 MiB

After

Width:  |  Height:  |  Size: 2.3 MiB

Binary file not shown.
Binary file not shown.
+11 -1
View File
@@ -47,6 +47,8 @@ class ForceConverter:
enabled: bool = True
extrapolate: bool = True
clamp_output_min: float | None = 0.0
input_mode: str = "magnitude"
signed: bool = False
@classmethod
def from_config(cls, calibration, config_name):
@@ -71,6 +73,8 @@ class ForceConverter:
enabled=True,
extrapolate=bool(calibration.get("extrapolate", True)),
clamp_output_min=calibration.get("clamp_output_min", 0.0),
input_mode=str(calibration.get("input", "magnitude")).lower(),
signed=bool(calibration.get("signed", False)),
)
def convert(self, raw):
@@ -104,8 +108,14 @@ class ForceConverter:
value = n0 + ratio * (n1 - n0)
return self._clamp(value)
def convert_component(self, raw):
if not self.signed:
return self.convert(raw)
raw = float(raw)
sign = -1.0 if raw < 0.0 else 1.0
return sign * self.convert(abs(raw))
def _clamp(self, value):
if self.clamp_output_min is not None:
value = max(float(self.clamp_output_min), value)
return value
+66 -11
View File
@@ -8,12 +8,20 @@ def shear_magnitude(sample):
return (float(sample["fshearx"]) ** 2 + float(sample["fsheary"]) ** 2) ** 0.5
def _component_shear_enabled(converter):
return getattr(converter, "input_mode", "magnitude") == "components"
@dataclass
class ForceBaseline:
left_normal: float = 0.0
right_normal: float = 0.0
left_shear: float = 0.0
right_shear: float = 0.0
left_shear_x: float = 0.0
left_shear_y: float = 0.0
right_shear_x: float = 0.0
right_shear_y: float = 0.0
@dataclass
@@ -27,6 +35,10 @@ class ForceFeedback:
right_shear: float
max_shear: float
shear_diff: float
left_shear_x: float = 0.0
left_shear_y: float = 0.0
right_shear_x: float = 0.0
right_shear_y: float = 0.0
class TactileFeedbackProcessor:
@@ -42,6 +54,17 @@ class TactileFeedbackProcessor:
self.right_normal_filter = ExponentialFilter(self.config.filter_alpha)
self.left_shear_filter = ExponentialFilter(self.config.shear_filter_alpha)
self.right_shear_filter = ExponentialFilter(self.config.shear_filter_alpha)
self.left_shear_x_filter = ExponentialFilter(self.config.shear_filter_alpha)
self.left_shear_y_filter = ExponentialFilter(self.config.shear_filter_alpha)
self.right_shear_x_filter = ExponentialFilter(self.config.shear_filter_alpha)
self.right_shear_y_filter = ExponentialFilter(self.config.shear_filter_alpha)
def _convert_shear(self, sample):
if _component_shear_enabled(self.shear_converter):
shear_x = self.shear_converter.convert_component(sample["fshearx"])
shear_y = self.shear_converter.convert_component(sample["fsheary"])
return (shear_x ** 2 + shear_y ** 2) ** 0.5, shear_x, shear_y
return self.shear_converter.convert(shear_magnitude(sample)), 0.0, 0.0
def calibrate_baseline(self, reader):
seconds = self.config.baseline_seconds
@@ -53,15 +76,25 @@ class TactileFeedbackProcessor:
right_values = []
left_shear_values = []
right_shear_values = []
left_shear_x_values = []
left_shear_y_values = []
right_shear_x_values = []
right_shear_y_values = []
deadline = time.perf_counter() + seconds
while time.perf_counter() < deadline:
left, right = reader.get_samples(timeout=0.2)
if left is not None:
left_values.append(self.normal_converter.convert(left["fnormal"]))
left_shear_values.append(self.shear_converter.convert(shear_magnitude(left)))
shear, shear_x, shear_y = self._convert_shear(left)
left_shear_values.append(shear)
left_shear_x_values.append(shear_x)
left_shear_y_values.append(shear_y)
if right is not None:
right_values.append(self.normal_converter.convert(right["fnormal"]))
right_shear_values.append(self.shear_converter.convert(shear_magnitude(right)))
shear, shear_x, shear_y = self._convert_shear(right)
right_shear_values.append(shear)
right_shear_x_values.append(shear_x)
right_shear_y_values.append(shear_y)
time.sleep(0.01)
self.baseline = ForceBaseline(
@@ -69,6 +102,10 @@ class TactileFeedbackProcessor:
right_normal=sum(right_values) / len(right_values) if right_values else 0.0,
left_shear=sum(left_shear_values) / len(left_shear_values) if left_shear_values else 0.0,
right_shear=sum(right_shear_values) / len(right_shear_values) if right_shear_values else 0.0,
left_shear_x=sum(left_shear_x_values) / len(left_shear_x_values) if left_shear_x_values else 0.0,
left_shear_y=sum(left_shear_y_values) / len(left_shear_y_values) if left_shear_y_values else 0.0,
right_shear_x=sum(right_shear_x_values) / len(right_shear_x_values) if right_shear_x_values else 0.0,
right_shear_y=sum(right_shear_y_values) / len(right_shear_y_values) if right_shear_y_values else 0.0,
)
return self.baseline
@@ -85,19 +122,34 @@ class TactileFeedbackProcessor:
0.0,
self.normal_converter.convert(right["fnormal"]) - self.baseline.right_normal,
)
left_shear = max(
0.0,
self.shear_converter.convert(shear_magnitude(left)) - self.baseline.left_shear,
)
right_shear = max(
0.0,
self.shear_converter.convert(shear_magnitude(right)) - self.baseline.right_shear,
)
if _component_shear_enabled(self.shear_converter):
_, raw_left_shear_x, raw_left_shear_y = self._convert_shear(left)
_, raw_right_shear_x, raw_right_shear_y = self._convert_shear(right)
left_shear_x = raw_left_shear_x - self.baseline.left_shear_x
left_shear_y = raw_left_shear_y - self.baseline.left_shear_y
right_shear_x = raw_right_shear_x - self.baseline.right_shear_x
right_shear_y = raw_right_shear_y - self.baseline.right_shear_y
left_shear = (left_shear_x ** 2 + left_shear_y ** 2) ** 0.5
right_shear = (right_shear_x ** 2 + right_shear_y ** 2) ** 0.5
else:
left_shear_x = left_shear_y = right_shear_x = right_shear_y = 0.0
left_shear = max(
0.0,
self.shear_converter.convert(shear_magnitude(left)) - self.baseline.left_shear,
)
right_shear = max(
0.0,
self.shear_converter.convert(shear_magnitude(right)) - self.baseline.right_shear,
)
left_normal = self.left_normal_filter.update(left_force)
right_normal = self.right_normal_filter.update(right_force)
left_shear_filtered = self.left_shear_filter.update(left_shear)
right_shear_filtered = self.right_shear_filter.update(right_shear)
left_shear_x_filtered = self.left_shear_x_filter.update(left_shear_x)
left_shear_y_filtered = self.left_shear_y_filter.update(left_shear_y)
right_shear_x_filtered = self.right_shear_x_filter.update(right_shear_x)
right_shear_y_filtered = self.right_shear_y_filter.update(right_shear_y)
return ForceFeedback(
left_normal=left_normal,
@@ -109,5 +161,8 @@ class TactileFeedbackProcessor:
right_shear=right_shear_filtered,
max_shear=max(left_shear_filtered, right_shear_filtered),
shear_diff=left_shear_filtered - right_shear_filtered,
left_shear_x=left_shear_x_filtered,
left_shear_y=left_shear_y_filtered,
right_shear_x=right_shear_x_filtered,
right_shear_y=right_shear_y_filtered,
)
+4 -2
View File
@@ -144,11 +144,13 @@ NORMAL_FORCE_CALIBRATION = {
}
# 切向力标定:sqrt(FSHEARX^2 + FSHEARY^2) 原始幅值 -> N。
# 目前临时沿用 N_.jpg 的法向标定;有切向力标定后替换 points
# 切向力标定:FSHEARX / FSHEARY 原始分量 -> N。
# X/Y 分量分别按同一条曲线标定;控制逻辑再使用标定后分量的合力
SHEAR_FORCE_CALIBRATION = {
"enabled": True,
"method": "piecewise_linear",
"input": "components",
"signed": True,
"extrapolate": True,
"clamp_output_min": 0.0,
"points": [
+104 -4
View File
@@ -21,10 +21,24 @@ class SideTriggerGripController:
self.shear_unit = feedback_processor.shear_converter.unit
self.status_callback = status_callback
self._stop_event = threading.Event()
self._manual_lock = threading.Lock()
self._manual_command = None
def stop(self):
self._stop_event.set()
def request_manual_command(self, command):
if command not in {"open", "close", "hold", "release"}:
raise ValueError(f"unsupported manual command: {command}")
with self._manual_lock:
self._manual_command = command
def _pop_manual_command(self):
with self._manual_lock:
command = self._manual_command
self._manual_command = None
return command
def run(self):
self._stop_event.clear()
self.reader.start()
@@ -122,19 +136,92 @@ class SideTriggerGripController:
while not self._stop_event.is_set():
tick_start = time.perf_counter()
now = time.perf_counter()
manual_action = None
manual_command = self._pop_manual_command()
if manual_command == "open":
force_pct = int(clamp(
config.open_force,
config.force_min,
config.force_max,
))
self.gripper.open(force_pct, "manual-open")
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
trigger_clear_since = None
trigger_armed = False
release_armed = False
hold_pos = None
reference = None
manual_action = "manual-open"
elif manual_command == "close":
force_pct = int(clamp(
config.close_force,
config.force_min,
config.force_max,
))
self.gripper.close(force_pct, "manual-close")
state = "manual_close"
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
release_armed = False
hold_pos = None
reference = None
manual_action = "manual-close"
elif manual_command == "hold":
force_pct = int(clamp(
config.hold_force,
config.force_min,
config.force_max,
))
self.gripper.set_force(force_pct)
hold_pos = self.gripper.hold_current_position(force_pct)
state = "manual_hold"
state_start = now
last_force_ramp_time = now
trigger_clear_since = None
trigger_armed = False
release_armed = False
reference = None
manual_action = "manual-hold"
elif manual_command == "release":
if state == "manual_hold":
state = "open_wait"
state_start = now
trigger_clear_since = None
trigger_armed = False
release_armed = False
hold_pos = None
reference = None
manual_action = "manual-release"
feedback = self.feedback_processor.read(self.reader, timeout=0.3)
if feedback is None:
print(f"state={state} action=waiting-for-sensor-samples")
action = manual_action or "waiting-for-sensor-samples"
print(f"state={state} action={action}")
self._emit_status(
{
"timestamp": time.time(),
"state": state,
"action": "waiting-for-sensor-samples",
"action": action,
"force_pct": force_pct,
"adaptive_target_force": adaptive_target_force,
"adaptive_grip_enabled": config.adaptive_grip_enabled,
"release_armed": release_armed,
"manual_hold_active": state == "manual_hold",
"speed_pct": config.speed,
"normal_unit": self.normal_unit,
"shear_unit": self.shear_unit,
@@ -147,9 +234,11 @@ class SideTriggerGripController:
grip_contact = self._grip_contact(feedback)
normal_change = 0.0
shear_change = 0.0
action = "wait"
action = manual_action or "wait"
if state == "open_wait":
if manual_action is not None:
pass
elif state == "open_wait":
trigger_edge = trigger and not last_trigger
if not trigger:
if trigger_clear_since is None:
@@ -387,6 +476,12 @@ class SideTriggerGripController:
recover_position_since = None
action = "open-recover-wait-force-clear"
elif state == "manual_hold":
action = "manual-hold"
elif state == "manual_close":
action = "manual-close"
self._log_tick(
feedback=feedback,
state=state,
@@ -577,6 +672,10 @@ class SideTriggerGripController:
"normal_diff": feedback.normal_diff,
"left_shear": feedback.left_shear,
"right_shear": feedback.right_shear,
"left_shear_x": feedback.left_shear_x,
"left_shear_y": feedback.left_shear_y,
"right_shear_x": feedback.right_shear_x,
"right_shear_y": feedback.right_shear_y,
"max_shear": feedback.max_shear,
"shear_diff": feedback.shear_diff,
"normal_change": normal_change,
@@ -586,6 +685,7 @@ class SideTriggerGripController:
"adaptive_target_force": adaptive_target_force,
"adaptive_grip_enabled": self.config.adaptive_grip_enabled,
"release_armed": bool(release_armed),
"manual_hold_active": state == "manual_hold",
"speed_pct": speed_pct,
"normal_unit": self.normal_unit,
"shear_unit": self.shear_unit,
+92 -35
View File
@@ -60,6 +60,8 @@ from .main import build_controller
PLOT_POINTS = 180
PLOT_FORCE_Y_MIN_N = 0.0
PLOT_FORCE_Y_MAX_N = 3.0
PLOT_SHEAR_Y_MIN_N = -3.0
PLOT_SHEAR_Y_MAX_N = 3.0
FORCE_DISPLAY_ZERO_THRESHOLD_N = 0.05
CURVE_COLOR = "#00E5FF"
@@ -183,6 +185,16 @@ QPushButton#RestartButton {
color: #e0c96a;
border: 1px solid #4d4520;
}
QPushButton#ManualButton {
background-color: #101822;
color: #9cc8ff;
border: 1px solid #23405f;
}
QPushButton#HoldButton {
background-color: #102019;
color: #7ee0a2;
border: 1px solid #24573a;
}
"""
@@ -284,6 +296,15 @@ class GripperDemo02Viewer(QMainWindow):
self.restart_button = QPushButton("重启")
self.restart_button.setObjectName("RestartButton")
self.restart_button.clicked.connect(self.restart_control)
self.manual_open_button = QPushButton("夹爪张开")
self.manual_open_button.setObjectName("ManualButton")
self.manual_open_button.clicked.connect(lambda: self.send_manual_command("open"))
self.manual_close_button = QPushButton("夹爪闭合")
self.manual_close_button.setObjectName("ManualButton")
self.manual_close_button.clicked.connect(lambda: self.send_manual_command("close"))
self.manual_hold_button = QPushButton("保持")
self.manual_hold_button.setObjectName("HoldButton")
self.manual_hold_button.clicked.connect(self.toggle_manual_hold)
self._set_button_state("idle")
top.addWidget(title)
top.addWidget(self.status_chip)
@@ -291,6 +312,9 @@ class GripperDemo02Viewer(QMainWindow):
top.addWidget(self.start_button)
top.addWidget(self.stop_button)
top.addWidget(self.restart_button)
top.addWidget(self.manual_open_button)
top.addWidget(self.manual_close_button)
top.addWidget(self.manual_hold_button)
outer.addLayout(top)
main = QGridLayout()
@@ -298,17 +322,13 @@ class GripperDemo02Viewer(QMainWindow):
outer.addLayout(main, 1)
self.left_flow = ImageBox("左侧光流")
self.left_mag = ImageBox("左侧幅值图")
self.right_flow = ImageBox("右侧光流")
self.right_mag = ImageBox("右侧幅值图")
main.addWidget(self.left_flow, 0, 0)
main.addWidget(self.left_mag, 0, 1)
main.addWidget(self.right_flow, 1, 0)
main.addWidget(self.right_mag, 1, 1)
side = QVBoxLayout()
side.setSpacing(8)
main.addLayout(side, 0, 2, 2, 1)
main.addLayout(side, 0, 1, 2, 1)
metrics_grid = QGridLayout()
metrics_grid.setSpacing(8)
@@ -356,32 +376,37 @@ class GripperDemo02Viewer(QMainWindow):
side.addWidget(self.info_box)
main.setColumnStretch(0, 2)
main.setColumnStretch(1, 2)
main.setColumnStretch(2, 3)
main.setColumnStretch(1, 4)
main.setRowStretch(0, 1)
main.setRowStretch(1, 1)
def _configure_plot(self):
self.plot = self.plot_widget.addPlot()
self.plot.setTitle("左右法向力", color="#999999", size="13pt")
self.plot.titleLabel.setFont(QFont("SimHei", 13))
self.plot.showAxis("top", False)
self.plot.showAxis("right", False)
self.plot.showGrid(x=False, y=True, alpha=0.12)
self.plot.setLabel("bottom", "时间", units="s")
self.plot.setLabel("left", "", units="N")
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.plots = []
self.curves = {}
plot_specs = [
("left_normal", "左法向力", "#00E5FF", PLOT_FORCE_Y_MIN_N, PLOT_FORCE_Y_MAX_N),
("right_normal", "右法向力", "#ffb000", PLOT_FORCE_Y_MIN_N, PLOT_FORCE_Y_MAX_N),
("left_shear_x", "左切向 X", "#61d394", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N),
("left_shear_y", "左切向 Y", "#f07178", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N),
("right_shear_x", "右切向 X", "#8ab4f8", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N),
("right_shear_y", "右切向 Y", "#d19a66", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N),
]
for idx, (key, title, color, y_min, y_max) in enumerate(plot_specs):
row = idx // 2
col = idx % 2
plot = self.plot_widget.addPlot(row=row, col=col)
plot.setTitle(title, color="#999999", size="11pt")
plot.titleLabel.setFont(QFont("SimHei", 11))
plot.showAxis("top", False)
plot.showAxis("right", False)
plot.showGrid(x=False, y=True, alpha=0.12)
plot.setLabel("bottom", "时间", units="s")
plot.setLabel("left", "", units="N")
plot.setYRange(y_min, y_max, padding=0)
plot.setLimits(yMin=y_min, yMax=y_max)
plot.enableAutoRange(axis=pg.ViewBox.YAxis, enable=False)
self.plots.append(plot)
self.curves[key] = plot.plot(pen=pg.mkPen(color, width=2))
def _set_button_state(self, state):
labels = {
@@ -407,6 +432,23 @@ class GripperDemo02Viewer(QMainWindow):
self.start_button.setEnabled(start_enabled)
self.stop_button.setEnabled(stop_enabled)
self.restart_button.setEnabled(restart_enabled)
manual_enabled = state == "running"
self.manual_open_button.setEnabled(manual_enabled)
self.manual_close_button.setEnabled(manual_enabled)
self.manual_hold_button.setEnabled(manual_enabled)
def send_manual_command(self, command):
if self.controller is None or self.control_thread is None or not self.control_thread.is_alive():
self.info_label.setText("请先启动控制器")
return
try:
self.controller.request_manual_command(command)
except Exception as exc:
self.info_label.setText(f"手动命令失败:{exc}")
def toggle_manual_hold(self):
command = "release" if self.manual_hold_button.text() == "释放" else "hold"
self.send_manual_command(command)
def start_control(self, from_restart=False):
if self.control_thread is not None and self.control_thread.is_alive():
@@ -482,6 +524,9 @@ class GripperDemo02Viewer(QMainWindow):
shear_unit = status.get("shear_unit", "N")
self.status_chip.setText(f"state={state}")
self.action_chip.setText(f"action={action}")
self.manual_hold_button.setText(
"释放" if bool(status.get("manual_hold_active")) else "保持"
)
self.left_force.set_value(
fmt(status.get("left_normal"), unit),
@@ -493,11 +538,11 @@ class GripperDemo02Viewer(QMainWindow):
)
self.left_shear_box.set_value(
fmt(status.get("left_shear"), shear_unit),
"left_shear",
f"x={fmt(status.get('left_shear_x'), shear_unit)} y={fmt(status.get('left_shear_y'), shear_unit)}",
)
self.right_shear_box.set_value(
fmt(status.get("right_shear"), shear_unit),
"right_shear",
f"x={fmt(status.get('right_shear_x'), shear_unit)} y={fmt(status.get('right_shear_y'), shear_unit)}",
)
self.force_pct_box.set_value(
str(status.get("force_pct", "--")),
@@ -527,9 +572,7 @@ class GripperDemo02Viewer(QMainWindow):
left_sample = status.get("left_sample") or {}
right_sample = status.get("right_sample") or {}
self.left_flow.set_image(left_sample.get("flow_view"))
self.left_mag.set_image(left_sample.get("magnitude_view"))
self.right_flow.set_image(right_sample.get("flow_view"))
self.right_mag.set_image(right_sample.get("magnitude_view"))
elapsed = time.time() - self.start_time
self.force_history.append(
@@ -537,6 +580,10 @@ class GripperDemo02Viewer(QMainWindow):
elapsed,
force_display_value(status.get("left_normal", 0.0) or 0.0),
force_display_value(status.get("right_normal", 0.0) or 0.0),
force_display_value(status.get("left_shear_x", 0.0) or 0.0),
force_display_value(status.get("left_shear_y", 0.0) or 0.0),
force_display_value(status.get("right_shear_x", 0.0) or 0.0),
force_display_value(status.get("right_shear_y", 0.0) or 0.0),
)
)
if len(self.force_history) > PLOT_POINTS:
@@ -562,11 +609,21 @@ class GripperDemo02Viewer(QMainWindow):
return
data = np.asarray(self.force_history, dtype=float)
x = data[:, 0]
self.left_curve.setData(x, data[:, 1])
self.right_curve.setData(x, data[:, 2])
self.plot.setYRange(PLOT_FORCE_Y_MIN_N, PLOT_FORCE_Y_MAX_N, padding=0)
keys = [
"left_normal",
"right_normal",
"left_shear_x",
"left_shear_y",
"right_shear_x",
"right_shear_y",
]
for idx, key in enumerate(keys, start=1):
self.curves[key].setData(x, data[:, idx])
if len(x) >= 2:
self.plot.setXRange(max(0.0, x[-1] - 6.0), max(6.0, x[-1]), padding=0)
x_min = max(0.0, x[-1] - 6.0)
x_max = max(6.0, x[-1])
for plot in self.plots:
plot.setXRange(x_min, x_max, padding=0)
def _update_thread_buttons(self):
if not self.controller_finished: