orisys/test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

v 1.2

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
N2.jpg
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 17 KiB

config/compound_eye.png
BIN
View File
Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 2.3 MiB

After

Width:  |  Height:  |  Size: 2.3 MiB

gripper_control/__pycache__/calibration.cpython-311.pyc
BIN
View File
Binary file not shown.
gripper_control/__pycache__/calibration.cpython-313.pyc
BIN
View File
Binary file not shown.
gripper_control/__pycache__/feedback.cpython-311.pyc
BIN
View File
Binary file not shown.
gripper_control/__pycache__/feedback.cpython-313.pyc
BIN
View File
Binary file not shown.
gripper_control/calibration.py
+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
gripper_control/feedback.py
+58 -3
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,6 +122,17 @@ class TactileFeedbackProcessor:
0.0,
self.normal_converter.convert(right["fnormal"]) - self.baseline.right_normal,
)
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,
@@ -98,6 +146,10 @@ class TactileFeedbackProcessor:
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,
)
gripper_control_02/__pycache__/controller.cpython-311.pyc
BIN
View File
Binary file not shown.
gripper_control_02/__pycache__/controller.cpython-313.pyc
BIN
View File
Binary file not shown.
gripper_control_02/__pycache__/visualizer.cpython-311.pyc
BIN
View File
Binary file not shown.
gripper_control_02/__pycache__/visualizer.cpython-313.pyc
BIN
View File
Binary file not shown.
gripper_control_02/config/__pycache__/gripper_demo_02.cpython-311.pyc
BIN
View File
Binary file not shown.
gripper_control_02/config/gripper_demo_02.py
+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": [
gripper_control_02/controller.py
+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,
gripper_control_02/visualizer.py
+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: