夹爪可视化界面部分更新，主要是3D模型

2026-07-09 13:49:47 +08:00
parent ec15850ffa
commit 73ce84f586
30 changed files with 3645 additions and 285 deletions
@@ -9,10 +9,12 @@ venv/
 env/
 ENV/

+
 # Test and coverage output
 .pytest_cache/
 .coverage
 htmlcov/
+debug/

 # PyInstaller output
 dist/
@@ -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 个开合档位，运行中只按实际位置切换缓存，避免拖慢控制延迟。

 ## 状态说明

@@ -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():
@@ -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,
@@ -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)
@@ -0,0 +1 @@
+controller_joint_names: ['', 'Left_1_Joint', 'Left_Support_Joint', 'Left_2_Joint', 'Right_2_Joint', 'Right_1_Joint', 'Right_Support_Joint', ]
@@ -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)
+<center>Figure1 ctag2f90d rviz interface</center>  
+  
+  
+![image-2](asserts/002.png#pic_center)
+
+<center>Figure2 joint state publisher interface</center>
+
+If warning coming out like <font color="red">**Could not find the GUI, install the 'joint_state_publisher_gui' package**</font>,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)
+<center>Figure3 Robot Model</center>
+
@@ -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)
+<center>图1 ctag2f90d rviz显示图</center>  
+  
+  
+![image-2](asserts/002.png)
+<center>图2 joint state publisher显示图</center>
+
+如果出现关于<font color="red">**Could not find the GUI, install the ‘joint_state_publisher_gui‘ package**</font>，需要安装相应的软件包，在终端里输入：  
+`sudo apt-get install ros-melodic-joint-state-publisher-gui `
+
+点击RViz左侧Displays下的Fixed Frame从map改为base_link
+
+再点击左下角add按钮，添加RobotModel
+
+显示出机器模型
+
+![image-3](asserts/003.png)
+
+<center>图3 机器模型图</center>
+
@@ -0,0 +1,20 @@
+<launch>
+  <arg
+    name="model" />
+  <param
+    name="robot_description"
+    textfile="$(find crt_ctag2f90d_gripper_visualization)/urdf/crt_ctag2f90d_gripper_visualization_sync.urdf" />
+  <node
+    name="joint_state_publisher_gui"
+    pkg="joint_state_publisher_gui"
+    type="joint_state_publisher_gui" />
+  <node
+    name="robot_state_publisher"
+    pkg="robot_state_publisher"
+    type="robot_state_publisher" />
+  <node
+    name="rviz"
+    pkg="rviz"
+    type="rviz"
+    args="-d $(find crt_ctag2f90d_gripper_visualization)/urdf.rviz" />
+</launch>
@@ -0,0 +1,21 @@
+<package format="2">
+  <name>crt_ctag2f90d_gripper_visualization</name>
+  <version>1.0.0</version>
+  <description>
+    <p>URDF Description package for crt_ctag2f90d_visualization</p>
+    <p>This package contains configuration data, 3D models and launch files
+for crt_ctag2f90d_visualization robot</p>
+  </description>
+  <author>TODO</author>
+  <maintainer email="TODO@email.com" />
+  <license>BSD</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>roslaunch</depend>
+  <depend>robot_state_publisher</depend>
+  <depend>rviz</depend>
+  <depend>joint_state_publisher_gui</depend>
+  <depend>gazebo</depend>
+  <export>
+    <architecture_independent />
+  </export>
+</package>
@@ -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: <Fixed 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: <Fixed 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
@@ -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,,,,,,,,
@@ -0,0 +1,430 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
+     Commit Version: 1.6.0-4-g7f85cfe  Build Version: 1.6.7995.38578
+     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
+<robot
+  name="crt_ctag2f90d_gripper_visualization">
+  <link name="base"> 
+  </link>
+  <link
+    name="base_link">
+    <inertial>
+      <origin
+        xyz="3.84872289645122E-06 -4.41448135187968E-05 0.0553867866771657"
+        rpy="0 0 0" />
+      <mass
+        value="0.953455976099042" />
+      <inertia
+        ixx="0.00177628843801648"
+        ixy="-8.19676071724033E-10"
+        ixz="-3.00472546877836E-08"
+        iyy="0.00187418176115802"
+        iyz="-3.98009757950365E-07"
+        izz="0.000875448946929789" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://crt_ctag2f90d_gripper_visualization/meshes/base_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.615686274509804 0.63921568627451 0.674509803921569 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://crt_ctag2f90d_gripper_visualization/meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="base_link_Joint"
+    type="fixed">
+    <origin
+      xyz="0 0 0"
+      rpy="0 0 0" />
+    <parent
+      link="base" />
+    <child
+      link="base_link" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="Left_1_Link">
+    <inertial>
+      <origin
+        xyz="0.0202604071221892 0.0187577877882694 2.20960470526592E-14"
+        rpy="0 0 0" />
+      <mass
+        value="0.0309052898336316" />
+      <inertia
+        ixx="4.31215511677353E-06"
+        ixy="-4.26885891149938E-06"
+        ixz="-2.46366459463114E-13"
+        iyy="4.9707522189533E-06"
+        iyz="-2.22956217486463E-13"
+        izz="9.19223373728405E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://crt_ctag2f90d_gripper_visualization/meshes/Left_1_Link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.615686274509804 0.63921568627451 0.674509803921569 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://crt_ctag2f90d_gripper_visualization/meshes/Left_1_Link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="Left_1_Joint"
+    type="revolute">
+    <origin
+      xyz="0.030141 0 0.10415"
+      rpy="1.5708 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="Left_1_Link" />
+    <axis
+      xyz="0 0 -1" />
+    <limit
+      lower="-0.91"
+      upper="0"
+      effort="10"
+      velocity="10" />
+  </joint>
+  <link
+    name="Left_Support_Link">
+    <inertial>
+      <origin
+        xyz="-0.0177925864232372 0.0354308985297288 -3.45319167880073E-05"
+        rpy="0 0 0" />
+      <mass
+        value="0.0828693338705683" />
+      <inertia
+        ixx="2.73957553511085E-05"
+        ixy="7.04281105447926E-06"
+        ixz="-1.08446711519888E-11"
+        iyy="6.34802949004157E-06"
+        iyz="1.18385736891846E-11"
+        izz="2.83129237821216E-05" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://crt_ctag2f90d_gripper_visualization/meshes/Left_Support_Link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.615686274509804 0.63921568627451 0.674509803921569 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://crt_ctag2f90d_gripper_visualization/meshes/Left_Support_Link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="Left_Support_Joint"
+    type="revolute">
+    <origin
+      xyz="0.040359 0.037365 0"
+      rpy="0 0 0" />
+    <parent
+      link="Left_1_Link" />
+    <child
+      link="Left_Support_Link" />
+    <axis
+      xyz="0 0 -1" />
+    <limit
+      lower="0"
+      upper="0"
+      effort="0"
+      velocity="0" />
+    <mimic
+      joint="Left_1_Joint"
+      multiplier="-1"
+      offset="0" />
+  </joint>
+  <link
+    name="Left_2_Link">
+    <inertial>
+      <origin
+        xyz="0.0202604071221829 0.0187577877878552 2.21631403129407E-14"
+        rpy="0 0 0" />
+      <mass
+        value="0.0309052898336314" />
+      <inertia
+        ixx="4.31215511674771E-06"
+        ixy="-4.26885891149736E-06"
+        ixz="-2.46366467847417E-13"
+        iyy="4.97075221897906E-06"
+        iyz="-2.22956225772745E-13"
+        izz="9.19223373728399E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://crt_ctag2f90d_gripper_visualization/meshes/Left_2_Link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.615686274509804 0.63921568627451 0.674509803921569 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://crt_ctag2f90d_gripper_visualization/meshes/Left_2_Link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="Left_2_Joint"
+    type="revolute">
+    <origin
+      xyz="0.016 0 0.121"
+      rpy="1.5708 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="Left_2_Link" />
+    <axis
+      xyz="0 0 -1" />
+    <limit
+      lower="0"
+      upper="0"
+      effort="0"
+      velocity="0" />
+    <mimic
+      joint="Left_1_Joint"
+      multiplier="1"
+      offset="0" />
+  </joint>
+  <link
+    name="Right_2_Link">
+    <inertial>
+      <origin
+        xyz="0.0202604071211299 0.0187577877883374 2.11370080968728E-14"
+        rpy="0 0 0" />
+      <mass
+        value="0.0309052898336316" />
+      <inertia
+        ixx="4.31215511674775E-06"
+        ixy="-4.2688589114974E-06"
+        ixz="-2.46366382911271E-13"
+        iyy="4.9707522189791E-06"
+        iyz="-2.22956145920037E-13"
+        izz="9.19223373728407E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://crt_ctag2f90d_gripper_visualization/meshes/Right_2_Link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.615686274509804 0.63921568627451 0.674509803921569 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://crt_ctag2f90d_gripper_visualization/meshes/Right_2_Link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="Right_2_Joint"
+    type="revolute">
+    <origin
+      xyz="-0.016 0 0.121"
+      rpy="1.5708 0 3.1416" />
+    <parent
+      link="base_link" />
+    <child
+      link="Right_2_Link" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="0"
+      upper="0"
+      effort="0"
+      velocity="0" />
+    <mimic
+      joint="Left_1_Joint"
+      multiplier="-1"
+      offset="0" />
+  </joint>
+  <link
+    name="Right_1_Link">
+    <inertial>
+      <origin
+        xyz="0.0202604071211989 0.0187577877891044 2.11409547592425E-14"
+        rpy="0 0 0" />
+      <mass
+        value="0.0309052898336314" />
+      <inertia
+        ixx="4.3121551167735E-06"
+        ixy="-4.26885891149935E-06"
+        ixz="-2.46366386343243E-13"
+        iyy="4.97075221895327E-06"
+        iyz="-2.22956149418598E-13"
+        izz="9.19223373728398E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://crt_ctag2f90d_gripper_visualization/meshes/Right_1_Link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.615686274509804 0.63921568627451 0.674509803921569 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://crt_ctag2f90d_gripper_visualization/meshes/Right_1_Link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="Right_1_Joint"
+    type="revolute">
+    <origin
+      xyz="-0.030141 0 0.10415"
+      rpy="1.5708 0 3.1416" />
+    <parent
+      link="base_link" />
+    <child
+      link="Right_1_Link" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="0"
+      upper="0"
+      effort="0"
+      velocity="0" />
+    <mimic
+      joint="Left_1_Joint"
+      multiplier="-1"
+      offset="0" />
+  </joint>
+  <link
+    name="Right_Support_Link">
+    <inertial>
+      <origin
+        xyz="-0.0177925878352495 0.0354308970643828 -2.81887513877937E-08"
+        rpy="0 0 0" />
+      <mass
+        value="0.082869359674167" />
+      <inertia
+        ixx="2.73957568837024E-05"
+        ixy="7.04281063834988E-06"
+        ixz="-9.62685216616108E-12"
+        iyy="6.34803242941661E-06"
+        iyz="1.28138996747735E-11"
+        izz="2.83129239152281E-05" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://crt_ctag2f90d_gripper_visualization/meshes/Right_Support_Link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.615686274509804 0.63921568627451 0.674509803921569 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://crt_ctag2f90d_gripper_visualization/meshes/Right_Support_Link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="Right_Support_Joint"
+    type="revolute">
+    <origin
+      xyz="0.040359 0.037365 0"
+      rpy="0 0 0" />
+    <parent
+      link="Right_1_Link" />
+    <child
+      link="Right_Support_Link" />
+    <axis
+      xyz="0 0 -1" />
+    <limit
+      lower="0"
+      upper="0"
+      effort="0"
+      velocity="0" />
+    <mimic
+      joint="Left_1_Joint"
+      multiplier="-1"
+      offset="0" />
+  </joint>
+</robot>
@@ -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,
@@ -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):
@@ -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()
				`@@ -0,0 +1 @@`
				`controller_joint_names: ['', 'Left_1_Joint', 'Left_Support_Joint', 'Left_2_Joint', 'Right_2_Joint', 'Right_1_Joint', 'Right_Support_Joint', ]`