diff --git a/.gitignore b/.gitignore index fc8a9cd..580720e 100644 --- a/.gitignore +++ b/.gitignore @@ -9,10 +9,12 @@ venv/ env/ ENV/ + # Test and coverage output .pytest_cache/ .coverage htmlcov/ +debug/ # PyInstaller output dist/ diff --git a/README.md b/README.md index 963b766..66c20fc 100644 --- a/README.md +++ b/README.md @@ -62,7 +62,7 @@ config\gripper.json ```powershell conda activate py311 -pip install pyinstaller +pip install pyinstaller PyOpenGL PyOpenGL_accelerate ``` 然后从项目根目录运行： @@ -150,6 +150,7 @@ dist\GripperDemo02\sdk - 当前 `release_armed`，用于判断系统是在补夹紧力，还是已经开始检测人取物松开。 - 折线图带图例，默认固定 y 轴为 `0~3 N`，范围在 `visualizer.py` 顶部的 `PLOT_FORCE_Y_MIN_N / PLOT_FORCE_Y_MAX_N` 调整。 - 左右传感器 FPS、接触标志、光流均值/最大值。 +- 中间 3D 夹爪使用 `crt_ctag2f90d_gripper_visualization` 里的完整 STL/URDF 实体面片模型；启动时会预缓存 41 个开合档位，运行中只按实际位置切换缓存，避免拖慢控制延迟。 ## 状态说明 diff --git a/build/build_exe.py b/build/build_exe.py index bd27b70..e637e64 100644 --- a/build/build_exe.py +++ b/build/build_exe.py @@ -8,6 +8,11 @@ from pathlib import Path BUILD_DIR = Path(__file__).resolve().parent PROJECT_ROOT = BUILD_DIR.parent +VISUALIZATION_DIR = PROJECT_ROOT / "crt_ctag2f90d_gripper_visualization" +LOGO_FILES = ( + PROJECT_ROOT / "gripper" / "logo.png", + PROJECT_ROOT / "gripper" / "logo_topbar.png", +) def module_exists(name): @@ -88,6 +93,29 @@ def build(app_name, clean_dist): "pyqtgraph", ] + if VISUALIZATION_DIR.exists(): + args.extend( + [ + "--add-data", + data_arg(VISUALIZATION_DIR, "crt_ctag2f90d_gripper_visualization"), + ] + ) + else: + print(f"Warning: visualization assets were not found: {VISUALIZATION_DIR}") + + for logo_file in LOGO_FILES: + if logo_file.exists(): + args.extend(["--add-data", data_arg(logo_file, "gripper")]) + else: + print(f"Warning: logo asset was not found: {logo_file}") + + if module_exists("OpenGL"): + args.extend(["--collect-all", "OpenGL"]) + else: + print( + "Warning: module 'OpenGL' was not found; install PyOpenGL for the solid 3D model." + ) + if module_exists("orisys"): args.extend(["--collect-submodules", "orisys"]) else: @@ -104,11 +132,28 @@ def build(app_name, clean_dist): app_dir = exe_path.parent config_dir = copy_runtime_dir(PROJECT_ROOT / "config", app_dir, "config") sdk_dir = copy_runtime_dir(PROJECT_ROOT / "sdk", app_dir, "sdk") + visualization_dir = None + if VISUALIZATION_DIR.exists(): + visualization_dir = copy_runtime_dir( + VISUALIZATION_DIR, + app_dir, + "crt_ctag2f90d_gripper_visualization", + ) + runtime_logo_dir = app_dir / "gripper" + runtime_logo_dir.mkdir(exist_ok=True) + copied_logos = [] + for logo_file in LOGO_FILES: + if logo_file.exists(): + copied_logos.append(shutil.copy2(logo_file, runtime_logo_dir / logo_file.name)) print() print(f"Build complete: {exe_path}") print(f"Runtime config copied to: {config_dir}") print(f"Runtime sdk copied to: {sdk_dir}") + if visualization_dir is not None: + print(f"Runtime 3D model copied to: {visualization_dir}") + for logo_path in copied_logos: + print(f"Runtime logo copied to: {logo_path}") def main(): diff --git a/config/compound_eye.png b/config/compound_eye.png index bd61a42..1f99841 100644 Binary files a/config/compound_eye.png and b/config/compound_eye.png differ diff --git a/config/gripper.json b/config/gripper.json index 53b3fb4..fa3f333 100644 --- a/config/gripper.json +++ b/config/gripper.json @@ -12,18 +12,18 @@ "serial_timeout": 0.2, "open_pos": 0, "close_pos": 9000, - "speed": 25, + "speed": 50, "accel": 80, "decel": 80, "release_open_speed": 50, "release_open_accel": 120, "release_open_decel": 120, - "open_force": 15, - "close_force": 15, - "grip_start_force": 10, - "hold_force": 15, - "initial_force": 15, - "force_min": 10, + "open_force": 20, + "close_force": 20, + "grip_start_force": 20, + "hold_force": 20, + "initial_force": 20, + "force_min": 20, "force_max": 80, "control_hz": 30.0, "baseline_seconds": 1.0, @@ -46,7 +46,7 @@ "adaptive_shear_start_force": 0.1, "adaptive_shear_full_force": 0.6, "adaptive_force_min": 15, - "adaptive_force_max": 45, + "adaptive_force_max": 80, "adaptive_force_step": 1, "adaptive_force_interval": 0.05, "release_arm_delay_seconds": 0.8, diff --git a/crt_ctag2f90d_gripper_visualization/CMakeLists.txt b/crt_ctag2f90d_gripper_visualization/CMakeLists.txt new file mode 100644 index 0000000..81ae449 --- /dev/null +++ b/crt_ctag2f90d_gripper_visualization/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 2.8.3) + +project(crt_ctag2f90d_gripper_visualization) + +find_package(catkin REQUIRED) + +catkin_package() + +find_package(roslaunch) + +foreach(dir config launch meshes urdf) + install(DIRECTORY ${dir}/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) +endforeach(dir) diff --git a/crt_ctag2f90d_gripper_visualization/config/joint_names_crt_ctag2f90d_visualization.yaml b/crt_ctag2f90d_gripper_visualization/config/joint_names_crt_ctag2f90d_visualization.yaml new file mode 100644 index 0000000..817848d --- /dev/null +++ b/crt_ctag2f90d_gripper_visualization/config/joint_names_crt_ctag2f90d_visualization.yaml @@ -0,0 +1 @@ +controller_joint_names: ['', 'Left_1_Joint', 'Left_Support_Joint', 'Left_2_Joint', 'Right_2_Joint', 'Right_1_Joint', 'Right_Support_Joint', ] diff --git a/crt_ctag2f90d_gripper_visualization/docs/README.md b/crt_ctag2f90d_gripper_visualization/docs/README.md new file mode 100644 index 0000000..67a21d9 --- /dev/null +++ b/crt_ctag2f90d_gripper_visualization/docs/README.md @@ -0,0 +1,58 @@ +# ctag2f90d + +## Environment configuration + +Ubuntu 18.04.6 LTS +Python 3.6.9 +ROS melodic 1.14.13 + +python package:minimalmodbus is needed. +Install minimalmodbus: input `pip install minimalmodbus` in terminal. + +copy **crt_ctag2f90d_gripper_visualization** to the src folder of your own Ubuntu system ROS workspace and execute the following statement +## Run + +### 1. After connecting the robot, make sure there is a USB port in Ubuntu + Input in terminal: + `ls -l /dev/ttyUSB* ` + Confirm the device serial port number and add permissions. + `sudo chmod 777 /dev/ttyUSB0` + + +### 2. Update ROS envirnment +Input in terminal: +```sh +cd ~/catkin_ws + +catkin_make +``` + +### 3. Start Rviz node + +```sh +cd ./src/crt_ctag2f90d_gripper_visualization + +roslaunch crt_ctag2f90d_gripper_visualization display_sync.launch # Synchronous mode +``` + +After inputting,Rviz interface will come out: + +![image-1](asserts/001.png#pic_center) +
Figure1 ctag2f90d rviz interface
+ + +![image-2](asserts/002.png#pic_center) + +
Figure2 joint state publisher interface
+ +If warning coming out like **Could not find the GUI, install the 'joint_state_publisher_gui' package**,need to install other package,input in terminal: +`sudo apt-get install ros-melodic-joint-state-publisher-gui ` + +Click the Fixed Frame under Displays on the left side of RViz to change from map to base_link + +Click the add button in the lower left corner to add the RobotModel + +The robot model is displayed +![image-3](asserts/003.png#pic_center) +
Figure3 Robot Model
+ diff --git a/crt_ctag2f90d_gripper_visualization/docs/README.zh-CN.md b/crt_ctag2f90d_gripper_visualization/docs/README.zh-CN.md new file mode 100644 index 0000000..059c16a --- /dev/null +++ b/crt_ctag2f90d_gripper_visualization/docs/README.zh-CN.md @@ -0,0 +1,59 @@ +# ctag2f90d + +## 一、环境配置说明 + +Ubuntu 18.04.6 LTS +Python 3.6.9 +ROS melodic 1.14.13 + +需要的python package：minimalmodbus +安装minimalmodbus，在终端输入 `pip install minimalmodbus` + +复制 **crt_ctag2f90d_gripper_visualization** 文件夹到你自己的ROS的workspace中 +## 二、使用说明 + +### 1. 在连接好机器手后，确认在Ubuntu有USB端口 +在终端输入： + `ls -l /dev/ttyUSB* ` + 确认USB端口名称后，修改端口使用权限 + `sudo chmod 777 /dev/ttyUSB0` + +### 2. 更新ROS环境 +在终端输入： +```sh +cd ~/ws_catkin + +catkin_make +``` + +### 3. 启动riz节点 + +```sh +# 如果要使用功能，在终端输入： +cd ./src/crt_ctag2f90d_gripper_visualization + +roslaunch crt_ctag2f90d_gripper_visualization display_sync.launch # 同步模式 +``` + +输入完成，会出现rviz界面，如下图： + +![image-1](asserts/001.png) +
图1 ctag2f90d rviz显示图
+ + +![image-2](asserts/002.png) +
图2 joint state publisher显示图
+ +如果出现关于**Could not find the GUI, install the ‘joint_state_publisher_gui‘ package**，需要安装相应的软件包，在终端里输入： +`sudo apt-get install ros-melodic-joint-state-publisher-gui ` + +点击RViz左侧Displays下的Fixed Frame从map改为base_link + +再点击左下角add按钮，添加RobotModel + +显示出机器模型 + +![image-3](asserts/003.png) + +
图3 机器模型图
+ diff --git a/crt_ctag2f90d_gripper_visualization/docs/asserts/001.png b/crt_ctag2f90d_gripper_visualization/docs/asserts/001.png new file mode 100644 index 0000000..e6d29c3 Binary files /dev/null and b/crt_ctag2f90d_gripper_visualization/docs/asserts/001.png differ diff --git a/crt_ctag2f90d_gripper_visualization/docs/asserts/002.png b/crt_ctag2f90d_gripper_visualization/docs/asserts/002.png new file mode 100644 index 0000000..3bbd11d Binary files /dev/null and b/crt_ctag2f90d_gripper_visualization/docs/asserts/002.png differ diff --git a/crt_ctag2f90d_gripper_visualization/docs/asserts/003.png b/crt_ctag2f90d_gripper_visualization/docs/asserts/003.png new file mode 100644 index 0000000..88f9176 Binary files /dev/null and b/crt_ctag2f90d_gripper_visualization/docs/asserts/003.png differ diff --git a/crt_ctag2f90d_gripper_visualization/launch/display.launch b/crt_ctag2f90d_gripper_visualization/launch/display.launch new file mode 100644 index 0000000..276f15a --- /dev/null +++ b/crt_ctag2f90d_gripper_visualization/launch/display.launch @@ -0,0 +1,20 @@ + + + + + + + diff --git a/crt_ctag2f90d_gripper_visualization/meshes/Left_1_Link.STL b/crt_ctag2f90d_gripper_visualization/meshes/Left_1_Link.STL new file mode 100644 index 0000000..415a79c Binary files /dev/null and b/crt_ctag2f90d_gripper_visualization/meshes/Left_1_Link.STL differ diff --git a/crt_ctag2f90d_gripper_visualization/meshes/Left_2_Link.STL b/crt_ctag2f90d_gripper_visualization/meshes/Left_2_Link.STL new file mode 100644 index 0000000..0864c3d Binary files /dev/null and b/crt_ctag2f90d_gripper_visualization/meshes/Left_2_Link.STL differ diff --git a/crt_ctag2f90d_gripper_visualization/meshes/Left_Support_Link.STL b/crt_ctag2f90d_gripper_visualization/meshes/Left_Support_Link.STL new file mode 100644 index 0000000..364e408 Binary files /dev/null and b/crt_ctag2f90d_gripper_visualization/meshes/Left_Support_Link.STL differ diff --git a/crt_ctag2f90d_gripper_visualization/meshes/Right_1_Link.STL b/crt_ctag2f90d_gripper_visualization/meshes/Right_1_Link.STL new file mode 100644 index 0000000..969bdb1 Binary files /dev/null and b/crt_ctag2f90d_gripper_visualization/meshes/Right_1_Link.STL differ diff --git a/crt_ctag2f90d_gripper_visualization/meshes/Right_2_Link.STL b/crt_ctag2f90d_gripper_visualization/meshes/Right_2_Link.STL new file mode 100644 index 0000000..82b16aa Binary files /dev/null and b/crt_ctag2f90d_gripper_visualization/meshes/Right_2_Link.STL differ diff --git a/crt_ctag2f90d_gripper_visualization/meshes/Right_Support_Link.STL b/crt_ctag2f90d_gripper_visualization/meshes/Right_Support_Link.STL new file mode 100644 index 0000000..401115b Binary files /dev/null and b/crt_ctag2f90d_gripper_visualization/meshes/Right_Support_Link.STL differ diff --git a/crt_ctag2f90d_gripper_visualization/meshes/base_link.STL b/crt_ctag2f90d_gripper_visualization/meshes/base_link.STL new file mode 100644 index 0000000..c2b988e Binary files /dev/null and b/crt_ctag2f90d_gripper_visualization/meshes/base_link.STL differ diff --git a/crt_ctag2f90d_gripper_visualization/package.xml b/crt_ctag2f90d_gripper_visualization/package.xml new file mode 100644 index 0000000..619c501 --- /dev/null +++ b/crt_ctag2f90d_gripper_visualization/package.xml @@ -0,0 +1,21 @@ + + crt_ctag2f90d_gripper_visualization + 1.0.0 + +

URDF Description package for crt_ctag2f90d_visualization

+

This package contains configuration data, 3D models and launch files +for crt_ctag2f90d_visualization robot

+ + TODO + + BSD + catkin + roslaunch + robot_state_publisher + rviz + joint_state_publisher_gui + gazebo + + + + diff --git a/crt_ctag2f90d_gripper_visualization/urdf.rviz b/crt_ctag2f90d_gripper_visualization/urdf.rviz new file mode 100644 index 0000000..db28a0d --- /dev/null +++ b/crt_ctag2f90d_gripper_visualization/urdf.rviz @@ -0,0 +1,193 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /TF1 + Splitter Ratio: 0.5 + Tree Height: 1008 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Left_1_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Left_2_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Left_Support_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + Right_1_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Right_2_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Right_Support_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: false + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 0.05000000074505806 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 0.43260106444358826 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.0013686262536793947 + Y: 0.0031668562442064285 + Z: 0.1230432540178299 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6153977513313293 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.530397653579712 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1305 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000047bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000047b000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000047bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000047b000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009bb0000003efc0100000002fb0000000800540069006d00650100000000000009bb000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000074a0000047b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2491 + X: 67 + Y: 27 diff --git a/crt_ctag2f90d_gripper_visualization/urdf/crt_ctag2f90d_gripper_visualization.csv b/crt_ctag2f90d_gripper_visualization/urdf/crt_ctag2f90d_gripper_visualization.csv new file mode 100644 index 0000000..cf6b806 --- /dev/null +++ b/crt_ctag2f90d_gripper_visualization/urdf/crt_ctag2f90d_gripper_visualization.csv @@ -0,0 +1,8 @@ +Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity +base_link,3.84872289645122E-06,-4.41448135187968E-05,0.0553867866771657,0,0,0,0.953455976099042,0.00177628843801648,-8.19676071724033E-10,-3.00472546877836E-08,0.00187418176115802,-3.98009757950365E-07,0.000875448946929789,0,0,0,0,0,0,package://crt_ctag2f90d_visualization/meshes/base_link.STL,0.615686274509804,0.63921568627451,0.674509803921569,1,0,0,0,0,0,0,package://crt_ctag2f90d_visualization/meshes/base_link.STL,,CTAG2F90D 90D两指协作手-机体11-1,坐标系0,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,, +Left_1_Link,0.0202604071221892,0.0187577877882694,2.20960470526592E-14,0,0,0,0.0309052898336316,4.31215511677353E-06,-4.26885891149938E-06,-2.46366459463114E-13,4.9707522189533E-06,-2.22956217486463E-13,9.19223373728405E-06,0,0,0,0,0,0,package://crt_ctag2f90d_visualization/meshes/Left_1_Link.STL,0.615686274509804,0.63921568627451,0.674509803921569,1,0,0,0,0,0,0,package://crt_ctag2f90d_visualization/meshes/Left_1_Link.STL,,CTAG2F90D 90D两指协作手-连杆1-3,坐标系11,基准轴11,Left_1_Joint,revolute,0.030141,0,0.10415,1.5708,0,0,base_link,0,0,-1,10,10,-1,0,,,,,,,, +Left_Support_Link,-0.0177925864232372,0.0354308985297288,-3.45319167880073E-05,0,0,0,0.0828693338705683,2.73957553511085E-05,7.04281105447926E-06,-1.08446711519888E-11,6.34802949004157E-06,1.18385736891846E-11,2.83129237821216E-05,0,0,0,0,0,0,package://crt_ctag2f90d_visualization/meshes/Left_Support_Link.STL,0.615686274509804,0.63921568627451,0.674509803921569,1,0,0,0,0,0,0,package://crt_ctag2f90d_visualization/meshes/Left_Support_Link.STL,,CTAG2F90D 90D两指协作手-指端1-2,坐标系12,基准轴12,Left_Support_Joint,revolute,0.040359,0.037365,0,0,0,0,Left_1_Link,0,0,-1,0,0,0,0,,,,,,,, +Left_2_Link,0.0202604071221829,0.0187577877878552,2.21631403129407E-14,0,0,0,0.0309052898336314,4.31215511674771E-06,-4.26885891149736E-06,-2.46366467847417E-13,4.97075221897906E-06,-2.22956225772745E-13,9.19223373728399E-06,0,0,0,0,0,0,package://crt_ctag2f90d_visualization/meshes/Left_2_Link.STL,0.615686274509804,0.63921568627451,0.674509803921569,1,0,0,0,0,0,0,package://crt_ctag2f90d_visualization/meshes/Left_2_Link.STL,,CTAG2F90D 90D两指协作手-连杆1-4,坐标系21,基准轴21,Left_2_Joint,revolute,0.016,0,0.121,1.5708,0,0,base_link,0,0,-1,0,0,0,0,,,,,,,, +Right_2_Link,0.0202604071211299,0.0187577877883374,2.11370080968728E-14,0,0,0,0.0309052898336316,4.31215511674775E-06,-4.2688589114974E-06,-2.46366382911271E-13,4.9707522189791E-06,-2.22956145920037E-13,9.19223373728407E-06,0,0,0,0,0,0,package://crt_ctag2f90d_visualization/meshes/Right_2_Link.STL,0.615686274509804,0.63921568627451,0.674509803921569,1,0,0,0,0,0,0,package://crt_ctag2f90d_visualization/meshes/Right_2_Link.STL,,CTAG2F90D 90D两指协作手-连杆1-2,坐标系31,基准轴31,Right_2_Joint,revolute,-0.016,0,0.121,1.5708,0,3.1416,base_link,0,0,1,0,0,0,0,,,,,,,, +Right_1_Link,0.0202604071211989,0.0187577877891044,2.11409547592425E-14,0,0,0,0.0309052898336314,4.3121551167735E-06,-4.26885891149935E-06,-2.46366386343243E-13,4.97075221895327E-06,-2.22956149418598E-13,9.19223373728398E-06,0,0,0,0,0,0,package://crt_ctag2f90d_visualization/meshes/Right_1_Link.STL,0.615686274509804,0.63921568627451,0.674509803921569,1,0,0,0,0,0,0,package://crt_ctag2f90d_visualization/meshes/Right_1_Link.STL,,CTAG2F90D 90D两指协作手-连杆1-1,坐标系41,基准轴41,Right_1_Joint,revolute,-0.030141,0,0.10415,1.5708,0,3.1416,base_link,0,0,1,0,0,0,0,,,,,,,, +Right_Support_Link,-0.0177925878352495,0.0354308970643828,-2.81887513877937E-08,0,0,0,0.082869359674167,2.73957568837024E-05,7.04281063834988E-06,-9.62685216616108E-12,6.34803242941661E-06,1.28138996747735E-11,2.83129239152281E-05,0,0,0,0,0,0,package://crt_ctag2f90d_visualization/meshes/Right_Support_Link.STL,0.615686274509804,0.63921568627451,0.674509803921569,1,0,0,0,0,0,0,package://crt_ctag2f90d_visualization/meshes/Right_Support_Link.STL,,CTAG2F90D 90D两指协作手-指端1-1,坐标系42,基准轴42,Right_Support_Joint,revolute,0.040359,0.037365,0,0,0,0,Right_1_Link,0,0,-1,0,0,0,0,,,,,,,, diff --git a/crt_ctag2f90d_gripper_visualization/urdf/crt_ctag2f90d_gripper_visualization_sync.urdf b/crt_ctag2f90d_gripper_visualization/urdf/crt_ctag2f90d_gripper_visualization_sync.urdf new file mode 100644 index 0000000..810d871 --- /dev/null +++ b/crt_ctag2f90d_gripper_visualization/urdf/crt_ctag2f90d_gripper_visualization_sync.urdf @@ -0,0 +1,430 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gripper/control/controller.py b/gripper/control/controller.py index 39f873d..bdc9a47 100644 --- a/gripper/control/controller.py +++ b/gripper/control/controller.py @@ -23,15 +23,41 @@ class SideTriggerGripController: self._stop_event = threading.Event() self._manual_lock = threading.Lock() self._manual_command = None + self._mode_lock = threading.Lock() + self._control_mode = "smart" def stop(self): self._stop_event.set() - def request_manual_command(self, command): - if command not in {"open", "close", "hold", "release"}: + def request_manual_command(self, command, speed_pct=None, force_pct=None, position=None): + if command not in { + "open", + "close", + "hold", + "release", + "move_position", + "restart_left_sensor", + "restart_right_sensor", + }: raise ValueError(f"unsupported manual command: {command}") + payload = { + "command": command, + "speed_pct": speed_pct, + "force_pct": force_pct, + "position": position, + } with self._manual_lock: - self._manual_command = command + self._manual_command = payload + + def set_control_mode(self, mode): + if mode not in {"smart", "manual"}: + raise ValueError(f"unsupported control mode: {mode}") + with self._mode_lock: + self._control_mode = mode + + def _get_control_mode(self): + with self._mode_lock: + return self._control_mode def _pop_manual_command(self): with self._manual_lock: @@ -134,6 +160,45 @@ class SideTriggerGripController: release_armed = False hold_pos = None reference = None + current_position = None + last_position_status_time = 0.0 + position_perf = { + "last_log": time.perf_counter(), + "count": 0, + "total": 0.0, + "max": 0.0, + } + + def update_current_position(now, force=False): + nonlocal current_position, last_position_status_time, position_perf + if force or now - last_position_status_time >= 0.1: + last_position_status_time = now + t0 = time.perf_counter() + current_position = self.gripper.read_position() + dt = time.perf_counter() - t0 + position_perf["count"] += 1 + position_perf["total"] += dt + position_perf["max"] = max(position_perf["max"], dt) + if now - position_perf["last_log"] >= 1.0: + avg_ms = ( + position_perf["total"] / position_perf["count"] * 1000.0 + if position_perf["count"] + else 0.0 + ) + print( + "[perf] position_read " + f"count={position_perf['count']} " + f"avg={avg_ms:.2f}ms " + f"max={position_perf['max'] * 1000.0:.2f}ms " + f"pos={current_position}" + ) + position_perf = { + "last_log": now, + "count": 0, + "total": 0.0, + "max": 0.0, + } + return current_position def reset_open_wait_stability(): nonlocal open_wait_samples, open_wait_stable_values @@ -144,15 +209,26 @@ class SideTriggerGripController: tick_start = time.perf_counter() now = time.perf_counter() manual_action = None + control_mode = self._get_control_mode() - manual_command = self._pop_manual_command() + manual_payload = self._pop_manual_command() + if isinstance(manual_payload, dict): + manual_command = manual_payload.get("command") + manual_speed_pct = manual_payload.get("speed_pct") + manual_force_pct = manual_payload.get("force_pct") + manual_position = manual_payload.get("position") + else: + manual_command = manual_payload + manual_speed_pct = None + manual_force_pct = None + manual_position = None if manual_command == "open": force_pct = int(clamp( - config.open_force, + config.open_force if manual_force_pct is None else manual_force_pct, config.force_min, config.force_max, )) - self.gripper.open(force_pct, "manual-open") + self.gripper.open(force_pct, "manual-open", speed_pct=manual_speed_pct) self.feedback_processor.reset_filters() state = "open_recover" state_start = now @@ -170,11 +246,11 @@ class SideTriggerGripController: manual_action = "manual-open" elif manual_command == "close": force_pct = int(clamp( - config.close_force, + config.close_force if manual_force_pct is None else manual_force_pct, config.force_min, config.force_max, )) - self.gripper.close(force_pct, "manual-close") + self.gripper.close(force_pct, "manual-close", speed_pct=manual_speed_pct) state = "manual_close" state_start = now last_force_ramp_time = now @@ -192,12 +268,15 @@ class SideTriggerGripController: manual_action = "manual-close" elif manual_command == "hold": force_pct = int(clamp( - config.hold_force, + config.hold_force if manual_force_pct is None else manual_force_pct, config.force_min, config.force_max, )) self.gripper.set_force(force_pct) - hold_pos = self.gripper.hold_current_position(force_pct) + hold_pos = self.gripper.hold_current_position( + force_pct, + speed_pct=manual_speed_pct, + ) state = "manual_hold" state_start = now last_force_ramp_time = now @@ -218,10 +297,62 @@ class SideTriggerGripController: hold_pos = None reference = None manual_action = "manual-release" + elif manual_command == "move_position": + target_min = min(int(config.open_pos), int(config.close_pos)) + target_max = max(int(config.open_pos), int(config.close_pos)) + target_position = int(clamp( + int(manual_position if manual_position is not None else config.open_pos), + target_min, + target_max, + )) + force_pct = int(clamp( + config.close_force if manual_force_pct is None else manual_force_pct, + config.force_min, + config.force_max, + )) + self.gripper.move( + target_position, + force_pct, + "manual-move-position", + speed_pct=manual_speed_pct, + ) + state = "manual_position" + state_start = now + last_force_ramp_time = now + last_open_position_check_time = 0.0 + last_open_position = target_position + last_open_position_ready = False + recover_quiet_since = None + recover_position_since = None + trigger_clear_since = None + trigger_armed = False + reset_open_wait_stability() + release_armed = False + hold_pos = None + reference = None + current_position = target_position + manual_action = "manual-move-position" + elif manual_command == "restart_left_sensor": + if self.reader.restart_sensor(0): + reset_open_wait_stability() + trigger_clear_since = None + trigger_armed = False + manual_action = "left-sensor-restart" + else: + manual_action = "left-sensor-restart-failed" + elif manual_command == "restart_right_sensor": + if self.reader.restart_sensor(1): + reset_open_wait_stability() + trigger_clear_since = None + trigger_armed = False + manual_action = "right-sensor-restart" + else: + manual_action = "right-sensor-restart-failed" feedback = self.feedback_processor.read(self.reader, timeout=0.3) if feedback is None: action = manual_action or "waiting-for-sensor-samples" + position_for_status = update_current_position(now) print(f"state={state} action={action}") self._emit_status( { @@ -233,7 +364,11 @@ class SideTriggerGripController: "adaptive_grip_enabled": config.adaptive_grip_enabled, "release_armed": release_armed, "manual_hold_active": state == "manual_hold", + "control_mode": control_mode, "speed_pct": config.speed, + "gripper_position": position_for_status, + "open_pos": config.open_pos, + "close_pos": config.close_pos, "normal_unit": self.normal_unit, "shear_unit": self.shear_unit, } @@ -249,6 +384,8 @@ class SideTriggerGripController: if manual_action is not None: pass + elif control_mode == "manual" and state not in {"manual_hold", "manual_close", "manual_position"}: + action = "manual-standby" elif state == "open_wait": open_wait_sample = self._open_wait_sample(feedback) self._append_open_wait_sample(open_wait_samples, open_wait_sample) @@ -506,6 +643,9 @@ class SideTriggerGripController: elif state == "manual_close": action = "manual-close" + elif state == "manual_position": + action = "manual-position" + self._log_tick( feedback=feedback, state=state, @@ -520,6 +660,7 @@ class SideTriggerGripController: action=action, trigger_armed=trigger_armed, ) + position_for_status = update_current_position(now) self._emit_status( self._make_status( feedback=feedback, @@ -534,6 +675,8 @@ class SideTriggerGripController: release_armed=release_armed, action=action, trigger_armed=trigger_armed, + control_mode=control_mode, + gripper_position=position_for_status, ) ) last_trigger = trigger @@ -713,6 +856,8 @@ class SideTriggerGripController: release_armed, action, trigger_armed, + control_mode="smart", + gripper_position=None, ): speed_pct = self.config.speed if action == "force-change-release-open": @@ -755,7 +900,11 @@ class SideTriggerGripController: "adaptive_grip_enabled": self.config.adaptive_grip_enabled, "release_armed": bool(release_armed), "manual_hold_active": state == "manual_hold", + "control_mode": control_mode, "speed_pct": speed_pct, + "gripper_position": gripper_position, + "open_pos": self.config.open_pos, + "close_pos": self.config.close_pos, "normal_unit": self.normal_unit, "shear_unit": self.shear_unit, "left_sample": left_sample, diff --git a/gripper/hardware/gripper_client.py b/gripper/hardware/gripper_client.py index 236e764..52bedea 100644 --- a/gripper/hardware/gripper_client.py +++ b/gripper/hardware/gripper_client.py @@ -6,6 +6,7 @@ class GripperClient: def __init__(self, config): self.config = config self.motor = None + self.last_commanded_position = None def connect(self): if self.config.dry_run: @@ -24,6 +25,7 @@ class GripperClient: def move(self, position, force_pct, label, speed_pct=None, accel=None, decel=None): config = self.config + self.last_commanded_position = int(position) speed_pct = config.speed if speed_pct is None else speed_pct accel = config.accel if accel is None else accel decel = config.decel if decel is None else decel @@ -60,8 +62,15 @@ class GripperClient: decel=decel, ) - def close(self, force_pct, label="close"): - self.move(self.config.close_pos, force_pct, label) + def close(self, force_pct, label="close", speed_pct=None, accel=None, decel=None): + self.move( + self.config.close_pos, + force_pct, + label, + speed_pct=speed_pct, + accel=accel, + decel=decel, + ) def set_force(self, force_pct): config = self.config @@ -77,14 +86,17 @@ class GripperClient: def read_position(self): if self.config.dry_run: - return None + return self.last_commanded_position try: - return self.motor.read_real_position() + position = self.motor.read_real_position() + if position is not None: + return int(position) + return self.last_commanded_position except Exception as exc: print(f"read_real_position failed: {exc}") - return None + return self.last_commanded_position - def hold_current_position(self, force_pct): + def hold_current_position(self, force_pct, speed_pct=None, accel=None, decel=None): current_pos = self.read_position() if current_pos is None: current_pos = self.config.hold_fallback_pos @@ -94,7 +106,14 @@ class GripperClient: self._stop_by_speed() return None - self.move(int(current_pos), force_pct, "clamp-hold") + self.move( + int(current_pos), + force_pct, + "clamp-hold", + speed_pct=speed_pct, + accel=accel, + decel=decel, + ) return int(current_pos) def final_open_if_needed(self): diff --git a/gripper/logo.png b/gripper/logo.png new file mode 100644 index 0000000..9679888 Binary files /dev/null and b/gripper/logo.png differ diff --git a/gripper/logo_topbar.png b/gripper/logo_topbar.png new file mode 100644 index 0000000..449121c Binary files /dev/null and b/gripper/logo_topbar.png differ diff --git a/gripper/sensors/force_reader.py b/gripper/sensors/force_reader.py index 0ec328e..9eb7b29 100644 --- a/gripper/sensors/force_reader.py +++ b/gripper/sensors/force_reader.py @@ -316,29 +316,55 @@ class TwoSensorForceReader: self.result_queues = [Queue(maxsize=self.queue_size) for _ in self.sensor_configs] self.stop_events = [mp.Event() for _ in self.sensor_configs] + self.processes = [None] * len(self.sensor_configs) self.latest = [None] * len(self.sensor_configs) - for idx, config in enumerate(self.sensor_configs): - process = Process( - target=_sensor_force_worker, - args=( - config["sensor_id"], - config["vid_src"], - config["config_name"], - config["rotation_config"], - self.result_queues[idx], - self.stop_events[idx], - self.target_fps, - self.cuda, - self.isstitch, - self.backend, - self.motion_threshold, - self.include_visuals, - self.visual_size, - ), - ) - process.start() - self.processes.append(process) + for idx in range(len(self.sensor_configs)): + self._start_sensor_process(idx) + + def _start_sensor_process(self, idx): + config = self.sensor_configs[idx] + process = Process( + target=_sensor_force_worker, + args=( + config["sensor_id"], + config["vid_src"], + config["config_name"], + config["rotation_config"], + self.result_queues[idx], + self.stop_events[idx], + self.target_fps, + self.cuda, + self.isstitch, + self.backend, + self.motion_threshold, + self.include_visuals, + self.visual_size, + ), + ) + process.start() + self.processes[idx] = process + + def restart_sensor(self, idx): + if idx < 0 or idx >= len(self.sensor_configs): + raise ValueError(f"invalid sensor index: {idx}") + if not self.processes: + return False + + old_event = self.stop_events[idx] + old_process = self.processes[idx] + old_event.set() + if old_process is not None: + old_process.join(timeout=2.0) + if old_process.is_alive(): + old_process.terminate() + old_process.join(timeout=1.0) + + self.result_queues[idx] = Queue(maxsize=self.queue_size) + self.stop_events[idx] = mp.Event() + self.latest[idx] = None + self._start_sensor_process(idx) + return True def update(self): for idx, result_queue in enumerate(self.result_queues): @@ -381,6 +407,8 @@ class TwoSensorForceReader: event.set() for process in self.processes: + if process is None: + continue process.join(timeout=2.0) if process.is_alive(): process.terminate() diff --git a/gripper/ui/visualizer.py b/gripper/ui/visualizer.py index bcb3a32..561d09b 100644 --- a/gripper/ui/visualizer.py +++ b/gripper/ui/visualizer.py @@ -1,8 +1,14 @@ import argparse +import json +import math import multiprocessing as mp +import os +from pathlib import Path +import struct import sys import threading import time +import xml.etree.ElementTree as ET import cv2 import numpy as np @@ -25,29 +31,43 @@ _fix_qt_plugin_path() _QT_IMPORT_ERROR = None try: - from PyQt5.QtCore import QEasingCurve, QPropertyAnimation, Qt, QTimer - from PyQt5.QtGui import QFont, QImage, QPixmap + from PyQt5.QtCore import QEasingCurve, QEvent, QPointF, QRectF, QSize, QPropertyAnimation, Qt, QTimer + from PyQt5.QtGui import QColor, QFont, QIcon, QImage, QLinearGradient, QPainter, QPen, QPixmap, QPolygonF, QRadialGradient from PyQt5.QtWidgets import ( QApplication, + QButtonGroup, + QCheckBox, QDialog, QFrame, QGraphicsOpacityEffect, QGridLayout, QHBoxLayout, QLabel, + QLineEdit, + QListWidget, + QMessageBox, QMainWindow, QPushButton, + QProgressBar, + QScrollArea, + QSizeGrip, QSizePolicy, + QSlider, + QSpinBox, + QStackedWidget, + QTextEdit, QSplitter, QVBoxLayout, QWidget, ) except Exception as exc: _QT_IMPORT_ERROR = exc - Qt = QTimer = QPropertyAnimation = QEasingCurve = QFont = QImage = QPixmap = QApplication = None - QGridLayout = QHBoxLayout = QLabel = QPushButton = QSizePolicy = QSplitter = None + Qt = QTimer = QPropertyAnimation = QEasingCurve = QEvent = QPointF = QRectF = QSize = QFont = QIcon = QImage = QPixmap = QApplication = None + QColor = QLinearGradient = QPainter = QPen = QPolygonF = QRadialGradient = None + QGridLayout = QHBoxLayout = QLabel = QPushButton = QSizeGrip = QSizePolicy = QSplitter = None QVBoxLayout = QWidget = None - QDialog = QFrame = QGraphicsOpacityEffect = QMainWindow = object + QButtonGroup = QCheckBox = QDialog = QFrame = QGraphicsOpacityEffect = QLineEdit = QListWidget = QMessageBox = QMainWindow = object + QProgressBar = QScrollArea = QSlider = QSpinBox = QStackedWidget = QTextEdit = None try: import pyqtgraph as pg @@ -57,16 +77,27 @@ except Exception as exc: else: _PYQTGRAPH_IMPORT_ERROR = None +try: + import pyqtgraph.opengl as gl +except Exception as exc: + gl = None + _PYQTGRAPH_OPENGL_IMPORT_ERROR = exc +else: + _PYQTGRAPH_OPENGL_IMPORT_ERROR = None + from ..app import build_controller +from ..common.paths import PACKAGE_DIR, REPO_ROOT 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 +PLOT_Y_DEFAULT_MIN_N = 0.0 +PLOT_Y_DEFAULT_MAX_N = 1.0 +PLOT_FONT_FAMILY = "Microsoft YaHei" +PLOT_TITLE_FONT_SIZE = 9 +PLOT_AXIS_FONT_SIZE = 8 FORCE_DISPLAY_ZERO_THRESHOLD_N = 0.05 -CURVE_COLOR = "#00E5FF" +CURVE_COLOR = "#536dce" +PLOT_SMOOTH_ALPHA = 0.28 def cv2_to_pixmap(img_bgr): @@ -109,6 +140,22 @@ def force_display_value(value): return value +def plot_force_value(value): + return max(0.0, force_display_value(value)) + + +def smooth_plot_values(values, alpha=PLOT_SMOOTH_ALPHA): + values = np.asarray(values, dtype=float) + if values.size <= 1: + return values + alpha = max(0.0, min(1.0, float(alpha))) + smoothed = np.empty_like(values, dtype=float) + smoothed[0] = values[0] + for idx in range(1, values.size): + smoothed[idx] = alpha * values[idx] + (1.0 - alpha) * smoothed[idx - 1] + return smoothed + + def fmt(value, unit="", width=0, precision=3): if value is None: text = "--" @@ -128,6 +175,7 @@ STATE_TEXT = { "open_recover": "张开恢复", "manual_close": "手动闭合", "manual_hold": "手动保持", + "manual_position": "手动到位", "error": "错误", } @@ -161,6 +209,13 @@ ACTION_TEXT = { "manual-close": "手动闭合", "manual-hold": "手动保持", "manual-release": "手动释放", + "manual-position": "指定位置保持", + "manual-move-position": "移动到指定位置", + "manual-standby": "手动待机", + "left-sensor-restart": "左传感器重启", + "right-sensor-restart": "右传感器重启", + "left-sensor-restart-failed": "左传感器重启失败", + "right-sensor-restart-failed": "右传感器重启失败", } @@ -179,142 +234,278 @@ def label_with_cn(value, mapping): STYLE = """ QMainWindow { - background-color: #000000; + background-color: #f5f7fa; } QWidget { - background-color: #000000; - color: #9a9a9a; + background-color: #f5f7fa; + color: #1f2933; font-family: "Microsoft YaHei", "SimHei"; } -QLabel#Title { - color: #d7d7d7; +QLabel { + background-color: transparent; +} +QFrame#TopBar { + background-color: #ffffff; + border: 1px solid #e3e8ef; + border-radius: 6px; +} +QLabel#BrandMark { + color: #3a9b45; font-size: 18px; - font-weight: 600; + font-weight: 800; +} +QLabel#Title { + color: #111827; + font-size: 17px; + font-weight: 700; } QLabel#Subtle { - color: #777777; + color: #667085; font-size: 12px; } +QLabel#SectionTitle { + color: #111827; + font-size: 13px; + font-weight: 700; +} QLabel#MetricLabel { - color: #888888; - font-size: 12px; + color: #667085; + font-size: 11px; } QLabel#MetricValue { - color: #f2f2f2; - font-family: "Cascadia Code", "Consolas"; - font-size: 22px; - font-weight: 600; -} -QLabel#SmallValue { - color: #d8d8d8; - font-family: "Cascadia Code", "Consolas"; - font-size: 15px; -} -QLabel#StatusChip { - background-color: #101010; - border: 1px solid #303030; - border-radius: 4px; - color: #00e5ff; - font-family: "Cascadia Code", "Consolas"; - font-size: 18px; - font-weight: 600; - padding: 8px 12px; -} -QFrame#StatusPanel { - background-color: #101010; - border: 1px solid #303030; - border-radius: 4px; -} -QLabel#StatusMain { - color: #00e5ff; + color: #111827; font-family: "Cascadia Code", "Consolas"; font-size: 17px; - font-weight: 600; + font-weight: 700; +} +QLabel#SmallValue { + color: #344054; + font-family: "Cascadia Code", "Consolas"; + font-size: 13px; +} +QFrame#StatusPanel { + background-color: #f8fafc; + border: 1px solid #e3e8ef; + border-radius: 6px; +} +QLabel#StatusMain { + color: #0369a1; + font-family: "Cascadia Code", "Consolas"; + font-size: 14px; + font-weight: 700; } QLabel#StatusSub { - color: #9a9a9a; + color: #667085; font-family: "Cascadia Code", "Consolas"; - font-size: 12px; -} -QLabel#ActionChip { - background-color: #0b0b0b; - border: 1px solid #262626; - border-radius: 4px; - color: #c8c8c8; - font-family: "Cascadia Code", "Consolas"; - font-size: 13px; - padding: 7px 10px; + font-size: 11px; } QLabel#ImagePanel { - background-color: #0b0b0b; - border: 1px solid #262626; - border-radius: 4px; + background-color: #ffffff; + border: 1px solid #e3e8ef; + border-radius: 8px; } QFrame#Panel { - background-color: #050505; - border: 1px solid #222222; - border-radius: 4px; + background-color: #ffffff; + border: 1px solid #e3e8ef; + border-radius: 6px; +} +QFrame#DarkPanel { + background-color: #ffffff; + border: 1px solid #e3e8ef; + border-radius: 6px; +} +QFrame#GripperStage { + background-color: #ffffff; + border: 1px solid #e3e8ef; + border-radius: 8px; +} +QFrame#ControlPanel { + background-color: #ffffff; + border: 1px solid #e3e8ef; + border-radius: 6px; +} +QDialog#SettingsPanel { + background-color: #ffffff; + border: 1px solid #d7dee8; + border-radius: 8px; } QPushButton { - background-color: #171717; - color: #d8d8d8; - border: 1px solid #363636; - border-radius: 4px; - padding: 7px 16px; - font-size: 13px; + background-color: #ffffff; + color: #1f2933; + border: 1px solid #d7dee8; + border-radius: 5px; + padding: 7px 14px; + font-size: 12px; } QPushButton:hover { - background-color: #242424; - color: #ffffff; - border: 1px solid #5a5a5a; + background-color: #f2f4f7; + border: 1px solid #b8c2cc; } QPushButton:pressed { - background-color: #0f0f0f; - border: 1px solid #8a8a8a; + background-color: #e9edf3; padding-top: 8px; padding-bottom: 6px; } QPushButton:disabled { - background-color: #101010; - color: #606060; - border: 1px solid #242424; + background-color: #f3f5f8; + color: #98a2b3; + border: 1px solid #e3e8ef; } QPushButton#StartButton { - background-color: #171717; - color: #d8d8d8; - border: 1px solid #363636; + background-color: #ffffff; + color: #157347; + border: 1px solid #b7dec6; + font-weight: 700; } QPushButton#StopButton { - background-color: #171717; - color: #d8d8d8; - border: 1px solid #363636; + background-color: #ffffff; + color: #b42318; + border: 1px solid #f1b8b4; + font-weight: 700; } QPushButton#RestartButton { - background-color: #171717; - color: #d8d8d8; - border: 1px solid #363636; + background-color: #ffffff; + color: #175cd3; + border: 1px solid #b7cdf7; + font-weight: 700; } -QPushButton#ManualButton { - background-color: #171717; - color: #d8d8d8; - border: 1px solid #363636; +QPushButton#TopIconButton { + background-color: #ffffff; + border: 1px solid #d7dee8; + border-radius: 6px; + padding: 0; + min-width: 30px; + max-width: 30px; + min-height: 30px; + max-height: 30px; } -QPushButton#HoldButton { - background-color: #171717; - color: #d8d8d8; - border: 1px solid #363636; +QPushButton#TopIconButton:hover { + background-color: #f2f4f7; + border: 1px solid #aeb9c8; +} +QPushButton#TopIconButton:pressed { + background-color: #e9edf3; + padding: 0; +} +QPushButton#TopIconButton:disabled { + background-color: #f7f9fc; + border: 1px solid #e3e8ef; +} +QPushButton#ManualButton, QPushButton#HoldButton { + background-color: #ffffff; + min-width: 72px; +} +QPushButton#ViewToggle { + background-color: #ffffff; + padding: 5px 10px; +} +QPushButton#ViewToggle:checked { + background-color: #eff6ff; + color: #175cd3; + border: 1px solid #9fc0f5; + font-weight: 700; +} +QPushButton#PositionButton { + background-color: #ffffff; + padding: 4px 6px; + min-width: 40px; +} +QPushButton#StepButton { + background-color: #ffffff; + color: #344054; + border: 1px solid #d7dee8; + border-radius: 4px; + padding: 2px 6px; + min-width: 38px; + max-height: 22px; +} +QPushButton#StepButton:hover { + background-color: #fff1f1; + border: 1px solid #f4a7a7; +} +QProgressBar { + background-color: #eef2f6; + border: 1px solid #d7dee8; + border-radius: 4px; + color: #344054; + text-align: center; + height: 14px; +} +QProgressBar::chunk { + background-color: #d92d20; + border-radius: 3px; +} +QSlider::groove:horizontal { + height: 9px; + background: #eef2f6; + border: 1px solid #d7dee8; + border-radius: 5px; +} +QSlider::sub-page:horizontal { + background: qlineargradient(x1:0, y1:0, x2:1, y2:0, stop:0 #ef4444, stop:1 #b91c1c); + border: 1px solid #ef4444; + border-radius: 5px; +} +QSlider::add-page:horizontal { + background: #eef2f6; + border-radius: 5px; +} +QSlider::handle:horizontal { + background: #ffffff; + border: 2px solid #dc2626; + width: 17px; + margin: -6px 0; + border-radius: 9px; +} +QSpinBox { + background-color: #ffffff; + color: #1f2933; + border: 1px solid #d7dee8; + border-radius: 4px; + padding: 3px 5px; +} +QLineEdit, QTextEdit { + background-color: #ffffff; + color: #1f2933; + border: 1px solid #d7dee8; + border-radius: 4px; + padding: 5px 7px; + font-family: "Cascadia Code", "Consolas"; + font-size: 12px; +} +QLineEdit:focus, QTextEdit:focus { + border: 1px solid #7aa7e8; +} +QListWidget { + background-color: #f8fafc; + border: 1px solid #e3e8ef; + border-radius: 6px; + padding: 4px; +} +QListWidget::item { + color: #344054; + padding: 8px 10px; + border-radius: 4px; +} +QListWidget::item:selected { + background-color: #eff6ff; + color: #175cd3; +} +QCheckBox { + color: #344054; + spacing: 8px; } QSplitter::handle { - background-color: #151515; + background-color: #eef2f6; } QSplitter::handle:hover { - background-color: #2a2a2a; + background-color: #d7dee8; } QSplitter::handle:horizontal { - width: 8px; + width: 6px; } QSplitter::handle:vertical { - height: 8px; + height: 6px; } """ @@ -349,13 +540,175 @@ else: AnimatedButton = None +def _draw_top_icon(kind, color="#344054", size=22): + pixmap = QPixmap(size, size) + pixmap.fill(Qt.transparent) + painter = QPainter(pixmap) + painter.setRenderHint(QPainter.Antialiasing, True) + pen = QPen(QColor(color), 2) + pen.setCapStyle(Qt.RoundCap) + pen.setJoinStyle(Qt.RoundJoin) + painter.setPen(pen) + painter.setBrush(Qt.NoBrush) + + if kind == "start": + points = QPolygonF( + [ + QPointF(size * 0.38, size * 0.28), + QPointF(size * 0.38, size * 0.72), + QPointF(size * 0.72, size * 0.50), + ] + ) + painter.setBrush(QColor(color)) + painter.setPen(Qt.NoPen) + painter.drawPolygon(points) + elif kind == "stop": + painter.setBrush(QColor(color)) + painter.setPen(Qt.NoPen) + side = size * 0.38 + painter.drawRoundedRect( + QRectF((size - side) / 2, (size - side) / 2, side, side), + 2, + 2, + ) + elif kind == "restart": + painter.drawArc(QRectF(size * 0.25, size * 0.25, size * 0.50, size * 0.50), 35 * 16, 285 * 16) + arrow = QPolygonF( + [ + QPointF(size * 0.74, size * 0.25), + QPointF(size * 0.78, size * 0.43), + QPointF(size * 0.61, size * 0.37), + ] + ) + painter.setBrush(QColor(color)) + painter.drawPolygon(arrow) + elif kind == "settings": + center = QPointF(size * 0.50, size * 0.50) + painter.drawEllipse(center, size * 0.13, size * 0.13) + for idx in range(8): + angle = math.tau * idx / 8.0 + inner = QPointF( + center.x() + math.cos(angle) * size * 0.29, + center.y() + math.sin(angle) * size * 0.29, + ) + outer = QPointF( + center.x() + math.cos(angle) * size * 0.39, + center.y() + math.sin(angle) * size * 0.39, + ) + painter.drawLine(inner, outer) + elif kind == "minimize": + painter.drawLine( + QPointF(size * 0.32, size * 0.62), + QPointF(size * 0.68, size * 0.62), + ) + elif kind == "fullscreen": + inset = size * 0.30 + outer = size * 0.70 + painter.drawLine(QPointF(inset, inset), QPointF(size * 0.45, inset)) + painter.drawLine(QPointF(inset, inset), QPointF(inset, size * 0.45)) + painter.drawLine(QPointF(outer, inset), QPointF(size * 0.55, inset)) + painter.drawLine(QPointF(outer, inset), QPointF(outer, size * 0.45)) + painter.drawLine(QPointF(inset, outer), QPointF(size * 0.45, outer)) + painter.drawLine(QPointF(inset, outer), QPointF(inset, size * 0.55)) + painter.drawLine(QPointF(outer, outer), QPointF(size * 0.55, outer)) + painter.drawLine(QPointF(outer, outer), QPointF(outer, size * 0.55)) + elif kind == "close": + painter.drawLine( + QPointF(size * 0.34, size * 0.34), + QPointF(size * 0.66, size * 0.66), + ) + painter.drawLine( + QPointF(size * 0.66, size * 0.34), + QPointF(size * 0.34, size * 0.66), + ) + + painter.end() + return QIcon(pixmap) + + +def _draw_company_logo(size=34): + pixmap = QPixmap(size, size) + pixmap.fill(Qt.transparent) + painter = QPainter(pixmap) + painter.setRenderHint(QPainter.Antialiasing, True) + + painter.setPen(Qt.NoPen) + painter.setBrush(QColor("#e7f6ec")) + painter.drawRoundedRect(QRectF(1, 1, size - 2, size - 2), 9, 9) + + pen = QPen(QColor("#2f9e44"), 3) + pen.setCapStyle(Qt.RoundCap) + pen.setJoinStyle(Qt.RoundJoin) + painter.setPen(pen) + painter.setBrush(Qt.NoBrush) + painter.drawArc(QRectF(size * 0.24, size * 0.22, size * 0.52, size * 0.52), 35 * 16, 275 * 16) + painter.drawLine(QPointF(size * 0.50, size * 0.55), QPointF(size * 0.72, size * 0.76)) + + painter.setPen(Qt.NoPen) + painter.setBrush(QColor("#5bbf6a")) + leaf = QPolygonF( + [ + QPointF(size * 0.58, size * 0.28), + QPointF(size * 0.78, size * 0.24), + QPointF(size * 0.72, size * 0.44), + ] + ) + painter.drawPolygon(leaf) + painter.end() + return pixmap + + +def _load_company_logo(size=34): + for path in ( + PACKAGE_DIR / "logo_topbar.png", + PACKAGE_DIR / "logo.png", + REPO_ROOT / "gripper" / "logo_topbar.png", + REPO_ROOT / "gripper" / "logo.png", + ): + try: + if not path.exists(): + continue + pixmap = QPixmap(str(path)) + if pixmap.isNull(): + continue + return pixmap.scaled( + size, + size, + Qt.KeepAspectRatio, + Qt.SmoothTransformation, + ) + except Exception: + continue + return _draw_company_logo(size) + + +def _load_company_icon(size=64): + return QIcon(_load_company_logo(size)) + + +if QPushButton is not None: + class TopIconButton(QPushButton): + def __init__(self, icon_name, tooltip): + super().__init__("") + self.setObjectName("TopIconButton") + self.setFixedSize(30, 30) + self.setIcon(_draw_top_icon(icon_name)) + self.setIconSize(QSize(22, 22)) + self.setToolTip(tooltip) + self.setToolTipDuration(3500) + self.setAccessibleName(tooltip) + self.setCursor(Qt.PointingHandCursor) +else: + TopIconButton = None + + class MetricBox(QFrame): def __init__(self, title, value="--"): super().__init__() self.setObjectName("Panel") layout = QVBoxLayout(self) - layout.setContentsMargins(10, 8, 10, 8) - layout.setSpacing(4) + layout.setContentsMargins(8, 5, 8, 5) + layout.setSpacing(2) self.title_label = QLabel(title) self.title_label.setObjectName("MetricLabel") self.value_label = QLabel(value) @@ -367,26 +720,101 @@ class MetricBox(QFrame): self.value_label.setText(value) +class StatusDot(QFrame): + def __init__(self, title, action_widget=None): + super().__init__() + self.setObjectName("Panel") + self._color = QColor("#a3aab4") + layout = QHBoxLayout(self) + layout.setContentsMargins(8, 5, 8, 5) + layout.setSpacing(6) + self.dot = QLabel() + self.dot.setFixedSize(12, 12) + self.label = QLabel(title) + self.label.setObjectName("MetricLabel") + self.value = QLabel("--") + self.value.setObjectName("SmallValue") + layout.addWidget(self.dot) + layout.addWidget(self.label) + if action_widget is not None: + action_widget.setFixedSize(24, 24) + action_widget.setIconSize(QSize(17, 17)) + layout.addWidget(action_widget) + layout.addStretch(1) + layout.addWidget(self.value) + self.set_state(None) + + def set_state(self, active, text=None): + if active is None: + color = "#a3aab4" + else: + color = "#16a34a" if active else "#dc2626" + self.dot.setStyleSheet( + "background-color: %s; border-radius: 6px; border: 1px solid rgba(0,0,0,0.18);" + % color + ) + if text is not None: + self.value.setText(text) + + class ImageBox(QFrame): - def __init__(self, title): + def __init__(self, title, compact=False): super().__init__() self.title = title + self.mode = "flow" + self._flow_img = None + self._magnitude_img = None + self.compact = compact self.setObjectName("Panel") layout = QVBoxLayout(self) - layout.setContentsMargins(8, 8, 8, 8) - layout.setSpacing(6) + layout.setContentsMargins(6, 6, 6, 6) + layout.setSpacing(5) + header = QHBoxLayout() + header.setSpacing(6) self.title_label = QLabel(title) - self.title_label.setObjectName("MetricLabel") + self.title_label.setObjectName("SectionTitle") + self.mode_toggle_button = AnimatedButton("显示：光流") + self.mode_toggle_button.setObjectName("ViewToggle") + self.mode_toggle_button.setToolTip("点击切换光流 / 深度") + self.mode_toggle_button.clicked.connect(self.toggle_mode) + header.addWidget(self.title_label) + header.addStretch(1) + header.addWidget(self.mode_toggle_button) self.image_label = QLabel() self.image_label.setObjectName("ImagePanel") self.image_label.setAlignment(Qt.AlignCenter) - self.image_label.setMinimumSize(180, 180) + image_min = 126 if compact else 220 + self.image_label.setMinimumSize(image_min, image_min) self.image_label.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) self.image_label.setScaledContents(True) - layout.addWidget(self.title_label) + if compact: + self.setMinimumWidth(120) + self.title_label.setObjectName("MetricLabel") + self.mode_toggle_button.setMaximumWidth(76) + layout.addLayout(header) layout.addWidget(self.image_label, 1) + def toggle_mode(self): + self.set_mode("magnitude" if self.mode == "flow" else "flow") + + def set_mode(self, mode): + self.mode = mode + if hasattr(self, "mode_toggle_button"): + self.mode_toggle_button.setText("显示：深度" if mode == "magnitude" else "显示：光流") + self._render_image() + + def set_images(self, flow_img, magnitude_img): + self._flow_img = flow_img + self._magnitude_img = magnitude_img + self._render_image() + def set_image(self, img): + self.set_images(img, None) + + def _render_image(self): + img = self._magnitude_img if self.mode == "magnitude" else self._flow_img + if img is None and self.mode == "magnitude": + img = self._flow_img self.image_label.setPixmap(cv2_to_pixmap(img)) def set_transform_label(self, transform): @@ -396,31 +824,1367 @@ class ImageBox(QFrame): self.title_label.setText(f"{self.title} {angle}deg {flip_x} {flip_y}") +class FloatingImageBox(ImageBox): + def __init__(self, title): + super().__init__(title, compact=True) + self._drag_mode = None + self._drag_start_global = None + self._start_geometry = None + self._user_placed = False + self._size_grip = QSizeGrip(self) + self._size_grip.setFixedSize(16, 16) + self._size_grip.raise_() + self.setMinimumSize(128, 150) + self.setMouseTracking(True) + self.title_label.installEventFilter(self) + self.image_label.installEventFilter(self) + self._size_grip.installEventFilter(self) + + def set_default_geometry(self, rect): + if self._user_placed: + self._keep_inside_parent() + return + self.setGeometry(rect) + self.raise_() + + def eventFilter(self, obj, event): + if obj not in (self.title_label, self.image_label, self._size_grip): + return super().eventFilter(obj, event) + event_type = event.type() + if event_type not in ( + QEvent.MouseButtonPress, + QEvent.MouseMove, + QEvent.MouseButtonRelease, + ): + return super().eventFilter(obj, event) + + mapped_pos = obj.mapTo(self, event.pos()) + if event_type == QEvent.MouseButtonPress and event.button() == Qt.LeftButton: + if obj is self._size_grip: + mapped_pos = QPointF(self.width() - 1, self.height() - 1).toPoint() + self._begin_drag(mapped_pos, event.globalPos()) + return True + if event_type == QEvent.MouseMove: + if self._drag_mode: + self._continue_drag(event.globalPos()) + return True + if event_type == QEvent.MouseButtonRelease and event.button() == Qt.LeftButton: + self._end_drag() + return True + return super().eventFilter(obj, event) + + def mousePressEvent(self, event): + if event.button() == Qt.LeftButton: + self._begin_drag(event.pos(), event.globalPos()) + event.accept() + return + super().mousePressEvent(event) + + def mouseMoveEvent(self, event): + if self._drag_mode: + self._continue_drag(event.globalPos()) + event.accept() + return + if self._is_resize_pos(event.pos()): + self.setCursor(Qt.SizeFDiagCursor) + else: + self.setCursor(Qt.ArrowCursor) + super().mouseMoveEvent(event) + + def mouseReleaseEvent(self, event): + if event.button() == Qt.LeftButton and self._drag_mode: + self._end_drag() + event.accept() + return + super().mouseReleaseEvent(event) + + def resizeEvent(self, event): + super().resizeEvent(event) + self._size_grip.move( + max(0, self.width() - self._size_grip.width() - 3), + max(0, self.height() - self._size_grip.height() - 3), + ) + self._size_grip.raise_() + + def _is_resize_pos(self, pos): + return pos.x() >= self.width() - 18 and pos.y() >= self.height() - 18 + + def _begin_drag(self, pos, global_pos): + self._drag_mode = "resize" if self._is_resize_pos(pos) else "move" + self._drag_start_global = global_pos + self._start_geometry = self.geometry() + self._user_placed = True + self.raise_() + + def _continue_drag(self, global_pos): + if self._start_geometry is None or self._drag_start_global is None: + return + delta = global_pos - self._drag_start_global + geom = QRectF(self._start_geometry) + if self._drag_mode == "resize": + new_w = max(self.minimumWidth(), int(geom.width() + delta.x())) + new_h = max(self.minimumHeight(), int(geom.height() + delta.y())) + self.setGeometry(int(geom.x()), int(geom.y()), new_w, new_h) + else: + self.move(int(geom.x() + delta.x()), int(geom.y() + delta.y())) + self._keep_inside_parent() + + def _end_drag(self): + self._drag_mode = None + self._drag_start_global = None + self._start_geometry = None + self.setCursor(Qt.ArrowCursor) + + def _keep_inside_parent(self): + parent = self.parentWidget() + if parent is None: + return + x = max(0, min(self.x(), parent.width() - self.width())) + y = max(0, min(self.y(), parent.height() - self.height())) + self.move(x, y) + + +class FloatingPanel(QFrame): + def __init__(self, parent=None): + super().__init__(parent) + self.setObjectName("ControlPanel") + self._drag_mode = None + self._drag_start_global = None + self._start_geometry = None + self._user_placed = False + self.setMouseTracking(True) + self.setMinimumSize(280, 96) + self._size_grip = QSizeGrip(self) + self._size_grip.setFixedSize(16, 16) + self._size_grip.raise_() + + def set_drag_handle(self, widget): + widget.installEventFilter(self) + widget.setCursor(Qt.SizeAllCursor) + + def set_default_geometry(self, rect): + if self._user_placed: + self._keep_inside_parent() + return + self.setGeometry(rect) + self.raise_() + + def eventFilter(self, obj, event): + event_type = event.type() + if event_type not in ( + QEvent.MouseButtonPress, + QEvent.MouseMove, + QEvent.MouseButtonRelease, + ): + return super().eventFilter(obj, event) + if event_type == QEvent.MouseButtonPress and event.button() == Qt.LeftButton: + self._begin_drag(obj.mapTo(self, event.pos()), event.globalPos()) + return True + if event_type == QEvent.MouseMove and self._drag_mode: + self._continue_drag(event.globalPos()) + return True + if event_type == QEvent.MouseButtonRelease and event.button() == Qt.LeftButton: + self._end_drag() + return True + return super().eventFilter(obj, event) + + def mousePressEvent(self, event): + if event.button() == Qt.LeftButton: + self._begin_drag(event.pos(), event.globalPos()) + event.accept() + return + super().mousePressEvent(event) + + def mouseMoveEvent(self, event): + if self._drag_mode: + self._continue_drag(event.globalPos()) + event.accept() + return + self.setCursor(Qt.SizeFDiagCursor if self._is_resize_pos(event.pos()) else Qt.ArrowCursor) + super().mouseMoveEvent(event) + + def mouseReleaseEvent(self, event): + if event.button() == Qt.LeftButton and self._drag_mode: + self._end_drag() + event.accept() + return + super().mouseReleaseEvent(event) + + def resizeEvent(self, event): + super().resizeEvent(event) + self._size_grip.move( + max(0, self.width() - self._size_grip.width() - 3), + max(0, self.height() - self._size_grip.height() - 3), + ) + self._size_grip.raise_() + + def _is_resize_pos(self, pos): + return pos.x() >= self.width() - 18 and pos.y() >= self.height() - 18 + + def _begin_drag(self, pos, global_pos): + self._drag_mode = "resize" if self._is_resize_pos(pos) else "move" + self._drag_start_global = global_pos + self._start_geometry = self.geometry() + self._user_placed = True + self.raise_() + + def _continue_drag(self, global_pos): + if self._start_geometry is None or self._drag_start_global is None: + return + delta = global_pos - self._drag_start_global + geom = QRectF(self._start_geometry) + if self._drag_mode == "resize": + new_w = max(self.minimumWidth(), int(geom.width() + delta.x())) + new_h = max(self.minimumHeight(), int(geom.height() + delta.y())) + self.setGeometry(int(geom.x()), int(geom.y()), new_w, new_h) + else: + self.move(int(geom.x() + delta.x()), int(geom.y() + delta.y())) + self._keep_inside_parent() + + def _end_drag(self): + self._drag_mode = None + self._drag_start_global = None + self._start_geometry = None + self.setCursor(Qt.ArrowCursor) + + def _keep_inside_parent(self): + parent = self.parentWidget() + if parent is None: + return + x = max(0, min(self.x(), parent.width() - self.width())) + y = max(0, min(self.y(), parent.height() - self.height())) + self.move(x, y) + + +class GripperStage(QFrame): + MODEL_OPEN_BINS = 40 + _mesh_cache = None + _urdf_cache = None + _mesh_link_cache = None + + def __init__(self): + super().__init__() + self.setObjectName("GripperStage") + self.state = "--" + self.left_force = 0.0 + self.right_force = 0.0 + self.force_pct = 0 + self.target_force = 0 + self.gripper_position = None + self.open_pos = 0 + self.close_pos = 9000 + self._gl_ready = False + self._gl_items = [] + self._gl_faces = {} + self._gl_item = None + self._combined_faces = None + self._mesh_dict = {} + self._mesh_face_counts = {} + self._vertex_cache = {} + self._mesh_cache_prewarmed = False + self._last_gl_open_bin = None + self._perf = { + "last_log": time.perf_counter(), + "status_count": 0, + "gl_updates": 0, + "cache_hits": 0, + "cache_misses": 0, + "mesh_total": 0.0, + "mesh_max": 0.0, + "set_total": 0.0, + "set_max": 0.0, + } + self._state_label = None + self._force_label = None + self._gl_error_text = "" + self._floating_control_panels = [] + self.left_flow = FloatingImageBox("左侧传感器") + self.right_flow = FloatingImageBox("右侧传感器") + self.setMinimumSize(240, 300) + self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) + self._init_gl_view() + + def _init_gl_view(self): + layout = QVBoxLayout(self) + layout.setContentsMargins(10, 8, 10, 8) + layout.setSpacing(6) + + header = QHBoxLayout() + title = QLabel("夹爪 3D 模型") + title.setObjectName("SectionTitle") + self._state_label = QLabel("--") + self._state_label.setObjectName("Subtle") + header.addWidget(title) + header.addStretch(1) + header.addWidget(self._state_label) + layout.addLayout(header) + + if gl is None: + self._gl_error_text = ( + "需要安装 3D 依赖: pip install PyOpenGL PyOpenGL_accelerate" + ) + self.gl_placeholder = QLabel(self._gl_error_text) + self.gl_placeholder.setAlignment(Qt.AlignCenter) + self.gl_placeholder.setObjectName("Subtle") + self.gl_placeholder.setMinimumSize(260, 260) + layout.addWidget(self.gl_placeholder, 1) + else: + self.gl_view = gl.GLViewWidget() + self.gl_view.setBackgroundColor("#ffffff") + self.gl_view.setCameraPosition(distance=0.42, elevation=23, azimuth=-38) + self.gl_view.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) + layout.addWidget(self.gl_view, 1) + + self.left_flow.setParent(self) + self.right_flow.setParent(self) + self.left_flow.show() + self.right_flow.show() + self.left_flow.raise_() + self.right_flow.raise_() + + self._force_label = QLabel("L --N R --N Force --% Target --%") + self._force_label.setObjectName("Subtle") + layout.addWidget(self._force_label) + + if gl is None: + return + + grid = gl.GLGridItem() + grid.setSize(x=4.0, y=4.0) + grid.setSpacing(x=0.05, y=0.05) + grid.setColor((208, 216, 226, 72)) + grid.translate(0.0, 0.0, -0.006) + self.gl_view.addItem(grid) + + if not self._build_gl_model(): + self._gl_error_text = "未找到 STL 模型" + if hasattr(self, "gl_placeholder"): + self.gl_placeholder.setText(self._gl_error_text) + return + + self._gl_ready = True + + def _show_gl_error(self, text): + if hasattr(self, "gl_view"): + label = QLabel(text) + label.setObjectName("Subtle") + label.setAlignment(Qt.AlignCenter) + + @staticmethod + def _mesh_to_vertex_faces(tris): + vertexes = np.asarray(tris.reshape(-1, 3), dtype=float) + faces = np.arange(vertexes.shape[0], dtype=np.uint32).reshape(-1, 3) + return vertexes, faces + + @staticmethod + def _part_base_color(name): + if "Support" in name: + return np.asarray((0.50, 0.53, 0.56), dtype=np.float32) + if "base" in name: + return np.asarray((0.66, 0.69, 0.72), dtype=np.float32) + return np.asarray((0.76, 0.79, 0.82), dtype=np.float32) + + @classmethod + def _face_colors(cls, name, tris): + p0 = tris[:, 0, :] + p1 = tris[:, 1, :] + p2 = tris[:, 2, :] + normals = np.cross(p1 - p0, p2 - p0) + norm = np.linalg.norm(normals, axis=1, keepdims=True) + normals = normals / np.maximum(norm, 1e-9) + + key_light = np.asarray((0.35, -0.55, 0.76), dtype=np.float32) + fill_light = np.asarray((-0.65, 0.30, 0.42), dtype=np.float32) + rim_light = np.asarray((0.08, 0.88, 0.48), dtype=np.float32) + key_light = key_light / np.linalg.norm(key_light) + fill_light = fill_light / np.linalg.norm(fill_light) + rim_light = rim_light / np.linalg.norm(rim_light) + + # Use abs(dot) so inconsistent STL winding does not turn whole parts black. + key = np.abs(normals @ key_light) + fill = np.abs(normals @ fill_light) + rim = np.clip(normals @ rim_light, 0.0, 1.0) + shade = 0.56 + 0.34 * key + 0.18 * fill + 0.12 * rim + shade = np.clip(shade, 0.48, 1.16).astype(np.float32) + + base = cls._part_base_color(name) + colors = np.empty((tris.shape[0], 4), dtype=np.float32) + colors[:, :3] = np.clip(base[None, :] * shade[:, None], 0.0, 1.0) + highlight = np.clip((shade - 0.82)[:, None] * 0.32, 0.0, 0.18) + colors[:, :3] = np.clip(colors[:, :3] + highlight, 0.0, 1.0) + colors[:, 3] = 1.0 + return colors + + def _combined_mesh_data(self, open_bin): + key = ("combined", int(open_bin)) + cached = self._vertex_cache.get(key) + if cached is not None: + self._perf["cache_hits"] += 1 + vertexes, face_colors = cached + return vertexes, self._combined_faces, face_colors + + self._perf["cache_misses"] += 1 + open_ratio = int(open_bin) / float(self.MODEL_OPEN_BINS) + vertex_parts = [] + face_parts = [] + color_parts = [] + offset = 0 + for name, tris in self._mesh_dict.items(): + transformed = self._transform_mesh(name, tris, open_ratio) + vertexes = np.asarray(transformed.reshape(-1, 3), dtype=np.float32) + faces = np.arange(vertexes.shape[0], dtype=np.uint32).reshape(-1, 3) + offset + vertex_parts.append(vertexes) + face_parts.append(faces) + color_parts.append(self._face_colors(name, transformed)) + offset += vertexes.shape[0] + if not vertex_parts: + return None, None, None + combined_vertexes = np.vstack(vertex_parts) + combined_colors = np.vstack(color_parts) + if self._combined_faces is None: + self._combined_faces = np.vstack(face_parts) + self._vertex_cache[key] = (combined_vertexes, combined_colors) + return combined_vertexes, self._combined_faces, combined_colors + + def _build_gl_model(self): + meshes = self._load_meshes() + if not meshes: + return False + self._mesh_dict = dict(meshes) + self._mesh_face_counts = {name: len(tris) for name, tris in meshes} + self._prewarm_mesh_cache() + + vertexes, faces, face_colors = self._combined_mesh_data(self.MODEL_OPEN_BINS) + if vertexes is None or faces is None: + return False + self._gl_item = gl.GLMeshItem( + vertexes=vertexes, + faces=faces, + faceColors=face_colors, + color=(0.72, 0.75, 0.78, 1.0), + smooth=False, + computeNormals=False, + drawFaces=True, + drawEdges=False, + shader=None, + ) + self._gl_item.setGLOptions("opaque") + self.gl_view.addItem(self._gl_item) + self._update_gl_model(force=True) + return True + + def _prewarm_mesh_cache(self): + if self._mesh_cache_prewarmed: + return + start = time.perf_counter() + for open_bin in range(self.MODEL_OPEN_BINS + 1): + self._combined_mesh_data(open_bin) + self._mesh_cache_prewarmed = True + elapsed_ms = (time.perf_counter() - start) * 1000.0 + triangle_count = sum(len(tris) for tris in self._mesh_dict.values()) + print( + "[perf] ui_3d prewarm " + f"bins={self.MODEL_OPEN_BINS + 1} " + f"triangles={triangle_count} " + f"elapsed={elapsed_ms:.1f}ms" + ) + + def _update_gl_model(self, force=False): + if not self._gl_ready and not force: + return + if self._gl_item is None: + return + open_ratio = self._position_open_ratio() + open_bin = self._open_ratio_bin(open_ratio) + if self._last_gl_open_bin == open_bin and not force: + return + self._last_gl_open_bin = open_bin + self._perf["gl_updates"] += 1 + t0 = time.perf_counter() + vertexes, faces, face_colors = self._combined_mesh_data(open_bin) + mesh_dt = time.perf_counter() - t0 + self._perf["mesh_total"] += mesh_dt + self._perf["mesh_max"] = max(self._perf["mesh_max"], mesh_dt) + if vertexes is None or faces is None: + return + t1 = time.perf_counter() + self._gl_item.setMeshData(vertexes=vertexes, faces=faces, faceColors=face_colors) + set_dt = time.perf_counter() - t1 + self._perf["set_total"] += set_dt + self._perf["set_max"] = max(self._perf["set_max"], set_dt) + + @staticmethod + def _open_ratio_bin(open_ratio): + return int( + round( + max(0.0, min(1.0, float(open_ratio))) + * float(GripperStage.MODEL_OPEN_BINS) + ) + ) + + def _cached_vertexes(self, name, open_bin): + key = (name, int(open_bin)) + cached = self._vertex_cache.get(key) + if cached is not None: + self._perf["cache_hits"] += 1 + return cached + self._perf["cache_misses"] += 1 + tris = self._mesh_dict.get(name) + if tris is None: + return None + open_ratio = int(open_bin) / float(self.MODEL_OPEN_BINS) + transformed = self._transform_mesh(name, tris, open_ratio) + vertexes, _ = self._mesh_to_vertex_faces(transformed) + vertexes = np.asarray(vertexes, dtype=np.float32) + self._vertex_cache[key] = vertexes + return vertexes + + def _position_open_ratio(self): + if self.gripper_position is None: + return 1.0 + try: + open_pos = float(self.open_pos) + close_pos = float(self.close_pos) + position = float(self.gripper_position) + except Exception: + return 1.0 + span = close_pos - open_pos + if abs(span) < 1e-6: + return 1.0 + closed_ratio = (position - open_pos) / span + return max(0.0, min(1.0, 1.0 - closed_ratio)) + + @classmethod + def _mesh_dir(cls): + for root in (REPO_ROOT, Path.cwd(), Path(__file__).resolve().parents[2]): + path = root / "crt_ctag2f90d_gripper_visualization" / "meshes" + if path.exists(): + return path + return None + + @classmethod + def _urdf_path(cls): + relative = ( + "crt_ctag2f90d_gripper_visualization", + "urdf", + "crt_ctag2f90d_gripper_visualization_sync.urdf", + ) + for root in (REPO_ROOT, Path.cwd(), Path(__file__).resolve().parents[2]): + path = root.joinpath(*relative) + if path.exists(): + return path + return None + + @classmethod + def _read_stl(cls, path, limit=None): + try: + data = path.read_bytes() + if len(data) >= 84: + count = struct.unpack_from(" 0 and expected <= len(data): + step = 1 if limit is None else max(1, math.ceil(count / limit)) + tris = [] + for idx in range(0, count, step): + off = 84 + idx * 50 + values = struct.unpack_from("<12f", data, off) + tris.append( + [ + values[3:6], + values[6:9], + values[9:12], + ] + ) + return np.asarray(tris, dtype=float) + except Exception: + pass + + tris = [] + vertices = [] + try: + for line in path.read_text(errors="ignore").splitlines(): + line = line.strip() + if not line.startswith("vertex"): + continue + parts = line.split() + if len(parts) != 4: + continue + vertices.append([float(parts[1]), float(parts[2]), float(parts[3])]) + if len(vertices) == 3: + tris.append(vertices) + vertices = [] + if limit is not None and len(tris) >= limit: + break + except Exception: + return np.empty((0, 3, 3), dtype=float) + return np.asarray(tris, dtype=float) + + @classmethod + def _load_meshes(cls): + if cls._mesh_cache is not None: + return cls._mesh_cache + mesh_dir = cls._mesh_dir() + meshes = [] + if mesh_dir is not None: + for name in ( + "base_link.STL", + "Left_1_Link.STL", + "Left_2_Link.STL", + "Left_Support_Link.STL", + "Right_1_Link.STL", + "Right_2_Link.STL", + "Right_Support_Link.STL", + ): + path = mesh_dir / name + if not path.exists(): + continue + tris = cls._read_stl(path) + if tris.size: + meshes.append((name, tris)) + cls._mesh_cache = meshes + return meshes + + @staticmethod + def _parse_xyz(value, default=(0.0, 0.0, 0.0)): + if not value: + return np.asarray(default, dtype=float) + parts = [float(x) for x in value.split()] + while len(parts) < 3: + parts.append(0.0) + return np.asarray(parts[:3], dtype=float) + + @classmethod + def _load_urdf_model(cls): + if cls._urdf_cache is not None: + return cls._urdf_cache + path = cls._urdf_path() + if path is None: + cls._urdf_cache = None + return None + try: + root = ET.parse(path).getroot() + except Exception: + cls._urdf_cache = None + return None + + links = {} + for link in root.findall("link"): + name = link.get("name") + visual = link.find("visual") + if name is None or visual is None: + continue + origin = visual.find("origin") + geometry = visual.find("geometry") + mesh = None if geometry is None else geometry.find("mesh") + if mesh is None: + continue + filename = mesh.get("filename", "") + if "/" in filename: + mesh_name = filename.rsplit("/", 1)[-1] + else: + mesh_name = filename + links[name] = { + "mesh": mesh_name, + "origin_xyz": cls._parse_xyz(None if origin is None else origin.get("xyz")), + "origin_rpy": cls._parse_xyz(None if origin is None else origin.get("rpy")), + } + + joints = {} + children = {} + for joint in root.findall("joint"): + name = joint.get("name") + parent = joint.find("parent") + child = joint.find("child") + if name is None or parent is None or child is None: + continue + parent_name = parent.get("link") + child_name = child.get("link") + origin = joint.find("origin") + axis = joint.find("axis") + mimic = joint.find("mimic") + info = { + "name": name, + "type": joint.get("type", "fixed"), + "parent": parent_name, + "child": child_name, + "origin_xyz": cls._parse_xyz(None if origin is None else origin.get("xyz")), + "origin_rpy": cls._parse_xyz(None if origin is None else origin.get("rpy")), + "axis": cls._parse_xyz(None if axis is None else axis.get("xyz"), default=(0.0, 0.0, 1.0)), + "mimic_joint": None if mimic is None else mimic.get("joint"), + "mimic_multiplier": 1.0 if mimic is None else float(mimic.get("multiplier", "1")), + "mimic_offset": 0.0 if mimic is None else float(mimic.get("offset", "0")), + } + joints[child_name] = info + children.setdefault(parent_name, []).append(child_name) + + cls._urdf_cache = {"links": links, "joints": joints, "children": children} + return cls._urdf_cache + + @classmethod + def _mesh_link_map(cls): + if cls._mesh_link_cache is not None: + return cls._mesh_link_cache + urdf = cls._load_urdf_model() + if urdf is None: + cls._mesh_link_cache = {} + else: + cls._mesh_link_cache = { + info["mesh"]: link_name + for link_name, info in urdf["links"].items() + } + return cls._mesh_link_cache + + @staticmethod + def _matrix_from_xyz_rpy(xyz, rpy): + mat = np.eye(4, dtype=float) + mat[:3, :3] = GripperStage._rotation_matrix(float(rpy[0]), float(rpy[1]), float(rpy[2])) + mat[:3, 3] = xyz + return mat + + @staticmethod + def _axis_angle_matrix(axis, angle): + axis = np.asarray(axis, dtype=float) + norm = np.linalg.norm(axis) + mat = np.eye(4, dtype=float) + if norm <= 1e-9: + return mat + x, y, z = axis / norm + c = math.cos(angle) + s = math.sin(angle) + t = 1.0 - c + mat[:3, :3] = np.asarray( + [ + [t * x * x + c, t * x * y - s * z, t * x * z + s * y], + [t * x * y + s * z, t * y * y + c, t * y * z - s * x], + [t * x * z - s * y, t * y * z + s * x, t * z * z + c], + ], + dtype=float, + ) + return mat + + @classmethod + def _joint_positions(cls, open_ratio): + # URDF limit: 0 is the open posture for this exported visualization, + # -0.91 is the closed end. open_ratio=1.0 therefore maps to 0. + main = -0.91 * (1.0 - float(max(0.0, min(1.0, open_ratio)))) + return {"Left_1_Joint": main} + + @classmethod + def _link_transform(cls, link_name, joint_positions, cache): + if link_name in cache: + return cache[link_name] + urdf = cls._load_urdf_model() + if urdf is None: + cache[link_name] = np.eye(4, dtype=float) + return cache[link_name] + joint = urdf["joints"].get(link_name) + if joint is None: + cache[link_name] = np.eye(4, dtype=float) + return cache[link_name] + parent_tf = cls._link_transform(joint["parent"], joint_positions, cache) + joint_tf = cls._matrix_from_xyz_rpy(joint["origin_xyz"], joint["origin_rpy"]) + angle = 0.0 + if joint["type"] in {"revolute", "continuous"}: + if joint["mimic_joint"]: + base = joint_positions.get(joint["mimic_joint"], 0.0) + angle = base * joint["mimic_multiplier"] + joint["mimic_offset"] + else: + angle = joint_positions.get(joint["name"], 0.0) + cache[link_name] = parent_tf @ joint_tf @ cls._axis_angle_matrix(joint["axis"], angle) + return cache[link_name] + + @staticmethod + def _rotation_matrix(rx, ry, rz): + sx, cx = math.sin(rx), math.cos(rx) + sy, cy = math.sin(ry), math.cos(ry) + sz, cz = math.sin(rz), math.cos(rz) + mx = np.asarray([[1, 0, 0], [0, cx, -sx], [0, sx, cx]], dtype=float) + my = np.asarray([[cy, 0, sy], [0, 1, 0], [-sy, 0, cy]], dtype=float) + mz = np.asarray([[cz, -sz, 0], [sz, cz, 0], [0, 0, 1]], dtype=float) + return mz @ my @ mx + + @classmethod + def _transform_mesh(cls, name, tris, open_ratio): + urdf = cls._load_urdf_model() + if urdf is not None: + link_name = cls._mesh_link_map().get(name) + if link_name is not None: + joint_positions = cls._joint_positions(open_ratio) + link_tf = cls._link_transform(link_name, joint_positions, {}) + visual = urdf["links"][link_name] + visual_tf = cls._matrix_from_xyz_rpy(visual["origin_xyz"], visual["origin_rpy"]) + tf = link_tf @ visual_tf + pts = tris.reshape(-1, 3) + pts_h = np.concatenate([pts, np.ones((pts.shape[0], 1), dtype=float)], axis=1) + out = (pts_h @ tf.T)[:, :3] + return out.reshape((-1, 3, 3)) + return tris + + def _draw_stl_model(self, painter, rect, scene_top, scene_bottom): + meshes = self._load_meshes() + if not meshes: + return False + + open_ratio = max(0.0, min(1.0, 1.0 - (float(self.force_pct) / 100.0))) + parts = [] + all_pts = [] + for name, tris in meshes: + transformed = self._transform_mesh(name, tris, open_ratio) + if transformed.size: + parts.append((name, transformed)) + all_pts.append(transformed.reshape(-1, 3)) + if not all_pts: + return False + + cloud = np.vstack(all_pts) + center = (cloud.min(axis=0) + cloud.max(axis=0)) * 0.5 + cloud = cloud - center + span = np.maximum(cloud.max(axis=0) - cloud.min(axis=0), 1e-6) + model_scale = min(rect.width() * 0.56 / span[0], (scene_bottom - scene_top) * 0.72 / span[2]) + + cx = rect.center().x() + cy = scene_top + (scene_bottom - scene_top) * 0.54 + render_items = [] + light = np.asarray([0.2, -0.55, 0.8], dtype=float) + light = light / np.linalg.norm(light) + for name, tris in parts: + pts = tris - center + for tri in pts: + depth = float(np.mean(tri[:, 1])) + p0, p1, p2 = tri + normal = np.cross(p1 - p0, p2 - p0) + nlen = np.linalg.norm(normal) + shade = 0.68 + if nlen > 1e-9: + normal = normal / nlen + shade = 0.58 + 0.34 * max(0.0, float(np.dot(normal, light))) + poly = [] + for x, y, z in tri: + px = cx + (x + 0.22 * y) * model_scale + py = cy - (z - 0.10 * y) * model_scale + poly.append(QPointF(px, py)) + render_items.append((depth, shade, QPolygonF(poly))) + + render_items.sort(key=lambda item: item[0]) + for _, shade, poly in render_items: + value = int(165 + 70 * shade) + painter.setPen(QPen(QColor("#9aa6b2"), 1)) + painter.setBrush(QColor(value, value + 2, min(255, value + 8), 235)) + painter.drawPolygon(poly) + + painter.setPen(QPen(QColor("#d92d20"), 3)) + width = rect.width() * 0.18 + y = scene_top + (scene_bottom - scene_top) * 0.44 + painter.drawLine(QPointF(cx - width, y), QPointF(cx - width * 0.35, y + 14)) + painter.drawLine(QPointF(cx + width, y), QPointF(cx + width * 0.35, y + 14)) + return True + + def update_state(self, status): + self._perf["status_count"] += 1 + self.state = str(status.get("state", "--")) + self.left_force = force_display_value(status.get("left_normal", 0.0) or 0.0) + self.right_force = force_display_value(status.get("right_normal", 0.0) or 0.0) + self.force_pct = int(status.get("force_pct", 0) or 0) + self.target_force = int(status.get("adaptive_target_force", 0) or 0) + self.gripper_position = status.get("gripper_position") + self.open_pos = status.get("open_pos", self.open_pos) + self.close_pos = status.get("close_pos", self.close_pos) + if self._state_label is not None: + self._state_label.setText(STATE_TEXT.get(self.state, self.state)) + if self._force_label is not None: + pos_text = "--" if self.gripper_position is None else str(int(self.gripper_position)) + self._force_label.setText( + f"L {self.left_force:.2f}N R {self.right_force:.2f}N " + f"Pos {pos_text} Force {self.force_pct}% Target {self.target_force}%" + ) + self._update_gl_model() + self._log_perf_if_due() + if not self._gl_ready: + self.update() + + def add_floating_control_panel(self, panel, role): + panel.setParent(self) + panel.show() + panel.raise_() + self._floating_control_panels.append((panel, role)) + self._layout_overlays() + + def _log_perf_if_due(self): + now = time.perf_counter() + if now - self._perf["last_log"] < 1.0: + return + mesh_ops = self._perf["cache_hits"] + self._perf["cache_misses"] + set_ops = max(1, self._perf["gl_updates"]) + mesh_avg = self._perf["mesh_total"] / max(1, mesh_ops) * 1000.0 + set_avg = self._perf["set_total"] / set_ops * 1000.0 + print( + "[perf] ui_3d " + f"status_hz={self._perf['status_count']} " + f"gl_updates={self._perf['gl_updates']} " + f"bin={self._last_gl_open_bin} " + f"cache_hit={self._perf['cache_hits']} " + f"cache_miss={self._perf['cache_misses']} " + f"mesh_avg={mesh_avg:.2f}ms " + f"mesh_max={self._perf['mesh_max'] * 1000.0:.2f}ms " + f"set_avg={set_avg:.2f}ms " + f"set_max={self._perf['set_max'] * 1000.0:.2f}ms " + f"pos={self.gripper_position}" + ) + self._perf = { + "last_log": now, + "status_count": 0, + "gl_updates": 0, + "cache_hits": 0, + "cache_misses": 0, + "mesh_total": 0.0, + "mesh_max": 0.0, + "set_total": 0.0, + "set_max": 0.0, + } + + def resizeEvent(self, event): + super().resizeEvent(event) + self._layout_overlays() + + def _layout_overlays(self): + if hasattr(self, "gl_view"): + area = self.gl_view.geometry() + elif hasattr(self, "gl_placeholder"): + area = self.gl_placeholder.geometry() + else: + area = self.rect().adjusted(10, 40, -10, -28) + + panel_w = max(140, min(230, int(area.width() * 0.23))) + panel_h = max(138, min(200, int(area.height() * 0.34))) + margin = 12 + self.left_flow.set_default_geometry( + QRectF(area.left() + margin, area.top() + margin, panel_w, panel_h).toRect() + ) + self.right_flow.set_default_geometry( + QRectF( + area.right() - panel_w - margin, + area.top() + margin, + panel_w, + panel_h, + ).toRect() + ) + control_h = max(112, min(170, int(area.height() * 0.25))) + gripper_w = max(430, min(660, int(area.width() * 0.48))) + sensor_w = max(440, min(680, int(area.width() * 0.48))) + available_w = max(560, area.width() - margin * 3) + if gripper_w + sensor_w > available_w: + gripper_w = max(280, int(available_w * 0.50)) + sensor_w = max(280, int(available_w - gripper_w)) + y = max(area.top() + margin, area.bottom() - control_h - margin) + plot_w = max(340, min(500, int(area.width() * 0.36))) + plot_h = max(300, min(440, int(area.height() * 0.52))) + plot_y = area.top() + panel_h + margin * 2 + if plot_y + plot_h > y - margin: + plot_h = max(260, y - margin - plot_y) + for panel, role in self._floating_control_panels: + if role == "gripper": + x = area.left() + margin + rect = QRectF(x, y, gripper_w, control_h).toRect() + elif role == "sensor": + x = area.right() - sensor_w - margin + rect = QRectF(x, y, sensor_w, control_h).toRect() + else: + x = area.right() - plot_w - margin + rect = QRectF(x, plot_y, plot_w, plot_h).toRect() + panel.set_default_geometry(rect) + panel.raise_() + + def paintEvent(self, event): + if self._gl_ready: + super().paintEvent(event) + return + super().paintEvent(event) + painter = QPainter(self) + painter.setRenderHint(QPainter.Antialiasing, True) + + rect = self.rect().adjusted(14, 14, -14, -14) + painter.setPen(QPen(QColor("#d7dee8"), 1)) + painter.setBrush(QColor("#ffffff")) + painter.drawRoundedRect(rect, 10, 10) + painter.setPen(QPen(QColor("#344054"), 1)) + painter.setFont(QFont("Microsoft YaHei", 10, QFont.Bold)) + text = self._gl_error_text or "3D 模型视窗未初始化" + painter.drawText(rect, Qt.AlignCenter, text) + + +class SliderControl(QFrame): + def __init__(self, title, minimum, maximum, value, step=1, suffix=""): + super().__init__() + self.setObjectName("Panel") + self.suffix = suffix + self.step = max(1, int(step)) + layout = QGridLayout(self) + layout.setContentsMargins(8, 5, 8, 5) + layout.setHorizontalSpacing(6) + layout.setVerticalSpacing(2) + self.title_label = QLabel(title) + self.title_label.setObjectName("MetricLabel") + self.value_label = QLabel("--") + self.value_label.setObjectName("SmallValue") + self.minus_button = AnimatedButton(f"-{self.step}") + self.minus_button.setObjectName("StepButton") + self.plus_button = AnimatedButton(f"+{self.step}") + self.plus_button.setObjectName("StepButton") + self.slider = QSlider(Qt.Horizontal) + self.slider.setRange(minimum, maximum) + self.slider.setValue(value) + self.slider.setTracking(False) + self.slider.setSingleStep(self.step) + self.slider.setPageStep(self.step) + self.slider.valueChanged.connect(self._update_value_label) + self.slider.sliderMoved.connect(self._update_value_label) + self.minus_button.clicked.connect(lambda: self.nudge(-self.step)) + self.plus_button.clicked.connect(lambda: self.nudge(self.step)) + layout.addWidget(self.title_label, 0, 0) + layout.addWidget(self.value_label, 0, 1) + layout.addWidget(self.minus_button, 0, 2) + layout.addWidget(self.plus_button, 0, 3) + layout.addWidget(self.slider, 1, 0, 1, 4) + layout.setColumnStretch(0, 1) + layout.setColumnStretch(1, 0) + layout.setColumnStretch(2, 0) + layout.setColumnStretch(3, 0) + self._update_value_label(self.slider.value()) + + def _update_value_label(self, value): + self.value_label.setText(f"{int(value)}{self.suffix}") + + def set_interactive_enabled(self, enabled): + self.slider.setEnabled(enabled) + self.minus_button.setEnabled(enabled) + self.plus_button.setEnabled(enabled) + + def nudge(self, delta): + self.slider.setValue( + max(self.slider.minimum(), min(self.slider.maximum(), self.slider.value() + int(delta))) + ) + + def set_value(self, value, force=False): + if value is None: + self.value_label.setText("--") + return + if not force and self.slider.isSliderDown(): + return + try: + value = int(value) + was_blocked = self.slider.blockSignals(True) + self.slider.setValue(value) + self.slider.blockSignals(was_blocked) + self._update_value_label(value) + except Exception: + pass + + def value(self): + return int(self.slider.value()) + + class VisualSettingsDialog(QDialog): + CONFIG_GROUPS = [ + ( + "相机 / 传感器", + [ + "video1", + "video2", + "sensor_config1", + "sensor_config2", + "sensor_fps", + "motion_threshold", + "sensor_cpu", + ], + ), + ( + "串口 / 设备", + ["port", "slave_id", "baudrate", "serial_timeout"], + ), + ( + "位置 / 运动", + [ + "open_pos", + "close_pos", + "speed", + "accel", + "decel", + "release_open_speed", + "release_open_accel", + "release_open_decel", + ], + ), + ( + "力度", + [ + "open_force", + "close_force", + "grip_start_force", + "hold_force", + "initial_force", + "force_min", + "force_max", + ], + ), + ( + "控制 / 滤波", + [ + "control_hz", + "baseline_seconds", + "filter_alpha", + "shear_filter_alpha", + "dry_run", + "open_at_end", + "trigger_force_update", + ], + ), + ( + "触发 / 夹持", + [ + "trigger_normal_force", + "trigger_shear_force", + "trigger_clear_stable_seconds", + "open_wait_stable_samples", + "open_wait_stable_range_n", + "open_wait_stable_trend_n", + "grip_normal_force", + "grip_requires_both", + "close_timeout_seconds", + "closing_command_interval", + "force_ramp_step", + "force_ramp_interval", + "grip_settle_seconds", + ], + ), + ( + "自适应 / 释放", + [ + "adaptive_grip_enabled", + "adaptive_shear_start_force", + "adaptive_shear_full_force", + "adaptive_force_min", + "adaptive_force_max", + "adaptive_force_step", + "adaptive_force_interval", + "release_arm_delay_seconds", + "release_normal_change", + "release_shear_change", + ], + ), + ( + "恢复", + [ + "rearm_seconds", + "recover_normal_force", + "recover_shear_force", + "recover_stable_seconds", + "open_position_recover_enabled", + "open_position_tolerance", + "open_position_stable_seconds", + "open_position_check_interval", + "hold_fallback_pos", + ], + ), + ("标定", ["normal_force_calibration", "shear_force_calibration"]), + ] + + PARAM_LABELS = { + "video1": "左相机编号", + "video2": "右相机编号", + "sensor_config1": "左传感器配置", + "sensor_config2": "右传感器配置", + "sensor_fps": "传感器 FPS", + "motion_threshold": "运动阈值", + "sensor_cpu": "传感器 CPU 模式", + "port": "串口号", + "slave_id": "从站 ID", + "baudrate": "波特率", + "serial_timeout": "串口超时", + "open_pos": "张开位置", + "close_pos": "闭合位置", + "speed": "默认速度", + "accel": "加速度", + "decel": "减速度", + "release_open_speed": "释放张开速度", + "release_open_accel": "释放张开加速度", + "release_open_decel": "释放张开减速度", + "open_force": "张开力", + "close_force": "闭合力", + "grip_start_force": "夹住起始力", + "hold_force": "保持力", + "initial_force": "初始力", + "force_min": "最小力", + "force_max": "最大力", + "control_hz": "控制频率", + "baseline_seconds": "基准时间", + "filter_alpha": "法向滤波", + "shear_filter_alpha": "切向滤波", + "dry_run": "干运行", + "open_at_end": "结束时张开", + "trigger_force_update": "触发力更新", + "trigger_normal_force": "法向触发阈值", + "trigger_shear_force": "切向触发阈值", + "trigger_clear_stable_seconds": "触发清除稳定时间", + "open_wait_stable_samples": "等待稳定样本数", + "open_wait_stable_range_n": "等待稳定范围", + "open_wait_stable_trend_n": "等待稳定趋势", + "grip_normal_force": "夹持接触阈值", + "grip_requires_both": "要求双侧接触", + "close_timeout_seconds": "闭合超时", + "closing_command_interval": "闭合命令间隔", + "force_ramp_step": "加力步进", + "force_ramp_interval": "加力间隔", + "grip_settle_seconds": "夹持稳定时间", + "adaptive_grip_enabled": "启用自适应夹持", + "adaptive_shear_start_force": "自适应起始切向力", + "adaptive_shear_full_force": "自适应满量程切向力", + "adaptive_force_min": "自适应最小力", + "adaptive_force_max": "自适应最大力", + "adaptive_force_step": "自适应加力步进", + "adaptive_force_interval": "自适应加力间隔", + "release_arm_delay_seconds": "释放检测延时", + "release_normal_change": "释放法向变化", + "release_shear_change": "释放切向变化", + "rearm_seconds": "重新武装休息时间", + "recover_normal_force": "恢复法向阈值", + "recover_shear_force": "恢复切向阈值", + "recover_stable_seconds": "恢复稳定时间", + "open_position_recover_enabled": "启用位置恢复", + "open_position_tolerance": "张开位置容差", + "open_position_stable_seconds": "张开位置稳定时间", + "open_position_check_interval": "位置检查间隔", + "hold_fallback_pos": "保持兜底位置", + "normal_force_calibration": "法向力标定 JSON", + "shear_force_calibration": "切向力标定 JSON", + } + def __init__(self, parent): super().__init__(parent) self.viewer = parent - self.setWindowTitle("画面设置") + self.setObjectName("SettingsPanel") + self.setWindowFlags(self.windowFlags() & ~Qt.WindowContextHelpButtonHint) + self.setWindowTitle("设置") + self.setWindowIcon(_load_company_icon(64)) self.setModal(False) - self.setMinimumWidth(360) - - layout = QVBoxLayout(self) - layout.setContentsMargins(12, 12, 12, 12) - layout.setSpacing(10) - + self.resize(980, 680) + self.config_path = self._resolve_config_path() + self.config_data = {} + self.controls = {} self.labels = {} + self._drag_start_global = None + self._drag_start_pos = None + self._user_moved = False + + self._load_config() + self._build_ui() + self._refresh_labels() + + def _resolve_config_path(self): + parser = argparse.ArgumentParser(add_help=False) + parser.add_argument("--control-config", default=str(REPO_ROOT / "config" / "gripper.json")) + try: + args, _ = parser.parse_known_args(self.viewer.argv) + return Path(args.control_config) + except Exception: + return REPO_ROOT / "config" / "gripper.json" + + def _load_config(self): + try: + with open(self.config_path, "r", encoding="utf-8") as f: + data = json.load(f) + if isinstance(data, dict): + self.config_data = data + else: + self.config_data = {} + except Exception: + self.config_data = {} + + def _build_ui(self): + outer = QVBoxLayout(self) + outer.setContentsMargins(12, 12, 12, 12) + outer.setSpacing(10) + + header = QHBoxLayout() + header.setSpacing(8) + title = QLabel("设置") + title.setObjectName("SectionTitle") + path_label = QLabel(f"配置文件: {self.config_path}") + path_label.setObjectName("MetricLabel") + title.installEventFilter(self) + path_label.installEventFilter(self) + self.panel_close_button = TopIconButton("close", "关闭设置面板") + self.panel_close_button.clicked.connect(self.close) + header.addWidget(title) + header.addWidget(path_label, 1) + header.addWidget(self.panel_close_button) + outer.addLayout(header) + + body = QHBoxLayout() + body.setSpacing(10) + outer.addLayout(body, 1) + + self.nav = QListWidget() + self.nav.setFixedWidth(180) + body.addWidget(self.nav) + + self.stack = QStackedWidget() + body.addWidget(self.stack, 1) + + self._add_page("画面", self._build_visual_page()) + used_keys = set() + for title, keys in self.CONFIG_GROUPS: + present = [key for key in keys if key in self.config_data] + if not present: + continue + used_keys.update(present) + self._add_page(title, self._build_config_page(title, present)) + + other_keys = [ + key + for key in self.config_data + if key not in used_keys + ] + if other_keys: + self._add_page("其他", self._build_config_page("其他", other_keys)) + + footer = QHBoxLayout() + footer.addStretch(1) + reload_button = AnimatedButton("重新载入") + reload_button.setObjectName("ManualButton") + reload_button.clicked.connect(self._reload) + save_button = AnimatedButton("保存到 gripper.json") + save_button.setObjectName("ManualButton") + save_button.clicked.connect(self._save) + close_button = AnimatedButton("关闭") + close_button.setObjectName("ManualButton") + close_button.clicked.connect(self.close) + footer.addWidget(reload_button) + footer.addWidget(save_button) + footer.addWidget(close_button) + outer.addLayout(footer) + + self.nav.currentRowChanged.connect(self.stack.setCurrentIndex) + self.nav.setCurrentRow(0) + + def _add_page(self, title, widget): + self.nav.addItem(title) + self.stack.addWidget(widget) + + def _build_visual_page(self): + page = QWidget() + layout = QVBoxLayout(page) + layout.setContentsMargins(8, 8, 8, 8) + layout.setSpacing(10) for side, title in (("left", "左侧光流"), ("right", "右侧光流")): panel = QFrame() panel.setObjectName("Panel") panel_layout = QVBoxLayout(panel) panel_layout.setContentsMargins(10, 8, 10, 10) panel_layout.setSpacing(8) - label = QLabel() label.setObjectName("MetricLabel") self.labels[side] = label panel_layout.addWidget(label) - row = QHBoxLayout() row.setSpacing(6) for text, action in ( @@ -437,8 +2201,113 @@ class VisualSettingsDialog(QDialog): row.addWidget(button) panel_layout.addLayout(row) layout.addWidget(panel) + layout.addStretch(1) + return page - self._refresh_labels() + def _build_config_page(self, title, keys): + scroll = QScrollArea() + scroll.setWidgetResizable(True) + page = QWidget() + grid = QGridLayout(page) + grid.setContentsMargins(10, 10, 10, 10) + grid.setHorizontalSpacing(12) + grid.setVerticalSpacing(8) + + heading = QLabel(title) + heading.setObjectName("SectionTitle") + grid.addWidget(heading, 0, 0, 1, 3) + + row = 1 + for key in keys: + value = self.config_data.get(key) + name = QLabel(self.PARAM_LABELS.get(key, key)) + name.setObjectName("MetricLabel") + key_label = QLabel(key) + key_label.setObjectName("Subtle") + control = self._make_control(key, value) + grid.addWidget(name, row, 0) + grid.addWidget(control, row, 1) + grid.addWidget(key_label, row, 2) + row += 1 + + grid.setColumnStretch(1, 1) + grid.setRowStretch(row, 1) + scroll.setWidget(page) + return scroll + + def _make_control(self, key, value): + if isinstance(value, bool): + control = QCheckBox() + control.setChecked(value) + elif isinstance(value, (dict, list)): + control = QTextEdit() + control.setPlainText(json.dumps(value, ensure_ascii=False, indent=2)) + control.setMinimumHeight(220) + control.setAcceptRichText(False) + else: + control = QLineEdit() + control.setText("null" if value is None else str(value)) + control.setToolTip(key) + self.controls[key] = control + return control + + def _control_value(self, key, original): + control = self.controls[key] + if isinstance(original, bool): + return bool(control.isChecked()) + if isinstance(original, (dict, list)): + return json.loads(control.toPlainText()) + text = control.text().strip() + if original is None: + if text.lower() in {"", "null", "none"}: + return None + try: + return json.loads(text) + except Exception: + return text + if isinstance(original, int) and not isinstance(original, bool): + return int(float(text)) + if isinstance(original, float): + return float(text) + return text + + def _set_control_value(self, key, value): + control = self.controls.get(key) + if control is None: + return + if isinstance(control, QCheckBox): + control.setChecked(bool(value)) + elif isinstance(control, QTextEdit): + control.setPlainText(json.dumps(value, ensure_ascii=False, indent=2)) + elif isinstance(control, QLineEdit): + control.setText("null" if value is None else str(value)) + + def _reload(self): + self._load_config() + for key, value in self.config_data.items(): + self._set_control_value(key, value) + + def _save(self): + updated = dict(self.config_data) + try: + for key, original in self.config_data.items(): + if key in self.controls: + updated[key] = self._control_value(key, original) + except Exception as exc: + QMessageBox.warning(self, "保存失败", f"参数格式不正确:\n{exc}") + return + + try: + self.config_path.parent.mkdir(parents=True, exist_ok=True) + with open(self.config_path, "w", encoding="utf-8") as f: + json.dump(updated, f, ensure_ascii=False, indent=2) + f.write("\n") + except Exception as exc: + QMessageBox.warning(self, "保存失败", f"无法写入配置文件:\n{exc}") + return + + self.config_data = updated + QMessageBox.information(self, "已保存", "配置已写入 gripper.json。重启控制程序后完整生效。") def _adjust(self, side, action): self.viewer.adjust_visual_transform(side, action) @@ -454,6 +2323,41 @@ class VisualSettingsDialog(QDialog): f"{title}: 旋转 {angle}deg / 水平镜像 {flip_x} / 垂直镜像 {flip_y}" ) + def eventFilter(self, obj, event): + event_type = event.type() + if event_type == QEvent.MouseButtonPress and event.button() == Qt.LeftButton: + self._begin_drag(event.globalPos()) + return True + if event_type == QEvent.MouseMove and self._drag_start_global is not None: + self._continue_drag(event.globalPos()) + return True + if event_type == QEvent.MouseButtonRelease and event.button() == Qt.LeftButton: + self._end_drag() + return True + return super().eventFilter(obj, event) + + def _begin_drag(self, global_pos): + self._drag_start_global = global_pos + self._drag_start_pos = self.pos() + self.setCursor(Qt.SizeAllCursor) + + def _continue_drag(self, global_pos): + if self._drag_start_global is None or self._drag_start_pos is None: + return + delta = global_pos - self._drag_start_global + target = self._drag_start_pos + delta + parent = self.parentWidget() + if parent is not None: + target.setX(max(0, min(target.x(), parent.width() - self.width()))) + target.setY(max(0, min(target.y(), parent.height() - self.height()))) + self.move(target) + self._user_moved = True + + def _end_drag(self): + self._drag_start_global = None + self._drag_start_pos = None + self.setCursor(Qt.ArrowCursor) + class GripperDemo02Viewer(QMainWindow): def __init__(self, argv=None): @@ -464,6 +2368,7 @@ class GripperDemo02Viewer(QMainWindow): raise RuntimeError(f"pyqtgraph import failed: {_PYQTGRAPH_IMPORT_ERROR}") self.setWindowTitle("Gripper Demo 02") + self.setWindowIcon(_load_company_icon(64)) self.setStyleSheet(STYLE) self.controller = None self.control_thread = None @@ -472,10 +2377,18 @@ class GripperDemo02Viewer(QMainWindow): self.controller_finished = False self.restart_pending = False self.button_state = "idle" + self.control_mode = "smart" + self._sensor_fps_refresh_time = 0.0 + self._left_sensor_fps_text = "--" + self._right_sensor_fps_text = "--" self.start_time = time.time() self.force_history = [] self.argv = argv self.visual_settings_dialog = None + self._pending_runtime_speed = None + self._runtime_speed_timer = QTimer(self) + self._runtime_speed_timer.setSingleShot(True) + self._runtime_speed_timer.timeout.connect(self._apply_pending_runtime_speed) self.visual_transforms = { "left": {"rotation_angle": 0, "flip_x": False, "flip_y": False}, "right": {"rotation_angle": 0, "flip_x": False, "flip_y": False}, @@ -488,17 +2401,55 @@ class GripperDemo02Viewer(QMainWindow): self.timer.timeout.connect(self._refresh) self.timer.start(33) + def _control_config_path(self): + config_path = REPO_ROOT / "config" / "gripper.json" + args = list(self.argv or []) + for idx, arg in enumerate(args): + if arg == "--control-config" and idx + 1 < len(args): + config_path = Path(args[idx + 1]) + break + if str(arg).startswith("--control-config="): + config_path = Path(str(arg).split("=", 1)[1]) + break + if not config_path.is_absolute(): + config_path = REPO_ROOT / config_path + return config_path + + def _load_position_config(self): + open_pos = 0 + close_pos = 9000 + try: + with open(self._control_config_path(), "r", encoding="utf-8") as f: + data = json.load(f) + open_pos = int(data.get("open_pos", open_pos)) + close_pos = int(data.get("close_pos", close_pos)) + except Exception: + pass + position_min = min(open_pos, close_pos) + position_max = max(open_pos, close_pos) + if position_min == position_max: + position_max = position_min + 1 + return open_pos, close_pos, position_min, position_max + def _build_ui(self): root = QWidget() self.setCentralWidget(root) outer = QVBoxLayout(root) - outer.setContentsMargins(8, 8, 8, 8) - outer.setSpacing(8) + outer.setContentsMargins(10, 10, 10, 10) + outer.setSpacing(10) - top = QHBoxLayout() + top_bar = QFrame() + top_bar.setObjectName("TopBar") + top = QHBoxLayout(top_bar) + top.setContentsMargins(12, 8, 12, 8) top.setSpacing(8) - title = QLabel("Orisys 夹爪") - title.setObjectName("Title") + logo = QLabel() + logo.setObjectName("BrandMark") + logo.setFixedSize(36, 36) + logo.setAlignment(Qt.AlignCenter) + logo.setPixmap(_load_company_logo(34)) + logo.setToolTip("公司 Logo") + logo.setToolTipDuration(3500) self.status_panel = QFrame() self.status_panel.setObjectName("StatusPanel") status_layout = QVBoxLayout(self.status_panel) @@ -510,15 +2461,12 @@ class GripperDemo02Viewer(QMainWindow): self.action_label.setObjectName("StatusSub") status_layout.addWidget(self.status_chip) status_layout.addWidget(self.action_label) - self.start_button = AnimatedButton("启动") - self.start_button.setObjectName("StartButton") + self.start_button = TopIconButton("start", "启动控制程序") self.start_button.clicked.connect(self.start_control) - self.stop_button = AnimatedButton("停止") - self.stop_button.setObjectName("StopButton") + self.stop_button = TopIconButton("stop", "停止控制程序") self.stop_button.clicked.connect(self.stop_control) self.stop_button.setEnabled(False) - self.restart_button = AnimatedButton("重启") - self.restart_button.setObjectName("RestartButton") + self.restart_button = TopIconButton("restart", "重启控制程序") self.restart_button.clicked.connect(self.restart_control) self.manual_open_button = AnimatedButton("夹爪张开") self.manual_open_button.setObjectName("ManualButton") @@ -529,51 +2477,151 @@ class GripperDemo02Viewer(QMainWindow): self.manual_hold_button = AnimatedButton("保持") self.manual_hold_button.setObjectName("HoldButton") self.manual_hold_button.clicked.connect(self.toggle_manual_hold) - self.visual_settings_button = AnimatedButton("画面设置") - self.visual_settings_button.setObjectName("ManualButton") + self.visual_settings_button = TopIconButton("settings", "打开画面设置") self.visual_settings_button.clicked.connect(self.open_visual_settings) + self.minimize_button = TopIconButton("minimize", "最小化窗口") + self.minimize_button.clicked.connect(self.showMinimized) + self.fullscreen_button = TopIconButton("fullscreen", "全屏/窗口切换") + self.fullscreen_button.clicked.connect(self.toggle_fullscreen) + self.close_button = TopIconButton("close", "退出程序") + self.close_button.clicked.connect(self.close) self._set_button_state("idle") - top.addWidget(title) + top.addWidget(logo) top.addWidget(self.status_panel) top.addStretch(1) + top.addWidget(self.visual_settings_button) 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) - top.addWidget(self.visual_settings_button) - outer.addLayout(top) + top.addWidget(self.minimize_button) + top.addWidget(self.fullscreen_button) + top.addWidget(self.close_button) + outer.addWidget(top_bar) - main_splitter = QSplitter(Qt.Horizontal) - main_splitter.setChildrenCollapsible(False) - outer.addWidget(main_splitter, 1) + center_splitter = QSplitter(Qt.Horizontal) + center_splitter.setChildrenCollapsible(False) + outer.addWidget(center_splitter, 1) - flow_splitter = QSplitter(Qt.Vertical) - flow_splitter.setChildrenCollapsible(False) - self.left_flow = ImageBox("左侧光流") - self.right_flow = ImageBox("右侧光流") + visual_panel = QFrame() + visual_panel.setObjectName("ControlPanel") + visual_layout = QGridLayout(visual_panel) + visual_layout.setContentsMargins(10, 10, 10, 10) + visual_layout.setSpacing(10) + self.gripper_stage = GripperStage() + self.left_flow = self.gripper_stage.left_flow + self.right_flow = self.gripper_stage.right_flow self.left_flow.set_transform_label(self.visual_transforms["left"]) self.right_flow.set_transform_label(self.visual_transforms["right"]) - flow_splitter.addWidget(self.left_flow) - flow_splitter.addWidget(self.right_flow) - flow_splitter.setStretchFactor(0, 1) - flow_splitter.setStretchFactor(1, 1) - flow_splitter.setSizes([360, 360]) - main_splitter.addWidget(flow_splitter) + visual_layout.addWidget(self.gripper_stage, 0, 0) + visual_layout.setColumnStretch(0, 1) + center_splitter.addWidget(visual_panel) - side_widget = QWidget() - side = QVBoxLayout(side_widget) - side.setContentsMargins(0, 0, 0, 0) - side.setSpacing(8) - main_splitter.addWidget(side_widget) - main_splitter.setStretchFactor(0, 2) - main_splitter.setStretchFactor(1, 4) - main_splitter.setSizes([520, 980]) + plot_panel = FloatingPanel(self.gripper_stage) + plot_panel.setMinimumSize(360, 260) + plot_layout = QVBoxLayout(plot_panel) + plot_layout.setContentsMargins(8, 6, 8, 8) + plot_layout.setSpacing(4) + plot_title = QLabel("曲线图区域") + plot_title.setObjectName("SectionTitle") + plot_title.setStyleSheet("color: #111827; background: transparent;") + plot_panel.set_drag_handle(plot_title) + plot_layout.addWidget(plot_title) - metrics_grid = QGridLayout() - metrics_grid.setSpacing(8) - side.addLayout(metrics_grid) + pg.setConfigOption("background", "#ffffff") + pg.setConfigOption("foreground", "#667085") + pg.setConfigOptions(antialias=True) + self.plot_widget = pg.GraphicsLayoutWidget() + self.plot_widget.ci.setSpacing(10) + self.plot_widget.ci.setContentsMargins(4, 2, 4, 4) + self.plot_widget.setMinimumHeight(220) + plot_layout.addWidget(self.plot_widget, 1) + self.plot_panel = plot_panel + center_splitter.setStretchFactor(0, 1) + center_splitter.setSizes([1200]) + + gripper_control = FloatingPanel(self.gripper_stage) + gripper_layout = QGridLayout(gripper_control) + gripper_layout.setContentsMargins(8, 6, 8, 8) + gripper_layout.setHorizontalSpacing(8) + gripper_layout.setVerticalSpacing(6) + gripper_title = QLabel("夹爪控制区域") + gripper_title.setObjectName("SectionTitle") + gripper_title.setAlignment(Qt.AlignLeft | Qt.AlignTop) + gripper_control.set_drag_handle(gripper_title) + self.speed_control = SliderControl("速度", 0, 100, 0, step=5, suffix="%") + self.force_control = SliderControl("最大力", 0, 100, 0, step=5, suffix="%") + self.speed_control.slider.valueChanged.connect(self.schedule_runtime_speed) + self.manual_mode_button = AnimatedButton("手动模式") + self.manual_mode_button.setObjectName("ViewToggle") + self.manual_mode_button.setCheckable(True) + self.manual_mode_button.setToolTip("切换到手动控制") + self.smart_mode_button = AnimatedButton("智能模式") + self.smart_mode_button.setObjectName("ViewToggle") + self.smart_mode_button.setCheckable(True) + self.smart_mode_button.setToolTip("切换到自动智能控制") + self.mode_button_group = QButtonGroup(self) + self.mode_button_group.setExclusive(True) + self.mode_button_group.addButton(self.smart_mode_button) + self.mode_button_group.addButton(self.manual_mode_button) + self.smart_mode_button.setChecked(True) + self.smart_mode_button.clicked.connect(lambda: self.set_control_mode("smart")) + self.manual_mode_button.clicked.connect(lambda: self.set_control_mode("manual")) + open_pos, close_pos, position_min, position_max = self._load_position_config() + self.gripper_open_pos = open_pos + self.gripper_close_pos = close_pos + self.position_control = SliderControl( + "夹爪位置", + position_min, + position_max, + open_pos, + step=100, + ) + self.position_control.slider.sliderReleased.connect(self.move_to_slider_position) + self.position_control.minus_button.clicked.connect(self.move_to_slider_position) + self.position_control.plus_button.clicked.connect(self.move_to_slider_position) + gripper_layout.addWidget(gripper_title, 0, 0, 1, 2, Qt.AlignLeft | Qt.AlignTop) + gripper_layout.addWidget(self.manual_mode_button, 0, 3) + gripper_layout.addWidget(self.smart_mode_button, 0, 4) + gripper_layout.addWidget(self.speed_control, 1, 0, 1, 4) + gripper_layout.addWidget(self.force_control, 2, 0, 1, 4) + gripper_layout.addWidget(self.position_control, 3, 0, 1, 4) + gripper_layout.addWidget(self.manual_hold_button, 1, 4) + gripper_layout.addWidget(self.manual_open_button, 2, 4) + gripper_layout.addWidget(self.manual_close_button, 3, 4) + for col in range(4): + gripper_layout.setColumnStretch(col, 1) + gripper_layout.setColumnStretch(4, 0) + gripper_layout.setRowStretch(0, 0) + gripper_layout.setRowStretch(1, 0) + gripper_layout.setRowStretch(2, 0) + gripper_layout.setRowStretch(3, 0) + gripper_layout.setRowStretch(4, 1) + self.gripper_control_panel = gripper_control + + sensor_panel = FloatingPanel(self.gripper_stage) + sensor_layout = QGridLayout(sensor_panel) + sensor_layout.setContentsMargins(8, 6, 8, 8) + sensor_layout.setHorizontalSpacing(6) + sensor_layout.setVerticalSpacing(5) + sensor_title = QLabel("传感器控制区域") + sensor_title.setObjectName("SectionTitle") + sensor_title.setAlignment(Qt.AlignLeft | Qt.AlignTop) + sensor_panel.set_drag_handle(sensor_title) + self.left_sensor_reset_button = TopIconButton("restart", "重启左传感器") + self.left_sensor_reset_button.setEnabled(False) + self.left_sensor_reset_button.clicked.connect( + lambda: self.send_manual_command("restart_left_sensor") + ) + self.right_sensor_reset_button = TopIconButton("restart", "重启右传感器") + self.right_sensor_reset_button.setEnabled(False) + self.right_sensor_reset_button.clicked.connect( + lambda: self.send_manual_command("restart_right_sensor") + ) + sensor_layout.addWidget(sensor_title, 0, 0, 1, 8, Qt.AlignLeft | Qt.AlignTop) + self.left_sensor_dot = StatusDot("左传感器", self.left_sensor_reset_button) + self.right_sensor_dot = StatusDot("右传感器", self.right_sensor_reset_button) + sensor_layout.addWidget(self.left_sensor_dot, 1, 0, 1, 4) + sensor_layout.addWidget(self.right_sensor_dot, 1, 4, 1, 4) self.left_force = MetricBox("左法向力", "--") self.right_force = MetricBox("右法向力", "--") self.left_shear_box = MetricBox("左切向力", "--") @@ -581,67 +2629,150 @@ class GripperDemo02Viewer(QMainWindow): self.force_pct_box = MetricBox("夹爪当前力比例（%）", "--") 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.left_shear_box, - self.right_shear_box, - 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) - - pg.setConfigOption("background", "#000000") - pg.setConfigOption("foreground", "#777777") - pg.setConfigOptions(antialias=True) - self.plot_widget = pg.GraphicsLayoutWidget() - self.plot_widget.setMinimumHeight(240) - side.addWidget(self.plot_widget, 1) + sensor_layout.addWidget(self.left_force, 2, 0, 1, 2) + sensor_layout.addWidget(self.right_force, 2, 2, 1, 2) + sensor_layout.addWidget(self.left_shear_box, 2, 4, 1, 2) + sensor_layout.addWidget(self.right_shear_box, 2, 6, 1, 2) + sensor_layout.addWidget(self.force_pct_box, 3, 0, 1, 3) + sensor_layout.addWidget(self.target_force_box, 3, 3, 1, 3) + sensor_layout.addWidget(self.speed_box, 3, 6, 1, 2) + for col in range(8): + sensor_layout.setColumnStretch(col, 1) + for row in range(4): + sensor_layout.setRowStretch(row, 0) + sensor_layout.setRowStretch(4, 1) + self._set_button_state(self.button_state) + self.sensor_control_panel = sensor_panel + self.gripper_stage.add_floating_control_panel(plot_panel, "plot") + self.gripper_stage.add_floating_control_panel(gripper_control, "gripper") + self.gripper_stage.add_floating_control_panel(sensor_panel, "sensor") def _configure_plot(self): self.plots = [] self.curves = {} + self.baseline_curves = {} + self.fill_items = {} + self.plot_by_key = {} + self.plot_y_ranges = {} + self.plot_title_templates = {} + self.plot_title_labels = {} + title_font = QFont(PLOT_FONT_FAMILY, PLOT_TITLE_FONT_SIZE, QFont.Bold) + axis_font = QFont(PLOT_FONT_FAMILY, PLOT_AXIS_FONT_SIZE) 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), + ("left_normal", "左法向力", 0, 0), + ("left_shear_x", "左切向力 X", 1, 0), + ("left_shear_y", "左切向力 Y", 2, 0), + ("right_normal", "右法向力", 0, 1), + ("right_shear_x", "右切向力 X", 1, 1), + ("right_shear_y", "右切向力 Y", 2, 1), ] - for idx, (key, title, color, y_min, y_max) in enumerate(plot_specs): - row = idx // 2 - col = idx % 2 + for key, title, row, col in plot_specs: plot = self.plot_widget.addPlot(row=row, col=col) - plot.setTitle(title, color="#999999", size="11pt") - plot.titleLabel.setFont(QFont("SimHei", 11)) + plot.setTitle("", color="#1f2937", size=f"{PLOT_TITLE_FONT_SIZE}pt") + plot.titleLabel.setFont(title_font) + plot.titleLabel.setFixedHeight(18) + plot.titleLabel.setText(title, color="#1f2937") + plot.setContentsMargins(6, 6, 6, 6) + plot.setMenuEnabled(False) + plot.setMouseEnabled(x=False, y=False) 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.showGrid(x=True, y=True, alpha=0.22) + plot.hideButtons() + plot.setLabel( + "bottom", + "时间", + units="s", + color="#596579", + **{"font-size": f"{PLOT_AXIS_FONT_SIZE}pt"}, + ) + plot.setLabel( + "left", + "力", + units="N", + color="#596579", + **{"font-size": f"{PLOT_AXIS_FONT_SIZE}pt"}, + ) + view_box = plot.getViewBox() + view_box.setBackgroundColor("#fbfdff") + view_box.setBorder(pg.mkPen("#cfd8e3", width=1)) + view_box.setDefaultPadding(0.02) + for axis_name in ("bottom", "left"): + axis = plot.getAxis(axis_name) + axis.setPen(pg.mkPen("#d8e0eb", width=1)) + axis.setTextPen(pg.mkPen("#667085")) + axis.setStyle(tickFont=axis_font, tickTextOffset=3, showValues=True) + plot.getAxis("left").setWidth(46) + plot.getAxis("bottom").setHeight(34) plot.getAxis("bottom").setTickSpacing(major=1.0, minor=1.0) - plot.setYRange(y_min, y_max, padding=0) - plot.setLimits(yMin=y_min, yMax=y_max) + plot.setYRange(PLOT_Y_DEFAULT_MIN_N, PLOT_Y_DEFAULT_MAX_N, padding=0) plot.enableAutoRange(axis=pg.ViewBox.YAxis, enable=False) + self.plot_y_ranges[key] = (PLOT_Y_DEFAULT_MIN_N, PLOT_Y_DEFAULT_MAX_N) self.plots.append(plot) - self.curves[key] = plot.plot(pen=pg.mkPen(color, width=2)) + self.plot_by_key[key] = plot + self.plot_title_templates[key] = title + self.plot_title_labels[key] = plot.titleLabel + baseline = plot.plot(pen=pg.mkPen(None)) + curve = plot.plot(pen=pg.mkPen(CURVE_COLOR, width=2.0)) + fill = pg.FillBetweenItem( + curve, + baseline, + brush=pg.mkBrush(83, 109, 206, 24), + ) + plot.addItem(fill) + self.baseline_curves[key] = baseline + self.fill_items[key] = fill + self.curves[key] = curve + + @staticmethod + def _plot_axis_step(value): + value = abs(float(value)) + if value <= 2.0: + return 0.5 + if value <= 5.0: + return 1.0 + return 2.0 + + def _reset_plot_y_ranges(self): + if not hasattr(self, "plot_y_ranges"): + return + for key, plot in getattr(self, "plot_by_key", {}).items(): + self.plot_y_ranges[key] = (PLOT_Y_DEFAULT_MIN_N, PLOT_Y_DEFAULT_MAX_N) + plot.setYRange(PLOT_Y_DEFAULT_MIN_N, PLOT_Y_DEFAULT_MAX_N, padding=0) + + def _update_plot_y_range(self, key, values): + plot = self.plot_by_key.get(key) + if plot is None or values.size == 0: + return + finite = values[np.isfinite(values)] + if finite.size == 0: + return + current_min, current_max = self.plot_y_ranges.get( + key, + (PLOT_Y_DEFAULT_MIN_N, PLOT_Y_DEFAULT_MAX_N), + ) + data_min = float(np.min(finite)) + data_max = float(np.max(finite)) + next_min = current_min + next_max = current_max + if data_max > current_max: + step = self._plot_axis_step(data_max) + next_max = math.ceil((data_max + step * 0.12) / step) * step + if next_min != PLOT_Y_DEFAULT_MIN_N: + next_min = PLOT_Y_DEFAULT_MIN_N + if next_min != current_min or next_max != current_max: + if next_max - next_min < 0.5: + next_max = next_min + 0.5 + self.plot_y_ranges[key] = (next_min, next_max) + plot.setYRange(next_min, next_max, padding=0) def _set_button_state(self, state): labels = { - "idle": ("启动", "停止", "重启"), - "starting": ("启动中", "停止", "重启"), - "running": ("启动", "停止", "重启"), - "stopping": ("启动", "停止中", "重启"), - "restarting": ("启动", "停止", "重启中"), + "idle": ("启动控制程序", "停止控制程序", "重启控制程序"), + "starting": ("正在启动控制程序", "停止控制程序", "重启控制程序"), + "running": ("控制程序运行中", "停止控制程序", "重启控制程序"), + "stopping": ("启动控制程序", "正在停止控制程序", "重启控制程序"), + "restarting": ("启动控制程序", "停止控制程序", "正在重启控制程序"), } enabled = { "idle": (True, False, True), @@ -653,24 +2784,119 @@ class GripperDemo02Viewer(QMainWindow): self.button_state = state start_text, stop_text, restart_text = labels[state] start_enabled, stop_enabled, restart_enabled = enabled[state] - self.start_button.setText(start_text) - self.stop_button.setText(stop_text) - self.restart_button.setText(restart_text) + self.start_button.setToolTip(start_text) + self.stop_button.setToolTip(stop_text) + self.restart_button.setToolTip(restart_text) 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) + mode_enabled = state in ("idle", "running") + manual_enabled = state == "running" and self.control_mode == "manual" + action_enabled = state == "running" + sensor_restart_enabled = state == "running" + self.manual_open_button.setEnabled(action_enabled) + self.manual_close_button.setEnabled(action_enabled) + self.manual_hold_button.setEnabled(action_enabled) + if hasattr(self, "speed_control"): + self.speed_control.set_interactive_enabled(manual_enabled) + if hasattr(self, "force_control"): + self.force_control.set_interactive_enabled(manual_enabled) + if hasattr(self, "position_control"): + self.position_control.set_interactive_enabled(manual_enabled) + if hasattr(self, "manual_mode_button"): + self.manual_mode_button.setEnabled(mode_enabled) + if hasattr(self, "smart_mode_button"): + self.smart_mode_button.setEnabled(mode_enabled) + if hasattr(self, "left_sensor_reset_button"): + self.left_sensor_reset_button.setEnabled(sensor_restart_enabled) + if hasattr(self, "right_sensor_reset_button"): + self.right_sensor_reset_button.setEnabled(sensor_restart_enabled) + + def set_control_mode(self, mode): + if mode not in ("smart", "manual"): + return + self.control_mode = mode + if self.controller is not None: + try: + self.controller.set_control_mode(mode) + except Exception as exc: + print(f"control mode update failed: {exc}") + if hasattr(self, "smart_mode_button"): + self.smart_mode_button.blockSignals(True) + self.smart_mode_button.setChecked(mode == "smart") + self.smart_mode_button.blockSignals(False) + if hasattr(self, "manual_mode_button"): + self.manual_mode_button.blockSignals(True) + self.manual_mode_button.setChecked(mode == "manual") + self.manual_mode_button.blockSignals(False) + self._set_button_state(self.button_state) def open_visual_settings(self): if self.visual_settings_dialog is None: self.visual_settings_dialog = VisualSettingsDialog(self) self.visual_settings_dialog._refresh_labels() - self.visual_settings_dialog.show() - self.visual_settings_dialog.raise_() - self.visual_settings_dialog.activateWindow() + if self.isFullScreen(): + self._show_visual_settings_embedded() + else: + self._show_visual_settings_window() + + def _show_visual_settings_embedded(self): + dialog = self.visual_settings_dialog + dialog.hide() + dialog._user_moved = False + dialog.setParent(self.centralWidget()) + dialog.setWindowFlags(Qt.Widget) + dialog.setWindowIcon(_load_company_icon(64)) + self._position_embedded_settings() + dialog.show() + dialog.raise_() + + def _show_visual_settings_window(self): + dialog = self.visual_settings_dialog + dialog.hide() + flags = ( + Qt.Dialog + | Qt.WindowTitleHint + | Qt.WindowSystemMenuHint + | Qt.WindowCloseButtonHint + | Qt.WindowMinimizeButtonHint + ) + dialog.setParent(self) + dialog.setWindowFlags(flags & ~Qt.WindowContextHelpButtonHint) + dialog.setWindowIcon(_load_company_icon(64)) + dialog.resize(980, 680) + dialog.show() + dialog.raise_() + dialog.activateWindow() + + def _position_embedded_settings(self): + dialog = self.visual_settings_dialog + parent = self.centralWidget() + if dialog is None or parent is None: + return + if getattr(dialog, "_user_moved", False): + dialog.move( + max(0, min(dialog.x(), parent.width() - dialog.width())), + max(0, min(dialog.y(), parent.height() - dialog.height())), + ) + return + margin = 24 + available_w = max(320, parent.width() - margin * 2) + available_h = max(260, parent.height() - margin * 2) + width = min(980, max(760, int(parent.width() * 0.72)), available_w) + height = min(680, max(560, int(parent.height() * 0.78)), available_h) + x = max(margin, (parent.width() - width) // 2) + y = max(margin, (parent.height() - height) // 2) + dialog.setGeometry(x, y, width, height) + + def toggle_fullscreen(self): + if self.isFullScreen(): + self.showNormal() + self.resize(1500, 900) + else: + self.showFullScreen() + if self.visual_settings_dialog is not None and self.visual_settings_dialog.isVisible(): + QTimer.singleShot(0, self.open_visual_settings) def adjust_visual_transform(self, side, action): transform = self.visual_transforms[side] @@ -688,14 +2914,58 @@ class GripperDemo02Viewer(QMainWindow): else: self.right_flow.set_transform_label(transform) - def send_manual_command(self, command): + def send_manual_command(self, command, position=None): if self.controller is None or self.control_thread is None or not self.control_thread.is_alive(): return + speed_pct = self.speed_control.value() + force_pct = self.force_control.value() + self.apply_runtime_speed(speed_pct) try: - self.controller.request_manual_command(command) + self.controller.request_manual_command( + command, + speed_pct=speed_pct, + force_pct=force_pct, + position=position, + ) except Exception as exc: print(f"manual command failed: {exc}") + def move_to_position(self, position): + position = max( + self.position_control.slider.minimum(), + min(self.position_control.slider.maximum(), int(position)), + ) + self.position_control.set_value(position, force=True) + self.send_manual_command("move_position", position=position) + + def move_to_target_position(self): + self.move_to_slider_position() + + def move_to_slider_position(self): + self.move_to_position(self.position_control.value()) + + def nudge_position(self, delta): + self.position_control.nudge(delta) + self.move_to_slider_position() + + def schedule_runtime_speed(self, value): + self._pending_runtime_speed = int(value) + self._runtime_speed_timer.start(180) + + def _apply_pending_runtime_speed(self): + if self._pending_runtime_speed is None: + return + self.apply_runtime_speed(self._pending_runtime_speed) + + def apply_runtime_speed(self, value): + self._pending_runtime_speed = int(value) + if self.controller is None: + return + try: + self.controller.config.speed = int(value) + except Exception as exc: + print(f"runtime speed update failed: {exc}") + def toggle_manual_hold(self): command = "release" if self.manual_hold_button.text() == "释放" else "hold" self.send_manual_command(command) @@ -704,6 +2974,7 @@ class GripperDemo02Viewer(QMainWindow): if self.control_thread is not None and self.control_thread.is_alive(): return self.force_history.clear() + self._reset_plot_y_ranges() self.start_time = time.time() self._set_button_state("restarting" if from_restart else "starting") self.controller_finished = False @@ -714,6 +2985,10 @@ class GripperDemo02Viewer(QMainWindow): include_visuals=True, visual_size=320, ) + try: + self.controller.set_control_mode(self.control_mode) + except Exception as exc: + print(f"control mode init failed: {exc}") self.control_thread = threading.Thread( target=self._run_controller, daemon=True, @@ -793,35 +3068,55 @@ class GripperDemo02Viewer(QMainWindow): self.speed_box.set_value( str(status.get("speed_pct", "--")), ) - self.trigger_box.set_value( - f"{int(bool(status.get('trigger')))} / {int(bool(status.get('trigger_armed')))}", - ) - self.contact_box.set_value( - str(int(bool(status.get("grip_contact")))), - ) - self.release_box.set_value( - str(int(bool(status.get("release_armed")))), - ) + if hasattr(self, "position_control"): + gripper_position = status.get("gripper_position") + self.position_control.set_value(gripper_position) + self.speed_control.set_value(status.get("speed_pct", 0) or 0) + self.force_control.set_value(status.get("force_pct", 0) or 0) + self.gripper_stage.update_state(status) left_sample = status.get("left_sample") or {} right_sample = status.get("right_sample") or {} - self.left_flow.set_image( - apply_visual_transform(left_sample.get("flow_view"), self.visual_transforms["left"]) + self.left_flow.set_images( + apply_visual_transform(left_sample.get("flow_view"), self.visual_transforms["left"]), + apply_visual_transform(left_sample.get("magnitude_view"), self.visual_transforms["left"]), ) - self.right_flow.set_image( - apply_visual_transform(right_sample.get("flow_view"), self.visual_transforms["right"]) + self.right_flow.set_images( + apply_visual_transform(right_sample.get("flow_view"), self.visual_transforms["right"]), + apply_visual_transform(right_sample.get("magnitude_view"), self.visual_transforms["right"]), + ) + now_wall = time.time() + if now_wall - self._sensor_fps_refresh_time >= 1.0: + self._sensor_fps_refresh_time = now_wall + self._left_sensor_fps_text = ( + f"{float(left_sample.get('fps', 0.0) or 0.0):.1f} FPS" + if left_sample + else "--" + ) + self._right_sensor_fps_text = ( + f"{float(right_sample.get('fps', 0.0) or 0.0):.1f} FPS" + if right_sample + else "--" + ) + self.left_sensor_dot.set_state( + bool(left_sample), + self._left_sensor_fps_text, + ) + self.right_sensor_dot.set_state( + bool(right_sample), + self._right_sensor_fps_text, ) elapsed = time.time() - self.start_time self.force_history.append( ( 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), + plot_force_value(status.get("left_normal", 0.0) or 0.0), + plot_force_value(status.get("right_normal", 0.0) or 0.0), + plot_force_value(status.get("left_shear_x", 0.0) or 0.0), + plot_force_value(status.get("left_shear_y", 0.0) or 0.0), + plot_force_value(status.get("right_shear_x", 0.0) or 0.0), + plot_force_value(status.get("right_shear_y", 0.0) or 0.0), ) ) if len(self.force_history) > PLOT_POINTS: @@ -842,7 +3137,12 @@ class GripperDemo02Viewer(QMainWindow): "right_shear_y", ] for idx, key in enumerate(keys, start=1): - self.curves[key].setData(x, data[:, idx]) + values = smooth_plot_values(np.maximum(0.0, data[:, idx])) + self.curves[key].setData(x, values) + baseline = self.baseline_curves.get(key) + if baseline is not None: + baseline.setData(x, np.zeros_like(values)) + self._update_plot_y_range(key, values) if len(x) >= 2: x_max = max(6.0, float(np.ceil(x[-1]))) x_min = max(0.0, x_max - 6.0) @@ -871,9 +3171,20 @@ class GripperDemo02Viewer(QMainWindow): self.showNormal() else: self.showFullScreen() + if self.visual_settings_dialog is not None and self.visual_settings_dialog.isVisible(): + QTimer.singleShot(0, self.open_visual_settings) return super().keyPressEvent(event) + def resizeEvent(self, event): + super().resizeEvent(event) + if ( + self.visual_settings_dialog is not None + and self.visual_settings_dialog.isVisible() + and self.visual_settings_dialog.parentWidget() is self.centralWidget() + ): + self._position_embedded_settings() + def closeEvent(self, event): self.timer.stop() self.restart_pending = False @@ -906,6 +3217,7 @@ def main(argv=None): QApplication.setAttribute(Qt.AA_EnableHighDpiScaling, True) QApplication.setAttribute(Qt.AA_UseHighDpiPixmaps, True) app = QApplication(sys.argv[:1]) + app.setWindowIcon(_load_company_icon(64)) window = GripperDemo02Viewer(controller_argv) if ui_args.windowed: window.resize(1500, 900)