# 算法服务

**Repository Path**: sutaodian/algorithm-services

## Basic Information

- **Project Name**: 算法服务
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2026-07-17
- **Last Updated**: 2026-07-17

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 折叠屏按压工作空间

本工作空间包含五轴机械臂、二指灵巧手、指尖力传感器、恒力控制、统一 TCP 服务端、WebSocket 状态推送和调试 UI。

## 主要目录

- `src/foldable_screen_press`：一体化 UI、联合动作和项目配置。
- `src/foldable_screen_press_bringup`：启动文件、控制器配置和硬件配置。
- `src/zklx_arm_10kg_b01_5axis_api`：统一服务端、机械臂控制接口和客户端。
- `src/zklx_arm_10kg_b01_hardware`：五轴机械臂 `ros2_control` 硬件层。
- `src/two_finger_hand`：二指手电机、指尖传感器和恒力控制。
- `src/zklx_arm_10kg_b01_5axis_description`、`src/bi_finger_description`：机械臂与二指手模型。

## 接口

- TCP 控制端口：`15005`
- WebSocket 状态端口：`15006`
- 动作命名空间：`system.*`、`arm.*`、`hand.*`
- `arm.raster_scan`：将栅格折线拆成多条直线，并逐段复用 `arm.move_linear` 的执行参数和返回规则。
- 恒力 UI 有两个独立开关：`合力` 对应接口参数 `force_source=magnitude`，未打开时为 `force_source=fz`；`双关节` 对应 `dual_joint_tracking=true`，未打开时只用当前手指 B 关节跟踪。
- 食指/无名指恒力启动按钮保持互相独立；双关节只作为当前启动手指的参数，不会自动启动另一根手指。
- 全速度模式增加硬件写入恢复：UDP 反馈按关节独立判定新鲜度，瞬态反馈/发送故障安全下发零速但不再杀死 `ros2_control` 写循环；无实际运动会直接返回 `velocity_command_not_effective`。

完整的请求参数、默认值、单位、返回字段、错误码和示例见：

- [`UNIFIED_ARM_HAND_API.md`](UNIFIED_ARM_HAND_API.md)

## 构建

```bash
cd foldable_screen_press_ws
./scripts/build.sh
```

## 启动

真实 UDP 硬件：

```bash
./scripts/run_hardware.sh udp.yaml
```

仿真：

```bash
./scripts/run_fake.sh
```

服务器和 UI 一起启动：

```bash
./scripts/run_all.sh udp.yaml
```

仿真模式：

```bash
./scripts/run_all.sh --fake
```

也可以直接使用 ROS 入口：

```bash
source install/setup.bash
ros2 launch foldable_screen_press_bringup foldable_screen_press.launch.py \
  use_fake_hardware:=false \
  hardware_profile:=udp.yaml

ros2 run foldable_screen_press foldable_screen_press_ui
```


## v3.11 UI 精简与段错误修复

- 一体化 UI 不再直接加载 `ControlCAN`、串口传感器或二指手硬件对象；机械臂与手部控制全部通过统一 TCP `15005`，状态全部通过 WebSocket `15006`。
- 删除关节曲线窗口及 Matplotlib、原始 JSON 请求页、旧关节轨迹编辑页、全局旧速度模式选择器，以及服务端已经忽略的 `time_step`、`z_tolerance`、`orientation_weight`、`skip_failed_ik` 控件。
- 五轴 UI 的后台请求不再为每个请求创建 `QThread`，改为普通守护线程执行网络请求，并通过线程安全队列在 Qt 主线程更新控件，避免跨线程访问 Qt 和关闭时销毁仍在运行的线程。
- 二指手页面改为统一服务端远程面板，显示四关节位置、电流、使能/找零状态及指尖力，不会与服务端重复打开 CAN/串口硬件。
- 零点校准 UI 固定发送 `enable=false`；校准成功后仍需手动使能，和硬件层零点保护保持一致。

启动 UI：

```bash
source install/setup.bash
ros2 run foldable_screen_press foldable_screen_press_ui \
  --arm-host 127.0.0.1 \
  --arm-port 15005 \
  --websocket-port 15006
```

## 统一客户端示例

```python
from zklx_arm_10kg_b01_5axis_api import UnifiedRobotClient

robot = UnifiedRobotClient(
    host="127.0.0.1",
    port=15005,
    password="123456",
)

print(robot.system.get_status())
print(robot.arm.get_joint_state())
print(robot.hand.get_joint_state())
```

## 接口测试

默认只读测试：

```bash
source install/setup.bash
ros2 run zklx_arm_10kg_b01_5axis_api test_unified_robot_interface
```

实际运动测试必须显式添加 `--enable-motion`，并先确认急停、零位、限位和周围空间。

WebSocket 返回内容测试：

```bash
source install/setup.bash
ros2 run zklx_arm_10kg_b01_5axis_api test_websocket_state \
  --host 127.0.0.1 \
  --port 15006 \
  --password 123456 \
  --count 20 \
  --rate 10 \
  --first-full
```

查看每一帧完整 JSON 时增加 `--raw`；保存全部返回消息时增加
`--save-jsonl websocket_state.jsonl`。该脚本只读取状态，不会发送运动命令。

## 硬件注意事项

- 二指手 CAN、串口和恒力线程由统一服务端独占，不要由其他进程同时打开同一硬件。
- 硬件配置位于 `src/foldable_screen_press_bringup/config/hardware_profiles/`。
- 二指手配置位于 `src/two_finger_hand/config/constant_force_press.json`。
- UI 配置位于 `src/foldable_screen_press/config/foldable_screen_press.yaml`。


## 机械臂零点设置硬件保护

从 `arm.set_motor_zero` 开始，到五个关节全部完成“写零点、保存参数、查询偏移验证”之前，硬件层会锁存零点保护。

保护期间：

- `write()` 不向电机发送位置或速度命令；
- 拒绝电机使能、电流控制、速度位置闭环和拖动模式；
- 允许状态查询、反馈读取、失能、急停和再次执行零点设置；
- 零点设置失败时保护不会自动解除，必须重新执行并成功完成；
- 零点设置成功后电机仍保持失能，需显式调用 `arm.enable_motors`；
- 运行状态为 `zero_setting` 或 `zero_incomplete_blocked`，会同时出现在 `system.get_status` 和 WebSocket `system.hardware_runtime_status` 中。

硬件参数：

| 参数 | 类型 | 默认值 | 说明 |
|---|---:|---:|---|
| `zero_guard_require_on_startup` | bool | `false` | 为 `true` 时，每次硬件启动后都必须重新设置零点才允许运动 |
| `zero_guard_verify_after_set` | bool | `true` | 设置后查询各电机偏移并验证 |
| `zero_guard_verify_tolerance_counts` | int | `10` | 偏移验证容差，编码器计数 |
| `zero_guard_settle_ms` | int | `100` | 失能、设置完成后的等待时间，毫秒 |

## v3.10 混合轨迹执行与实际轨迹记录

- `arm.circle_scan` 使用一次连续的 `arm.track_cartesian_trajectory`，圆周中间不停车。
- `arm.raster_scan` 和 `arm.matrix_scan` 在几何拐点处分段，每一段调用现有 `arm.move_linear`，到达并稳定后再换向。
- `arm.track_cartesian_trajectory` 保留为通用的平滑曲线/连续路径接口，不建议直接输入带尖锐拐点且未做圆角过渡的路径。
- 扫描和通用跟踪均恢复实际轨迹记录：根据 `/joint_states` 记录关节位置、速度、力矩及末端 FK，默认保存到 `~/zklx_trajectory_records/`。
- 响应返回 `actual_trajectory`、`trajectory` 和 `trajectory_record_file`；完整高频数据以服务端文件为准。