Metadata-Version: 2.2
Name: fa-robot-skills
Version: 0.2.9
Summary: ROS 2 robot skills library for arm, dexhand, and gripper control
Author-email: wu-yidong <1486439216@qq.com>
License: MIT
Keywords: robotics,ros2,robot,skills,manipulation
Classifier: Development Status :: 3 - Alpha
Classifier: Intended Audience :: Developers
Classifier: Intended Audience :: Science/Research
Classifier: Topic :: Scientific/Engineering
Classifier: License :: OSI Approved :: MIT License
Classifier: Programming Language :: Python :: 3
Classifier: Programming Language :: Python :: 3.12
Requires-Python: <3.13,>=3.12
Description-Content-Type: text/markdown
License-File: LICENSE
Requires-Dist: numpy
Requires-Dist: open3d
Requires-Dist: pyyaml
Requires-Dist: scipy
Requires-Dist: typeguard
Provides-Extra: dev
Requires-Dist: build; extra == "dev"
Requires-Dist: twine; extra == "dev"
Requires-Dist: pre-commit; extra == "dev"

# fa_robot_skills 使用说明

本文档面向 `fa_robot_skills` 项目的调用方，介绍目录结构、快速使用方式，以及每个 skill 函数的用途和调用时机。
若需 ROS2 topic/node 调用方式与 JSON 指令示例，请参见 [`README_ROS2.md`](README_ROS2.md)。

## 目录

- **[0. 安装](#0-安装)** — 环境要求、PyPI 安装、源码直接使用（不 pip install）、版本查看、本地发布与导入方式
- **[1. 文件结构与功能说明](#1-文件结构与功能说明)** — 各目录/文件职责，以及 `__init__.py` 包根导出范围
- **[2. 快速使用](#2-快速使用)** — 示例命令与代码片段：basic、MoveJ、MoveL、MoveL Path、MoveJ Path、灵巧手、夹爪
- **[3. skill 函数简略清单](#3-skill-函数简略清单)** — 按类别汇总全部 skill 函数索引
- **[4. skill 函数详细说明](#4-skill-函数详细说明)** — 各函数参数、返回值与典型场景
  - 4.1 基础函数 — 初始化、连接、状态切换、关节/位姿读取
  - 4.2 笛卡尔运动 — MoveL、路径运动
  - 4.3 关节运动 — MoveJ、关节路径、头/身/腰
  - 4.4 灵巧手
  - 4.5 夹爪

## 0. 安装

**环境要求：Python 3.12**

### 从 PyPI 安装

```bash
pip install fa-robot-skills
```
参考 https://pypi.org/project/fa-robot-skills

**ROS2 依赖说明：** `rclpy`、`sensor-msgs` 等 ROS2 Python 包不在 PyPI 上发布。安装本库前请先 `source` 你的 ROS2 工作区，并确保运行环境中已具备相应 ROS2 依赖。

```python
from fa_robot_skills import get_default_config_path, init_robot

robot = init_robot(config_path=get_default_config_path())
```

### 不 pip install（源码直接使用）

**可以不执行 `pip install`，直接在本仓库源码目录下使用。** `examples/`、`examples_ros2/` 均采用此方式，与 pip 安装后的 API 能力一致，仅导入路径与配置路径写法不同。

| 对比项 | pip install / `pip install -e .` | 源码直接使用（不安装） |
| --- | --- | --- |
| 导入 | `from fa_robot_skills import init_robot` | `from src.robot_skills import init_robot` |
| 默认配置 | `get_default_config_path()` | 显式路径，如 `configs/enhanced_robot_config.yaml` |
| 依赖 | 随 `pip install` 一并安装 | 需自行安装 `pyproject.toml` 中的依赖（`numpy`、`open3d`、`rclpy` 等） |

在含 `pyproject.toml` 的包根目录下，可直接运行示例：

```bash
python examples/test_robot_basic.py
```

或在自有脚本中将包根目录加入 `sys.path`（与 examples 相同）：

```python
import os
import sys

# 含 src/、configs/、pyproject.toml 的目录
PROJECT_ROOT = os.path.abspath("/path/to/fa_robot_skills")
if PROJECT_ROOT not in sys.path:
    sys.path.insert(0, PROJECT_ROOT)

from src.robot_skills import init_robot, connect_robot, disconnect_robot

config_path = os.path.join(PROJECT_ROOT, "configs", "enhanced_robot_config.yaml")
robot = init_robot(config_path=config_path)
```

说明：

- 源码模式下请使用 `from src.xxx import ...`，**不要**写 `from fa_robot_skills import ...`（除非已将**上一级**目录加入 `PYTHONPATH` 且子目录名为 `fa_robot_skills`）。
- `get_default_config_path()` 依赖已安装的包资源；源码模式下请像 examples 一样传入 `configs/enhanced_robot_config.yaml` 的绝对或相对路径。
- 若希望本地改代码的同时使用 `from fa_robot_skills import ...`，推荐 `pip install -e .`（可编辑安装，见下节）。

### 查看版本

维护者在 `pyproject.toml` 的 `[project].version` 字段维护版本号（当前单一来源）。

安装者可通过以下方式查看：

```bash
# 命令行
pip show fa-robot-skills
pip list | grep fa-robot-skills
```

```python
# Python 代码
import fa_robot_skills
print(fa_robot_skills.__version__)

from importlib.metadata import version
print(version("fa-robot-skills"))
```

### 本地开发与发布

在本目录（含 `pyproject.toml` 的 `fa_robot_skills/`）下执行。构建产物为**单个** wheel（`fa-robot-skills`），其中内嵌 `ros2_robot_interface` 源码模块，不会生成独立的 `ros2-robot-interface` wheel。

```bash
# 可编辑安装
pip install -e .

# 构建 wheel / sdist
python -m build

# 上传到 PyPI（需先配置 ~/.pypirc 或环境变量）
python -m twine upload dist/*
```

安装后同时可用：

```python
import fa_robot_skills
from fa_robot_skills.ros2_robot_interface import ROS2RobotInterface
```

### 导入方式

根据使用场景选择导入路径：

| 场景 | 导入示例 |
| --- | --- |
| **PyPI / `pip install -e .` 后** | `from fa_robot_skills import init_robot, moveL_robot` |
| **源码直接使用（不 pip install）** | `from src.robot_skills import init_robot`（见 [不 pip install](#不-pip-install源码直接使用)） |
| **仓库根目录运行 `examples/`** | 同上；示例脚本会自动将包根目录加入 `sys.path` |
| **未从包根导出的函数**（见 [对照表](#包根导出范围)） | 优先改用包根函数；确需低层接口时：`from fa_robot_skills.src.robot_skills import ...` |
| **子模块详细文档** | 灵巧手 / 夹爪等亦可 `from fa_robot_skills import send_dual_hand_joint_positions_blocking`（见 [包根导出范围](#包根导出范围)） |

`fa_robot_skills` 包根（`__init__.py`）导出的函数见 [1. 文件结构](#1-文件结构与功能说明) 中的说明；未导出的函数见 [包根导出范围](#包根导出范围) 对照表，其中列出了**推荐改用的包根替代函数**。

## 1. 文件结构与功能说明

| 文件/目录 | 功能说明 |
| --- | --- |
| `__init__.py` | 包根统一导出入口：从 `src/robot_skills.py`、`src/dexhand_skills.py`、`src/gripper_skills.py` 再导出对外 skill（见下方「包根导出范围」） |
| `src/` | 基础源码目录，包含核心实现与工具模块 |
| `src/robot_skills.py` | 核心 skill 实现，包含连接、状态切换、MoveJ/MoveL/路径、单双臂目标下发、腰部升降等 |
| `src/dexhand_skills.py` | 灵巧手 skill 实现，包含左/右/双手关节目标下发与阻塞等待 |
| `src/gripper_skills.py` | 夹爪 skill 实现，包含单/双夹爪位置下发与阻塞等待到位 |
| `src/robot_skills_ros2_topic_node.py` | ROS2 topic node 实现，对外提供 `/robot_skills/command` 指令入口 |
| `src/pose.py` | 位姿表示与转换相关封装，供位姿处理逻辑复用 |
| `src/pose_visualization.py` | 位姿/点云可视化相关能力，供调试与可视化场景使用 |
| `src/logger.py` | 日志封装，供各模块复用 |
| `support_type.py` | 类型定义与别名，统一函数签名与类型标注 |
| `configs/enhanced_robot_config.yaml` | 默认机器人 YAML 配置 |
| `examples/test_robot_basic.py` | 基础示例：初始化、连接、读取关节/位姿、断连 |
| `examples/test_robot_movej.py` | MoveJ 示例：关节空间阻塞运动流程 |
| `examples/test_robot_pose.py` | 位姿示例：单臂/双臂 MoveL（交互式 `y/n` 确认） |
| `examples/test_robot_path_pose.py` | 路径位姿示例：双臂/单臂 MoveL Path（`pose_list` 与 `delta_list` 两种风格） |
| `examples/test_robot_path_joint.py` | 关节路径示例：MoveJ Path（waypoints 轨迹） |
| `examples/test_dexhand_skills.py` | 灵巧手示例：左手、右手、双手与双手接口单手模式 |
| `examples/test_gripper_skills.py` | 夹爪示例：单夹爪或双夹爪同时运动（参数字典风格） |
| `examples_ros2/` | ROS2 topic 调用示例与 node 启动脚本，详见 [`README_ROS2.md`](README_ROS2.md) |
| `README.md` | 使用文档（当前文件） |
| `README_ROS2.md` | ROS2 topic/node 调用说明与 JSON 指令示例 |

### 包根导出范围（`from fa_robot_skills import ...`）

**已导出**（可直接从包根导入）：

| 函数 | 功能 |
| --- | --- |
| `init_robot` | 初始化机器人接口实例 |
| `connect_robot` | 连接机器人并切换到 HOLD 可用状态 |
| `disconnect_robot` | 断开机器人连接 |
| `robot_go_home` | 执行固定回 Home 流程（HOLD -> HOME -> HOLD -> OCS2） |
| `switch_to_movej_state` | 切换到 MOVEJ（关节空间）状态 |
| `switch_to_ocs2_state` | 切换到 OCS2（笛卡尔空间）状态 |
| `switch_to_hold_state` | 切换到 HOLD 安全状态 |
| `get_joint` | 获取当前关节状态 |
| `left_get_pose` | 获取左臂末端位姿 |
| `right_get_pose` | 获取右臂末端位姿 |
| `moveL_robot` | 单臂/双臂笛卡尔位姿运动入口 |
| `moveL_path_robot_by_pose_list` | 路径运动（pose_list + 单一 delta） |
| `moveL_path_robot_by_delta_list` | 路径运动（单一 pose + delta_list） |
| `adjust_body_height` | 身体关节升降控制（按比例计算关节角） |
| `move_waist_lifting_relative_blocking` | 腰部相对升降（米）并阻塞等待到位 |
| `moveJ_robot` | 全身关节阻塞运动入口 |
| `moveJ_path_robot` | 关节空间路径（waypoints）阻塞运动入口 |
| `send_dual_hand_joint_positions` | 双手灵巧手目标下发（非阻塞，支持单手） |
| `send_dual_hand_joint_positions_blocking` | 双手灵巧手目标下发并可阻塞等待（支持单手） |
| `gripper_control_blocking` | 单/双夹爪位置下发并可阻塞等待到位（多侧同时运动） |

另含 `__version__`、`get_default_config_path()`。

**未从包根导出**（需从子模块导入；**推荐改用下列包根函数**）：

| 未导出函数 | 定义模块 | 推荐改用（包根） | 说明 |
| --- | --- | --- | --- |
| `left_move_pose_stamped_blocking` | `src/robot_skills.py` | [`moveL_robot`](#fn-movel) | 在 `pose_params` 中仅填 `left_pose_6d`（及可选 `left_pose_delta`） |
| `right_move_pose_stamped_blocking` | `src/robot_skills.py` | [`moveL_robot`](#fn-movel) | 在 `pose_params` 中仅填 `right_pose_6d`（及可选 `right_pose_delta`） |
| `send_dual_arm_stamped_blocking` | `src/robot_skills.py` | [`moveL_robot`](#fn-movel) | 双臂同步时在 `pose_params` 中同时填 `left_pose_6d` 与 `right_pose_6d` |
| `left_joint_positions_blocking` | `src/robot_skills.py` | [`moveJ_robot`](#fn-movej) | `joint_params={"left_joints": [...]}` |
| `right_joint_positions_blocking` | `src/robot_skills.py` | [`moveJ_robot`](#fn-movej) | `joint_params={"right_joints": [...]}` |
| `dual_arm_joint_positions_blocking` | `src/robot_skills.py` | [`moveJ_robot`](#fn-movej) | `joint_params={"left_joints": [...], "right_joints": [...]}` |
| `send_head_joint_positions_blocking` | `src/robot_skills.py` | [`moveJ_robot`](#fn-movej) | `joint_params={"head_joints": [...]}` |
| `send_body_joint_positions_blocking` | `src/robot_skills.py` | [`moveJ_robot`](#fn-movej) 或 [`adjust_body_height`](#fn-adjust-body-height) | 精确关节角用前者；按比例升降用后者 |
| `send_left_hand_joint_positions` | `src/dexhand_skills.py` | [`send_dual_hand_joint_positions`](#fn-dual-hand-send) | `left_positions=..., right_positions=None` |
| `send_right_hand_joint_positions` | `src/dexhand_skills.py` | [`send_dual_hand_joint_positions`](#fn-dual-hand-send) | `left_positions=None, right_positions=...` |
| `send_left_hand_joint_positions_blocking` | `src/dexhand_skills.py` | [`send_dual_hand_joint_positions_blocking`](#fn-dual-hand) | `left_positions=..., right_positions=None` |
| `send_right_hand_joint_positions_blocking` | `src/dexhand_skills.py` | [`send_dual_hand_joint_positions_blocking`](#fn-dual-hand) | `left_positions=None, right_positions=...` |

子模块导入示例：`from fa_robot_skills.src.robot_skills import left_joint_positions_blocking`（本地：`from src.robot_skills import ...`）。仅在必须调用低层接口时使用。

## 2. 快速使用

下文示例命令均在 `fa_robot_skills` 目录下执行（即包含 `examples/`、`src/` 的目录）。代码片段使用 `from src.xxx import ...`，与 `examples/` 脚本一致；若已通过 `pip install` 安装，请改用 [导入方式](#导入方式) 中的 `fa_robot_skills` 路径。

### 2.1 使用 basic（连接 + 状态读取）

#### examples 目录下的 `test_robot_basic.py` 文件

```bash
python examples/test_robot_basic.py
```

可选参数：

- `--config`：指定机器人 YAML 配置路径（默认使用 `configs/enhanced_robot_config.yaml`）。

#### 使用流程

1. `init_robot(...)`：创建机器人接口实例
2. `connect_robot(robot)`：建立连接并切换到 HOLD 可用状态
3. `get_joint(robot)`：读取当前关节状态
4. `disconnect_robot(robot)`：流程结束后安全断线

#### 代码示例

```python
from src.robot_skills import connect_robot, disconnect_robot, get_joint, init_robot

robot = init_robot(config_path="configs/enhanced_robot_config.yaml")
if robot is None:
    raise RuntimeError("Robot 初始化失败")

if not connect_robot(robot):
    raise RuntimeError("Robot 连接失败")

joint_state = get_joint(robot)
print("当前关节状态:", joint_state)

disconnect_robot(robot)
```

### 2.2 使用 MoveJ（关节空间）

#### examples 目录下的 `test_robot_movej.py` 文件

```bash
python examples/test_robot_movej.py
```

可选参数：

- `--config`：指定机器人 YAML 配置路径（默认使用 `configs/enhanced_robot_config.yaml`）。
- `test_robot_movej.py` 会在执行运动前要求输入 `y/n` 确认，避免误触发运动。

#### 使用流程

1. `init_robot(...)`：创建机器人接口实例
2. `connect_robot(robot)`：建立连接并切换到 HOLD 可用状态
3. `moveJ_robot(...)`：执行全身关节阻塞运动
4. `disconnect_robot(robot)`：流程结束后安全断线

#### 代码示例

```python
from src.robot_skills import (
    connect_robot,
    disconnect_robot,
    init_robot,
    moveJ_robot,
)

robot = init_robot(config_path="configs/enhanced_robot_config.yaml")
if robot is None:
    raise RuntimeError("Robot 初始化失败")

if not connect_robot(robot):
    raise RuntimeError("Robot 连接失败")

ok = moveJ_robot(
    robot=robot,
    joint_params={
        "left_joints": [0.0, -0.4, 0.2, -1.2, 0.0, 0.8, 0.0],
        "right_joints": [0.0, 0.4, -0.2, 1.2, 0.0, -0.8, 0.0],
    },
)
if not ok:
    raise RuntimeError("MoveJ 执行失败")

disconnect_robot(robot)
```

### 2.3 使用 MoveL（笛卡尔空间，单臂/双臂）

#### examples 目录下的 `test_robot_pose.py` 文件（MoveL 部分）

```bash
python examples/test_robot_pose.py
```

可选参数：

- `--config`：指定机器人 YAML 配置路径（默认使用 `configs/enhanced_robot_config.yaml`）。
- `test_robot_pose.py` 会依次询问是否执行单臂 MoveL、双臂 MoveL（均为 `y/n`），避免误触发运动。

#### 使用流程

1. `init_robot(...)`：创建机器人接口实例
2. `connect_robot(robot)`：建立连接并切换到 HOLD 可用状态
3. `switch_to_ocs2_state(robot)`：切换到 OCS2 状态
4. `left_get_pose` / `right_get_pose`：读取当前末端位姿（可选）
5. `moveL_robot(...)`：执行笛卡尔位姿运动（仅左或仅右 6D 为单臂；同时提供左右 6D 为双臂同步）
6. `disconnect_robot(robot)`：流程结束后安全断线

说明：

- 单臂与双臂均通过 `moveL_robot` 下发；`test_robot_pose.py` 中依次演示左臂单臂与双臂同步。
- 双臂同步时在 `pose_params` 中同时提供 `left_pose_6d` 与 `right_pose_6d`；参考系以 `left_frame_id` 为准（默认 `arm_base`）。底层经 `send_dual_arm_stamped_blocking`（**未从包根导出**，见 [对照表](#包根导出范围)）同步下发，对外请直接使用 `moveL_robot`。
- 若需**多点路径**，建议优先使用：
  - [`moveL_path_robot_by_pose_list`](#fn-movel-path-pose-list)：`pose_list + 单一 delta`
  - [`moveL_path_robot_by_delta_list`](#fn-movel-path-delta-list)：`单一 pose + delta_list`

#### 代码示例（单臂 MoveL）

```python
from src.robot_skills import (
    connect_robot,
    disconnect_robot,
    init_robot,
    moveL_robot,
    switch_to_ocs2_state,
)

robot = init_robot(config_path="configs/enhanced_robot_config.yaml")
if not connect_robot(robot) or not switch_to_ocs2_state(robot):
    raise RuntimeError("Robot 连接或状态切换失败")

result = moveL_robot(
    robot=robot,
    pose_params={
        "left_frame_id": "arm_base",
        "left_pose_6d": [0.50, 0.416, -0.472, -2.996, -1.548, -0.139],
        "left_pose_delta": {"center": [0.0, 0.0, 0.0], "rotate": [0.0, 0.0, 0.0]},
    },
    max_timeout=10.0,
)
if result is None:
    raise RuntimeError("单臂 MoveL 失败")

disconnect_robot(robot)
```

#### 代码示例（双臂同步 MoveL）

```python
from src.robot_skills import (
    connect_robot,
    disconnect_robot,
    init_robot,
    moveL_robot,
    switch_to_ocs2_state,
)

robot = init_robot(config_path="configs/enhanced_robot_config.yaml")
if not connect_robot(robot) or not switch_to_ocs2_state(robot):
    raise RuntimeError("Robot 连接或状态切换失败")

result = moveL_robot(
    robot=robot,
    pose_params={
        "left_frame_id": "arm_base",
        "right_frame_id": "arm_base",
        "left_pose_6d": [0.50, 0.416, -0.472, -2.996, -1.548, -0.139],
        "right_pose_6d": [0.50, -0.416, -0.472, -2.996, -1.548, -0.139],
        "left_pose_delta": {"center": [0.0, 0.0, 0.0], "rotate": [0.0, 0.0, 0.0]},
        "right_pose_delta": {"center": [0.0, 0.0, 0.0], "rotate": [0.0, 0.0, 0.0]},
        "left_delta_type": "update_pose_with_delta",
        "right_delta_type": "update_pose_with_self_delta",
    },
    max_timeout=10.0,
)
if result is None:
    raise RuntimeError("双臂 MoveL 失败")

disconnect_robot(robot)
```

### 2.4 使用 MoveL Path（笛卡尔路径，单臂/双臂）

#### examples 目录下的 `test_robot_path_pose.py` 文件

```bash
python examples/test_robot_path_pose.py
```

可选参数：

- `--config`：指定机器人 YAML 配置路径（默认使用 `configs/enhanced_robot_config.yaml`）。
- 脚本会依次询问是否执行双臂 MoveL Path、单臂 MoveL Path、单臂 MoveL Path（`delta_list` 风格），均为 `y/n` 确认。

#### 使用流程

1. `init_robot(...)`：创建机器人接口实例
2. `connect_robot(robot)`：建立连接并切换到 HOLD 可用状态
3. `switch_to_ocs2_state(robot)`：切换到 OCS2 状态
4. `moveL_path_robot_by_pose_list(...)` 或 `moveL_path_robot_by_delta_list(...)`：执行路径运动
5. `disconnect_robot(robot)`：流程结束后安全断线

#### 代码示例（pose_list 风格）

```python
from src.robot_skills import (
    connect_robot,
    disconnect_robot,
    init_robot,
    moveL_path_robot_by_pose_list,
    switch_to_ocs2_state,
)

robot = init_robot(config_path="configs/enhanced_robot_config.yaml")
if not connect_robot(robot) or not switch_to_ocs2_state(robot):
    raise RuntimeError("Robot 连接或状态切换失败")

result = moveL_path_robot_by_pose_list(
    robot=robot,
    pose_params={
        "left_pose_6d_list": [
            [0.50, 0.416, -0.472, -2.996, -1.548, -0.139],
            [0.52, 0.406, -0.462, -2.996, -1.548, -0.139],
        ],
        "right_pose_6d_list": [
            [0.50, -0.416, -0.472, -2.996, -1.548, -0.139],
            [0.52, -0.406, -0.462, -2.996, -1.548, -0.139],
        ],
        "left_pose_delta": {"center": [0.0, 0.0, 0.0], "rotate": [0.0, 0.0, 0.0]},
        "right_pose_delta": {"center": [0.0, 0.0, 0.0], "rotate": [0.0, 0.0, 0.0]},
    },
    frame_id="arm_base",
    trajectory_duration=6.0,
)
if result is None:
    raise RuntimeError("MoveL Path 失败")

disconnect_robot(robot)
```

### 2.5 使用 MoveJ Path（关节空间路径）

#### examples 目录下的 `test_robot_path_joint.py` 文件

```bash
python examples/test_robot_path_joint.py
```

可选参数：

- `--config`：指定机器人 YAML 配置路径（默认使用 `configs/enhanced_robot_config.yaml`）。
- 脚本会要求输入 `y/n` 确认是否执行 MoveJ Path，避免误触发运动。

#### 使用流程

1. `init_robot(...)`：创建机器人接口实例
2. `connect_robot(robot)`：建立连接并切换到 HOLD 可用状态
3. `moveJ_path_robot(...)`：按 `joint_names` + `waypoints` 下发关节轨迹并阻塞等待到位
4. `disconnect_robot(robot)`：流程结束后安全断线

#### 关键说明

- `moveJ_path_robot` 已从包根 `fa_robot_skills` 导出，亦可 `from src.robot_skills import moveJ_path_robot`。

#### 代码示例

```python
from src.robot_skills import connect_robot, disconnect_robot, init_robot, moveJ_path_robot

robot = init_robot(config_path="configs/enhanced_robot_config.yaml")
if not connect_robot(robot):
    raise RuntimeError("Robot 连接失败")

ok = moveJ_path_robot(
    robot=robot,
    joint_path_params={
        "joint_names": [
            "left_joint1", "left_joint2", "left_joint3",
            "left_joint4", "left_joint5", "left_joint6", "left_joint7",
        ],
        "waypoints": [
            [-0.50, 0.81, 0.89, -2.31, -0.95, -0.19, 0.11],
            [0.38, 0.97, 0.85, -2.36, -0.89, -1.00, 0.0],
        ],
        "trajectory_duration": 4.0,
    },
    max_timeout=10.0,
)
if not ok:
    raise RuntimeError("MoveJ Path 执行失败")

disconnect_robot(robot)
```

关键说明：

- `joint_names` 须与机器人 `joint_states` 中的名称一致。
- `waypoints` 至少包含 2 个路径点，每个路径点维度须与 `joint_names` 长度一致。

### 2.6 使用灵巧手（双手/单手）

#### examples 目录下的 `test_dexhand_skills.py` 文件

```bash
python examples/test_dexhand_skills.py
```

可选参数：

- `--config`：指定机器人 YAML 配置路径（默认使用 `configs/enhanced_robot_config.yaml`）。
- `--max-timeout`：灵巧手阻塞等待超时（秒），默认 `15.0`。
- `test_dexhand_skills.py` 会在执行运动前要求输入 `y/n` 确认，避免误触发运动。

#### 使用流程

1. `init_robot(...)`：创建机器人接口实例
2. `connect_robot(robot)`：建立连接并切换到 HOLD 可用状态
3. `send_dual_hand_joint_positions_blocking`：下发目标并可阻塞等待到位（单手时另一侧传 `None`）
4. `disconnect_robot(robot)`：流程结束后安全断线

#### 关键说明

- 灵巧手推荐从包根导入：`from fa_robot_skills import send_dual_hand_joint_positions_blocking`；本地开发亦可 `from src.dexhand_skills import ...`（见 [包根导出范围](#包根导出范围)）。
- 灵巧手目标值使用归一化范围 `0.0-1.0`（`0.0=最小位置`，`1.0=最大位置`）。
- 若只需控制单手，可将另一侧参数传 `None`（例如 `right_positions=None`）。
- 阻塞接口发送前会自动尝试切换到 OCS2 状态。

#### 代码示例

```python
from src.dexhand_skills import send_dual_hand_joint_positions_blocking
from src.robot_skills import connect_robot, disconnect_robot, init_robot

robot = init_robot(config_path="configs/enhanced_robot_config.yaml")
if not connect_robot(robot):
    raise RuntimeError("Robot 连接失败")

ok = send_dual_hand_joint_positions_blocking(
    robot=robot,
    left_positions=[0.98, 0.0, 0.78, 0.78, 0.78, 0.78, 0.98],
    right_positions=None,
    max_timeout=10.0,
)
if not ok:
    raise RuntimeError("灵巧手运动失败")

disconnect_robot(robot)
```

### 2.7 使用夹爪（单夹爪/双夹爪）

#### examples 目录下的 `test_gripper_skills.py` 文件

```bash
python examples/test_gripper_skills.py
```

可选参数：

- `--config`：指定机器人 YAML 配置路径（默认使用 `configs/enhanced_robot_config.yaml`）。
- 脚本会依次询问是否执行两段夹爪运动（均为 `y/n` 确认）。

#### 使用流程

1. `init_robot(...)`：创建机器人接口实例
2. `connect_robot(robot)`：建立连接并切换到 HOLD 可用状态
3. `gripper_control_blocking(...)`：批量下发目标并统一等待到位
4. `disconnect_robot(robot)`：流程结束后安全断线

#### 关键说明

- 夹爪推荐从包根导入：`from fa_robot_skills import gripper_control_blocking`；本地开发亦可 `from src.gripper_skills import ...`（见 [包根导出范围](#包根导出范围)）。
- 目标位置与 `GripperHandler.send_joint_positions` 一致，通常为行程值（如 `gripper_min_position` / `gripper_max_position`）。
- `gripper_types` 控制参与运动的侧别：单夹爪 `["left"]`，双夹爪 `["left", "right"]`。
- 多侧运动时先**同时下发**，再**统一轮询**等待全部到位。
- 到位判断使用 `GripperHandler.check_arrival`：打开时按距离阈值；关闭时距离达标或位置历史稳定即视为到位。

#### 代码示例

```python
from src.gripper_skills import gripper_control_blocking
from src.robot_skills import connect_robot, disconnect_robot, init_robot

DEFAULT_GRIPPER_CLOSE_PARAMS = {
    "gripper_types": ["left", "right"],
    "left_gripper_target_position": 0.0,
    "right_gripper_target_position": 0.0,
    "wait_until_complete": True,
    "max_timeout": 5.0,
}

robot = init_robot(config_path="configs/enhanced_robot_config.yaml")
if not connect_robot(robot):
    raise RuntimeError("Robot 连接失败")

ok = gripper_control_blocking(robot=robot, **DEFAULT_GRIPPER_CLOSE_PARAMS)
if not ok:
    raise RuntimeError("夹爪闭合失败")

disconnect_robot(robot)
```


## 3. skill 函数简略清单

> **标注说明**：表中标注 **〔未导出〕** 的函数未从包根 `fa_robot_skills` 导出；**→** 后给出推荐改用的包根函数（详见 [包根导出范围](#包根导出范围) 对照表）。

| 基础函数 | 功能 |
| --- | --- |
| [`init_robot`](#fn-init-robot) | 初始化机器人接口实例 |
| [`connect_robot`](#fn-connect-robot) | 连接机器人并切换到 HOLD 可用状态 |
| [`disconnect_robot`](#fn-disconnect-robot) | 断开机器人连接 |
| [`robot_go_home`](#fn-robot-go-home) | 执行固定回 Home 流程（HOLD -> HOME -> HOLD -> OCS2） |
| [`switch_to_movej_state`](#fn-switch-movej) | 切换到 MOVEJ（关节空间）状态 |
| [`switch_to_ocs2_state`](#fn-switch-ocs2) | 切换到 OCS2（笛卡尔空间）状态 |
| [`switch_to_hold_state`](#fn-switch-hold) | 切换到 HOLD 安全状态 |
| [`get_joint`](#fn-get-joint) | 获取当前关节状态 |
| [`left_get_pose`](#fn-get-pose) | 获取左臂末端位姿 |
| [`right_get_pose`](#fn-get-pose) | 获取右臂末端位姿 |


| 运动函数（笛卡尔空间运动） | 功能 |
| --- | --- |
| [`left_move_pose_stamped_blocking`](#fn-single-arm-pose) | 左臂位姿下发并可阻塞等待 **〔未导出〕→ [`moveL_robot`](#fn-movel)** |
| [`right_move_pose_stamped_blocking`](#fn-single-arm-pose) | 右臂位姿下发并可阻塞等待 **〔未导出〕→ [`moveL_robot`](#fn-movel)** |
| [`send_dual_arm_stamped_blocking`](#fn-dual-arm-path) | 双臂 pose/path 下发并等待到位 **〔未导出〕→ [`moveL_robot`](#fn-movel) / 路径函数** |
| [`moveL_robot`](#fn-movel) | 单臂/双臂笛卡尔位姿运动入口 |
| [`moveL_path_robot_by_pose_list`](#fn-movel-path-pose-list) | 路径运动（pose_list + 单一 delta） |
| [`moveL_path_robot_by_delta_list`](#fn-movel-path-delta-list) | 路径运动（单一 pose + delta_list） |


| 运动函数（关节空间运动） | 功能 |
| --- | --- |
| [`send_head_joint_positions_blocking`](#fn-head-joint) | 头部关节下发并可阻塞等待 **〔未导出〕→ [`moveJ_robot`](#fn-movej)** |
| [`send_body_joint_positions_blocking`](#fn-body-joint) | 身体关节下发并可阻塞等待 **〔未导出〕→ [`moveJ_robot`](#fn-movej) / [`adjust_body_height`](#fn-adjust-body-height)** |
| [`left_joint_positions_blocking`](#fn-single-arm-joint) | 左臂关节阻塞运动 **〔未导出〕→ [`moveJ_robot`](#fn-movej)** |
| [`right_joint_positions_blocking`](#fn-single-arm-joint) | 右臂关节阻塞运动 **〔未导出〕→ [`moveJ_robot`](#fn-movej)** |
| [`dual_arm_joint_positions_blocking`](#fn-dual-arm-joint) | 双臂关节阻塞运动 **〔未导出〕→ [`moveJ_robot`](#fn-movej)** |
| [`adjust_body_height`](#fn-adjust-body-height) | 身体关节升降控制（按比例计算关节角） |
| [`move_waist_lifting_relative_blocking`](#fn-waist-lifting) | 腰部相对升降（米）并阻塞等待到位 |
| [`moveJ_robot`](#fn-movej) | 全身关节阻塞运动入口 |
| [`moveJ_path_robot`](#fn-movej-path) | 关节空间路径（waypoints）阻塞运动入口 |

| 灵巧手函数（`src/dexhand_skills.py`） | 功能 |
| --- | --- |
| [`send_left_hand_joint_positions`](#fn-left-hand) | 左灵巧手目标下发（非阻塞） **〔未导出〕→ [`send_dual_hand_joint_positions`](#fn-dual-hand-send)** |
| [`send_right_hand_joint_positions`](#fn-right-hand) | 右灵巧手目标下发（非阻塞） **〔未导出〕→ [`send_dual_hand_joint_positions`](#fn-dual-hand-send)** |
| [`send_dual_hand_joint_positions`](#fn-dual-hand-send) | 双手灵巧手目标下发（非阻塞，支持单手） |
| [`send_left_hand_joint_positions_blocking`](#fn-left-hand-blocking) | 左灵巧手目标下发并可阻塞等待 **〔未导出〕→ [`send_dual_hand_joint_positions_blocking`](#fn-dual-hand)** |
| [`send_right_hand_joint_positions_blocking`](#fn-right-hand-blocking) | 右灵巧手目标下发并可阻塞等待 **〔未导出〕→ [`send_dual_hand_joint_positions_blocking`](#fn-dual-hand)** |
| [`send_dual_hand_joint_positions_blocking`](#fn-dual-hand) | 双手灵巧手目标下发并可阻塞等待（支持单手） |

| 夹爪函数（`src/gripper_skills.py`） | 功能 |
| --- | --- |
| [`gripper_control_blocking`](#fn-gripper-control) | 单/双夹爪位置下发并可阻塞等待到位（多侧同时运动） |

## 4. skill 函数详细说明

> **入参示例说明**：下列已从包根导出的函数均提供「入参示例」；`pose_params`、`joint_params` 等字典结构与 `examples/` 脚本一致，可直接复制修改后使用。

### 4.1 基础函数

<a id="fn-init-robot"></a>
## `init_robot(config=None, config_path=None)`

- 用途：初始化 `ROS2RobotInterface` 实例。
- 典型场景：程序启动时创建机器人控制对象。
- 入参：
  - `config`（`Optional[Dict[str, Any]]`）：动态配置字典。
  - `config_path`（`Optional[str]`）：YAML 配置文件路径。
- 入参示例：

```python
from fa_robot_skills import get_default_config_path, init_robot

robot = init_robot(config_path=get_default_config_path())
# 或指定路径：robot = init_robot(config_path="configs/enhanced_robot_config.yaml")
```

- 出参：
  - `Optional[ROS2RobotInterface]`：成功返回实例，失败返回 `None`。

<a id="fn-connect-robot"></a>
## `connect_robot(robot, hold_wait=0.1)`

- 用途：连接机器人并切换到 HOLD 可用状态。
- 典型场景：所有动作执行前的连接与状态准备。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
  - `hold_wait`（`float`）：切换到 HOLD 后等待时间（秒）。
- 入参示例：

```python
from fa_robot_skills import connect_robot

ok = connect_robot(robot, hold_wait=0.1)
```

- 出参：
  - `bool`：`True` 表示成功；`False` 表示实例为空或连接失败。

<a id="fn-disconnect-robot"></a>
## `disconnect_robot(robot)`

- 用途：断开机器人连接。
- 典型场景：业务收尾、程序退出前。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
- 入参示例：

```python
from fa_robot_skills import disconnect_robot

disconnect_robot(robot)
```

- 出参：
  - `bool`：`True` 表示成功；`False` 表示实例为空或断开失败。

<a id="fn-robot-go-home"></a>
## `robot_go_home(robot, hold_wait=1.0, home_wait=5.0, ocs2_wait=0.1)`

- 用途：执行固定回 Home 流程（HOLD -> HOME -> HOLD -> OCS2）。
- 典型场景：动作序列结束后回归标准姿态。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
  - `hold_wait`（`float`）：切换 HOLD 后等待时间（秒）。
  - `home_wait`（`float`）：HOME 动作等待时间（秒）。
  - `ocs2_wait`（`float`）：切回 OCS2 后等待时间（秒）。
- 入参示例：

```python
from fa_robot_skills import robot_go_home

ok = robot_go_home(robot, hold_wait=1.0, home_wait=5.0, ocs2_wait=0.1)
```

- 出参：
  - `bool`：`True` 表示流程完成；`False` 表示实例为空、未连接且重连失败，或其他前置条件不满足。
- 关键说明：
  - 若 `robot.is_connected` 为 `False`，会先尝试 `connect_robot`；重连失败则直接返回 `False`，与 `switch_to_movej_state` 行为一致。

<a id="fn-switch-movej"></a>
## `switch_to_movej_state(robot, hold_wait=0.1, movej_wait=0.1)`

- 用途：切换到 MOVEJ（关节空间）状态。
- 典型场景：执行关节角运动前。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
  - `hold_wait`（`float`）：切换 HOLD 后等待时间（秒）。
  - `movej_wait`（`float`）：切换 MOVEJ 后等待时间（秒）。
- 入参示例：

```python
from fa_robot_skills import switch_to_movej_state

ok = switch_to_movej_state(robot, hold_wait=0.1, movej_wait=0.1)
```

- 出参：
  - `bool`：`True` 表示切换成功；`False` 表示连接或切换失败。

<a id="fn-switch-ocs2"></a>
## `switch_to_ocs2_state(robot, hold_wait=0.1, ocs2_wait=0.1)`

- 用途：切换到 OCS2（笛卡尔空间）状态。
- 典型场景：执行位姿运动前。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
  - `hold_wait`（`float`）：切换 HOLD 后等待时间（秒）。
  - `ocs2_wait`（`float`）：切换 OCS2 后等待时间（秒）。
- 入参示例：

```python
from fa_robot_skills import switch_to_ocs2_state

ok = switch_to_ocs2_state(robot, hold_wait=0.1, ocs2_wait=0.1)
```

- 出参：
  - `bool`：`True` 表示切换成功；`False` 表示连接或切换失败。

<a id="fn-switch-hold"></a>
## `switch_to_hold_state(robot, hold_wait=0.1)`

- 用途：切换到 HOLD 安全状态。
- 典型场景：动作结束后停留、切状态前中间态。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
  - `hold_wait`（`float`）：切换 HOLD 后等待时间（秒）。
- 入参示例：

```python
from fa_robot_skills import switch_to_hold_state

ok = switch_to_hold_state(robot, hold_wait=0.1)
```

- 出参：
  - `bool`：`True` 表示切换成功；`False` 表示连接或切换失败。

<a id="fn-get-joint"></a>
## `get_joint(robot)`

- 用途：获取机器人当前关节状态。
- 典型场景：动作前后状态采样、调试监控。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
- 入参示例：

```python
from fa_robot_skills import get_joint

joint_state = get_joint(robot)
```

- 出参：
  - `Optional[Dict[str, Any]]`：成功返回关节状态，失败返回 `None`。

<a id="fn-get-pose"></a>
## `left_get_pose(robot)` / `right_get_pose(robot)`

- 用途：获取左臂/右臂当前末端位姿。
- 典型场景：位姿闭环校验、记录当前 TCP 姿态。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
- 入参示例：

```python
from fa_robot_skills import left_get_pose, right_get_pose

left_pose = left_get_pose(robot)
right_pose = right_get_pose(robot)
```

- 出参：
  - `Optional[Pose]`：成功返回末端位姿，失败返回 `None`。

### 4.2 运动函数（笛卡尔空间运动）

<a id="fn-movel"></a>
## `moveL_robot(robot, pose_params, left_x_range=None, left_y_range=None, right_x_range=None, right_y_range=None, pcd=None, max_timeout=6.0, delta_position_threshold=None, required_delta_stable_cycles=10, vis_flag=False)`

- 用途：笛卡尔位姿运动入口（单臂或双臂）。
- 典型场景：单臂抓取对位；双臂同步预位姿、协同抓取前对齐。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
  - `pose_params`（`Optional[Dict[str, Any]]`）：位姿参数字典，支持：
    - `left_frame_id` / `right_frame_id`：左右臂参考坐标系（默认 `"arm_base"`）；双臂同步模式下底层使用单一 `frame_id`，与 `send_dual_arm_target_stamped` 一致，当前实现以 `left_frame_id` 为准。
    - `left_pose_6d` / `right_pose_6d`：左右臂基础 6D 位姿（至少提供一侧；同时提供两侧则为双臂同步）。
    - `left_pose_delta` / `right_pose_delta`：左右臂位姿增量（可选），含 `center` `[dx,dy,dz]`、`rotate` `[droll,dpitch,dyaw]`。
    - `left_delta_type` / `right_delta_type`：左右臂增量施加方式（可选，默认 `"update_pose_with_delta"`）：
      - `"update_pose_with_delta"`：在**参考系**（如 `arm_base`）下加减平移、左乘旋转。
      - `"update_pose_with_self_delta"`：在**末端局部坐标系**下施加增量（`T_new = T_old @ T_delta`）。
  - `left_x_range` / `left_y_range`：左臂工作空间 x/y 范围 `(min, max)`，为 `None` 时不检查。
  - `right_x_range` / `right_y_range`：右臂工作空间 x/y 范围，为 `None` 时不检查。
  - `pcd`（`Optional[o3d.geometry.PointCloud]`）：点云，可视化时使用。
  - `max_timeout`（`float`）：最大等待时间（秒）。
  - `delta_position_threshold`（`Optional[float]`）：连续两次 distance 差值阈值；小于阈值并连续满足次数后可判定到位。
  - `required_delta_stable_cycles`（`int`）：触发 delta 到位判定所需的连续稳定次数。
  - `vis_flag`（`bool`）：是否开启可视化。
- 入参示例：

```python
from fa_robot_skills import moveL_robot

# 单臂 MoveL（见 examples/test_robot_pose.py）
pose_params = {
    "left_frame_id": "arm_base",
    "left_pose_6d": [
        0.50,
        0.41605525354326556,
        -0.4722839588803575,
        -2.9961878046703516,
        -1.5480078367563173,
        -0.13907640660132747,
    ],
    "left_pose_delta": {"center": [0.0, 0.0, 0.0], "rotate": [0.0, 0.0, 0.0]},
}
result = moveL_robot(robot=robot, pose_params=pose_params, max_timeout=10.0)

# 双臂同步 MoveL
dual_pose_params = {
    "left_frame_id": "arm_base",
    "right_frame_id": "arm_base",
    "left_pose_6d": pose_params["left_pose_6d"],
    "right_pose_6d": [
        0.50,
        -0.4160552535432655,
        -0.4722839588803578,
        -2.9961878046703516,
        -1.5480078367563173,
        -0.13907640660132747,
    ],
    "left_pose_delta": {"center": [0.0, 0.0, 0.0], "rotate": [0.0, 0.0, 0.0]},
    "right_pose_delta": {"center": [0.0, 0.0, 0.0], "rotate": [0.0, 0.0, 0.0]},
}
result = moveL_robot(robot=robot, pose_params=dual_pose_params, max_timeout=10.0)
```

- 出参：
  - `Optional[Dict[str, Optional[Pose]]]`：成功返回左右臂最终位姿字典（单臂时未执行的一侧为 `None`）；失败返回 `None`。
- 关键说明：
  - 左右臂可分别指定 `left_delta_type` / `right_delta_type`，互不影响。
  - 双臂时通过 `send_dual_arm_stamped_blocking` 同步下发，支持位置阈值+delta 稳定联合到位检查。

<a id="fn-movel-path-pose-list"></a>
## `moveL_path_robot_by_pose_list(robot, pose_params, frame_id="arm_base", left_x_range=None, left_y_range=None, right_x_range=None, right_y_range=None, pcd=None, trajectory_duration=10.0, max_timeout=10.0, delta_position_threshold=None, required_delta_stable_cycles=10, vis_flag=False)`

- 用途：基于 `execute_path` 的路径运动（`*_pose_6d_list` + 单一 `*_pose_delta`）。
- 典型场景：已规划好多个关键位姿点，每个点统一附加同一增量。
- 入参（核心）：
  - `pose_params`：支持 `left_pose_6d_list` / `right_pose_6d_list`（路径点列表）；
  - `left_pose_delta` / `right_pose_delta`：单一位姿增量，应用到列表中的每个路径点；
  - `left_delta_type` / `right_delta_type`：增量施加方式（默认 `update_pose_with_delta`，可选 `update_pose_with_self_delta`）；
  - `frame_id`：路径输入参考系（统一单入参，不再拆分左右 `frame_id`）；
  - `trajectory_duration`：路径总执行时长（秒）；
  - `max_timeout`、`delta_position_threshold`、`required_delta_stable_cycles`：到位等待策略参数。
- 入参示例：

```python
from fa_robot_skills import moveL_path_robot_by_pose_list

# 双臂路径（见 examples/test_robot_path_pose.py）
pose_params = {
    "left_pose_6d_list": [
        [
            0.50,
            0.41605525354326556,
            -0.4722839588803575,
            -2.9961878046703516,
            -1.5480078367563173,
            -0.13907640660132747,
        ],
        [
            0.52,
            0.40605525354326556,
            -0.4622839588803575,
            -2.9961878046703516,
            -1.5480078367563173,
            -0.13907640660132747,
        ],
    ],
    "right_pose_6d_list": [
        [
            0.50,
            -0.4160552535432655,
            -0.4722839588803578,
            -2.9961878046703516,
            -1.5480078367563173,
            -0.13907640660132747,
        ],
        [
            0.52,
            -0.4060552535432655,
            -0.4622839588803578,
            -2.9961878046703516,
            -1.5480078367563173,
            -0.13907640660132747,
        ],
    ],
    "left_pose_delta": {"center": [0.0, 0.0, 0.0], "rotate": [0.0, 0.0, 0.0]},
    "right_pose_delta": {"center": [0.0, 0.0, 0.0], "rotate": [0.0, 0.0, 0.0]},
}
result = moveL_path_robot_by_pose_list(
    robot=robot,
    pose_params=pose_params,
    frame_id="arm_base",
    trajectory_duration=6.0,
    max_timeout=10.0,
)
```

- 出参：
  - `Optional[Dict[str, List[Pose]]]`：成功返回 `{"left_poses": [...], "right_poses": [...]}`；失败返回 `None`。

<a id="fn-movel-path-delta-list"></a>
## `moveL_path_robot_by_delta_list(robot, pose_params, frame_id="arm_base", left_x_range=None, left_y_range=None, right_x_range=None, right_y_range=None, pcd=None, trajectory_duration=10.0, max_timeout=10.0, delta_position_threshold=None, required_delta_stable_cycles=10, vis_flag=False)`

- 用途：基于 `execute_path` 的路径运动（单一 `*_pose_6d` + `*_pose_delta_list`）。
- 典型场景：以一个基准位姿为起点，按多个增量逐点生成路径。
- 入参（核心）：
  - `pose_params`：支持单一 `left_pose_6d` / `right_pose_6d` 与对应的 `left_pose_delta_list` / `right_pose_delta_list`；
  - `frame_id`、`trajectory_duration`、`max_timeout`、`delta_position_threshold`、`required_delta_stable_cycles` 同上。
- 入参示例：

```python
from fa_robot_skills import moveL_path_robot_by_delta_list

# 单臂路径（见 examples/test_robot_path_pose.py）
pose_params = {
    "left_pose_6d": [
        0.6637955857096086,
        0.6523936241384611,
        -0.31678532759582423,
        1.4450741275231045,
        -1.4571234650515925,
        0.07933317415685659,
    ],
    "left_pose_delta_list": [
        {
            "center": [0.205, -0.085, 0.004],
            "rotate": [1.5707963267948966, 0.0, 0.0],
        },
        {
            "center": [0.07, -0.085, 0.004],
            "rotate": [1.5707963267948966, 0.0, 0.0],
        },
        {
            "center": [0.205, -0.085, 0.004],
            "rotate": [1.5707963267948966, 0.0, 0.0],
        },
    ],
    "left_delta_type": "update_pose_with_self_delta",
}
result = moveL_path_robot_by_delta_list(
    robot=robot,
    pose_params=pose_params,
    frame_id="arm_base",
    trajectory_duration=6.0,
    max_timeout=10.0,
)
```

- 出参：
  - `Optional[Dict[str, List[Pose]]]`：成功返回 `{"left_poses": [...], "right_poses": [...]}`；失败返回 `None`。

<a id="fn-single-arm-pose"></a>
## `left_move_pose_stamped_blocking(...)` / `right_move_pose_stamped_blocking(...)`

> **〔未从包根导出〕** 推荐改用 [`moveL_robot`](#fn-movel)：左臂仅填 `left_pose_6d`，右臂仅填 `right_pose_6d`；`moveL_robot` 会自动切换 OCS2 并阻塞等待到位。

- 用途：左臂/右臂单臂位姿目标下发，并可阻塞等待。
- 典型场景：单臂抓取点对位、单臂放置动作。
- 入参（核心）：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
  - `xyz` + `rpy` 或 `pose`：目标位姿。
  - `frame_id`（`Optional[str]`）：目标坐标系（`send_target_stamped=True` 时必填）。
  - `wait_until_complete`（`bool`）：是否等待到位。
  - `max_timeout`（`float`）：最大等待时间（秒）。
- 出参：
  - `bool`：`True` 表示成功；`False` 表示参数错误、下发失败或超时。

<a id="fn-dual-arm-path"></a>
## `send_dual_arm_stamped_blocking(robot, left_poses, right_poses, frame_id=None, wait_until_complete=True, max_timeout=15.0, delta_position_threshold=None, required_delta_stable_cycles=10, use_stamped=True)`

> **〔未从包根导出〕** 推荐改用 [`moveL_robot`](#fn-movel)（单点双臂）或 [`moveL_path_robot_by_pose_list`](#fn-movel-path-pose-list) / [`moveL_path_robot_by_delta_list`](#fn-movel-path-delta-list)（多点路径）。`moveL_robot` 内部即调用本函数。

- 用途：双臂 Pose/路径下发入口。
- 典型场景：双臂同时到位、协同抓取前预位姿、双臂镜像动作。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
  - `left_poses`（`List[Pose | PoseStamped]`）：左臂目标列表。
  - `right_poses`（`List[Pose | PoseStamped]`）：右臂目标列表。
  - `frame_id`（`Optional[str]`）：参考坐标系；`use_stamped=False` 时透传。
  - `wait_until_complete`（`bool`）：是否阻塞等待双臂到位。
  - `max_timeout`（`float`）：最大等待时间（秒）。
  - `delta_position_threshold`（`Optional[float]`）：连续两次 distance 差值阈值；小于阈值并连续满足次数后可判定到位。
  - `required_delta_stable_cycles`（`int`）：触发 delta 到位判定所需的连续稳定次数。
  - `use_stamped`（`bool`）：是否使用 stamped 接口发送（默认 `True`）。
- 出参：
  - `bool`：`True` 表示双臂动作成功；`False` 表示下发失败或等待超时。
- 关键说明：
  - `use_stamped=True` 时，要求 `left_poses` 和 `right_poses` 都非空，且长度必须一致；
  - `use_stamped=True` 时会按 `left_poses[i]` 与 `right_poses[i]` 成对逐点发送（for 循环），而不是只发送首个目标；
  - 双臂模式使用单一 `frame_id`，请确保左右臂目标在同一参考坐标系下表达。

### 4.3 运动函数（关节空间运动）

<a id="fn-movej"></a>
## `moveJ_robot(robot, joint_params=None, hold_wait=0.1, movej_wait=0.1, max_timeout=15.0)`

- 用途：全身关节阻塞运动入口。
- 典型场景：关节空间批量动作执行。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
  - `joint_params`（`Optional[Dict[str, Any]]`）：关节目标字典，支持 `left_joints` / `right_joints` / `head_joints` / `body_joints`。
    - 其中各字段可传 `None` 或空列表 `[]`，两者都会被视为“该部位未提供目标”并自动跳过。
  - `hold_wait`（`float`）：切换 HOLD 后等待时间（秒）。
  - `movej_wait`（`float`）：切换 MOVEJ 后等待时间（秒）。
  - `max_timeout`（`float`）：各部位到位等待的最大时间（秒）。
- 入参示例：

```python
from fa_robot_skills import moveJ_robot

# 见 examples/test_robot_movej.py
joint_params = {
    "left_joints": [
        1.457133809892805,
        -1.0633699963902674,
        -1.1994065285584414,
        -0.8074199090303408,
        -0.6949558811978124,
        -0.6864381599253574,
        -0.1295225067208675,
    ],
    "right_joints": [
        -1.457133809892805,
        -1.0633699963902674,
        1.1994065285584414,
        -0.8074199090303408,
        0.6949558811978124,
        -0.6864381599253574,
        0.1295225067208675,
    ],
    "head_joints": [0.0, 0.26],
    "body_joints": [-1.2, -2.4, -1.2, 0],
}
ok = moveJ_robot(robot=robot, joint_params=joint_params, max_timeout=15.0)
```

- 出参：
  - `bool`：`True` 表示动作完成；`False` 表示前置或执行失败。

<a id="fn-movej-path"></a>
## `moveJ_path_robot(robot, joint_path_params=None, hold_wait=0.1, movej_wait=0.1, max_timeout=10.0, wait_send_time=1.0)`

- 用途：关节空间路径阻塞运动入口（基于 `send_joint_trajectory`）。
- 典型场景：已规划好多组关节角路径点，需沿轨迹平滑运动。
- 导入：`from fa_robot_skills import moveJ_path_robot`（或本地 `from src.robot_skills import moveJ_path_robot`）。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
  - `joint_path_params`（`Optional[Dict[str, Any]]`）：关节路径参数字典，支持：
    - `joint_names`（`List[str]`）：轨迹涉及的关节名列表。
    - `waypoints`（`List[List[float]]`）：路径点列表，至少 2 个；每个路径点维度须与 `joint_names` 一致。
    - `trajectory_duration`（`float`）：轨迹总执行时长（秒），默认 `6.0`。
  - `hold_wait` / `movej_wait`（`float`）：状态切换等待时间（秒）。
  - `max_timeout`（`float`）：最大等待时间（秒）。
  - `wait_send_time`（`float`）：轨迹下发后的初始等待时间（秒），用于等待轨迹发布完成。
- 入参示例：

```python
from fa_robot_skills import moveJ_path_robot

# 见 examples/test_robot_path_joint.py；joint_names 须与 /joint_states 一致
joint_path_params = {
    "joint_names": [
        "left_joint1",
        "left_joint2",
        "left_joint3",
        "left_joint4",
        "left_joint5",
        "left_joint6",
        "left_joint7",
    ],
    "waypoints": [
        [
            -0.49656642553247676,
            0.811654135426485,
            0.8946941613762457,
            -2.310527402767993,
            -0.9513723546436391,
            -0.18877228162635498,
            0.10809830785756154,
        ],
        [
            0.3812173287120383,
            0.9668056394767574,
            0.8510952182754222,
            -2.3587700332825605,
            -0.8949941664730432,
            -1.0036957468965348,
            -0.0,
        ],
    ],
    "trajectory_duration": 4.0,
}
ok = moveJ_path_robot(
    robot=robot,
    joint_path_params=joint_path_params,
    max_timeout=10.0,
)
```

- 出参：
  - `bool`：`True` 表示末路径点到位；`False` 表示前置或执行失败。
- 关键说明：
  - 执行前会自动切换到 MOVEJ 状态。
  - 到位判定以末路径点与当前 `joint_states` 的欧氏距离是否小于 `robot.config.position_threshold` 为准。

<a id="fn-single-arm-joint"></a>
## `left_joint_positions_blocking(robot, positions, max_timeout=15.0)` / `right_joint_positions_blocking(robot, positions, max_timeout=15.0)`

> **〔未从包根导出〕** 推荐改用 [`moveJ_robot`](#fn-movej)：
>
> ```python
> moveJ_robot(robot, joint_params={"left_joints": positions})   # 左臂
> moveJ_robot(robot, joint_params={"right_joints": positions})  # 右臂
> ```
>
> `moveJ_robot` 会先切换 MOVEJ 状态，再下发并阻塞等待到位。

- 用途：左臂/右臂关节阻塞运动。
- 典型场景：单臂关节动作、手工示教复现。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
  - `positions`（`List[float]`）：目标关节角列表。
  - `max_timeout`（`float`）：最大等待时间（秒）。
- 出参：
  - `bool`：`True` 表示到位；`False` 表示下发失败或超时。

<a id="fn-dual-arm-joint"></a>
## `dual_arm_joint_positions_blocking(robot, left_positions, right_positions, max_timeout=15.0)`

> **〔未从包根导出〕** 推荐改用 [`moveJ_robot`](#fn-movej)：
>
> ```python
> moveJ_robot(robot, joint_params={
>     "left_joints": left_positions,
>     "right_joints": right_positions,
> })
> ```

- 用途：双臂关节阻塞运动。
- 典型场景：双臂同步开合、镜像关节动作。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
  - `left_positions`（`List[float]`）：左臂目标关节角。
  - `right_positions`（`List[float]`）：右臂目标关节角。
  - `max_timeout`（`float`）：最大等待时间（秒）。
- 出参：
  - `bool`：`True` 表示双臂到位；`False` 表示下发失败或超时。

<a id="fn-head-joint"></a>
## `send_head_joint_positions_blocking(robot, positions, wait_until_complete=True, max_timeout=15.0)`

> **〔未从包根导出〕** 推荐改用 [`moveJ_robot`](#fn-movej)：`joint_params={"head_joints": positions}`。

- 用途：头部关节目标下发，并可阻塞等待。
- 典型场景：视野调整、传感器姿态归位。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
  - `positions`（`List[float]`）：头部关节目标角。
  - `wait_until_complete`（`bool`）：是否等待到位。
  - `max_timeout`（`float`）：最大等待时间（秒）。
- 出参：
  - `bool`：`True` 表示成功；`False` 表示下发失败或等待超时。

<a id="fn-body-joint"></a>
## `send_body_joint_positions_blocking(robot, positions, wait_until_complete=True, max_timeout=15.0)`

> **〔未从包根导出〕** 推荐改用：
>
> - 精确关节角：[`moveJ_robot`](#fn-movej) 的 `joint_params={"body_joints": positions}`
> - 按比例升降：[`adjust_body_height`](#fn-adjust-body-height) 的 `up_rate` 参数

- 用途：身体关节目标下发，并可阻塞等待。
- 典型场景：机身高度调整、姿态准备。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
  - `positions`（`List[float]`）：身体关节目标角。
  - `wait_until_complete`（`bool`）：是否等待到位。
  - `max_timeout`（`float`）：最大等待时间（秒）。
- 出参：
  - `bool`：`True` 表示成功；`False` 表示下发失败或等待超时。

<a id="fn-adjust-body-height"></a>
## `adjust_body_height(robot, up_rate=0.5)`

- 用途：按照比例计算并执行身体关节升降。
- 典型场景：工位高度适配、视角高度调节。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
  - `up_rate`（`float`）：升降比例，建议范围 `[0, 1]`。
- 入参示例：

```python
from fa_robot_skills import adjust_body_height

adjust_body_height(robot, up_rate=0.5)  # 0=最低，1=最高
```

- 出参：
  - `None`。

<a id="fn-waist-lifting"></a>
## `move_waist_lifting_relative_blocking(robot, relative_position, body_delta_threshold=1e-4, required_delta_stable_cycles=10, waist_lifting_duration=5.0, wait_time=7.0)`

- 用途：控制腰部相对升降（米），并阻塞等待到位。
- 典型场景：工位高度微调、按相对位移升降机身。
- 导入：`from fa_robot_skills import move_waist_lifting_relative_blocking`（或本地 `from src.robot_skills import ...`）。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
  - `relative_position`（`float`）：相对位移（米）；正数上升，负数下降。
  - `body_delta_threshold`（`float`）：body 关节位置变化阈值，用于判定稳定。
  - `required_delta_stable_cycles`（`int`）：连续稳定到位所需的次数。
  - `waist_lifting_duration`（`float`）：单次升降运动时间（秒）。
  - `wait_time`（`float`）：最大等待时间（秒）。
- 入参示例：

```python
from fa_robot_skills import move_waist_lifting_relative_blocking

# relative_position 单位：米；正数上升，负数下降
ok = move_waist_lifting_relative_blocking(
    robot=robot,
    relative_position=0.05,
    waist_lifting_duration=5.0,
    wait_time=7.0,
)
```

- 出参：
  - `bool`：`True` 表示到位；`False` 表示失败或超时。
- 关键说明：
  - 执行前会自动切换到 MOVEJ 状态，并通过 `/body_joint_controller` 设置 `waist_lifting_duration`。
  - 到位后自动切回 HOLD 状态；超时也会切回 HOLD。

### 4.4 灵巧手函数

灵巧手对外推荐从包根导入（`send_dual_hand_joint_positions` / `send_dual_hand_joint_positions_blocking`）；完整 API 定义于 `src/dexhand_skills.py`：

```python
# pip install 后（推荐）
from fa_robot_skills import send_dual_hand_joint_positions_blocking

# 本地开发（与 examples 一致）
from src.dexhand_skills import send_dual_hand_joint_positions_blocking
```

<a id="fn-left-hand"></a>
## `send_left_hand_joint_positions(robot, positions)` / `send_right_hand_joint_positions(robot, positions)`

> **〔未从包根导出〕** 推荐改用 [`send_dual_hand_joint_positions`](#fn-dual-hand-send)，另一侧传 `None`：
>
> ```python
> send_dual_hand_joint_positions(robot, left_positions=positions, right_positions=None)   # 左手
> send_dual_hand_joint_positions(robot, left_positions=None, right_positions=positions)  # 右手
> ```

- 用途：左/右灵巧手关节目标下发（非阻塞）。
- 典型场景：仅需下发目标、由上层自行等待或无需等待到位。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
  - `positions`（`List[float]`）：各关节目标（归一化 `0.0-1.0`）。
- 出参：
  - `bool`：`True` 表示已尝试发布；`False` 表示 robot 无效、未连接或发布器未创建。

<a id="fn-dual-hand-send"></a>
## `send_dual_hand_joint_positions(robot, left_positions=None, right_positions=None)`

- 用途：双手灵巧手关节目标下发（非阻塞），支持只下发单手。
- 入参：
  - `left_positions` / `right_positions`：传 `None` 表示跳过对应侧。
- 入参示例：

```python
from fa_robot_skills import send_dual_hand_joint_positions

# 非阻塞：仅下发，不等待到位
ok = send_dual_hand_joint_positions(
    robot=robot,
    left_positions=[0.9804, 0.0, 0.7843, 0.7843, 0.7843, 0.7843, 0.9804],
    right_positions=[0.9804, 0.0, 0.7843, 0.7843, 0.7843, 0.7843, 0.9804],
)

# 单手模式：仅左手
ok = send_dual_hand_joint_positions(
    robot=robot,
    left_positions=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
    right_positions=None,
)
```

- 出参：
  - `bool`：`True` 表示所有请求下发的手均成功发布。

<a id="fn-left-hand-blocking"></a>
## `send_left_hand_joint_positions_blocking(robot, positions, wait_until_complete=True, max_timeout=10.0, position_threshold=None)` / `send_right_hand_joint_positions_blocking(...)`

> **〔未从包根导出〕** 推荐改用 [`send_dual_hand_joint_positions_blocking`](#fn-dual-hand)，另一侧传 `None`：
>
> ```python
> send_dual_hand_joint_positions_blocking(robot, left_positions=positions, right_positions=None)   # 左手
> send_dual_hand_joint_positions_blocking(robot, left_positions=None, right_positions=positions)  # 右手
> ```
>
> 包根函数会先 `switch_to_ocs2_state`，行为比单函数更完整。

- 用途：左/右灵巧手关节目标下发，并可按需阻塞等待到位。
- 入参：
  - `wait_until_complete`（`bool`）：是否等待到位后返回。
  - `max_timeout`（`float`）：最大等待时间（秒）。
  - `position_threshold`（`Optional[float]`）：逐关节绝对误差阈值；`None` 时使用 `robot.config.position_threshold`。
- 出参：
  - `bool`：`True` 表示下发成功且（若等待）关节反馈在阈值内。

<a id="fn-dual-hand"></a>
## `send_dual_hand_joint_positions_blocking(robot, left_positions=None, right_positions=None, wait_until_complete=True, max_timeout=15.0, position_threshold=None)`

- 用途：下发双手灵巧手目标，并可按需阻塞等待到位。
- 典型场景：双手同步抓取准备、单手手势控制、抓取前手指预形。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
  - `left_positions`（`Optional[List[float]]`）：左手目标关节向量，传 `None` 表示跳过左手。
  - `right_positions`（`Optional[List[float]]`）：右手目标关节向量，传 `None` 表示跳过右手。
  - `wait_until_complete`（`bool`）：是否等待到位后返回。
  - `max_timeout`（`float`）：最大等待时间（秒）。
  - `position_threshold`（`Optional[float]`）：逐关节绝对误差阈值；`None` 时使用 `robot.config.position_threshold`。
- 入参示例：

```python
from fa_robot_skills import send_dual_hand_joint_positions_blocking

# 见 examples/test_dexhand_skills.py
ok = send_dual_hand_joint_positions_blocking(
    robot=robot,
    left_positions=[0.9804, 0.0, 0.7843, 0.7843, 0.7843, 0.7843, 0.9804],
    right_positions=[0.9804, 0.0, 0.7843, 0.7843, 0.7843, 0.7843, 0.9804],
    wait_until_complete=True,
    max_timeout=15.0,
    position_threshold=0.5,
)

# 单手模式：仅左手张开
ok = send_dual_hand_joint_positions_blocking(
    robot=robot,
    left_positions=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
    right_positions=None,
    wait_until_complete=True,
    max_timeout=15.0,
)
```

- 出参：
  - `bool`：`True` 表示下发成功且（若等待）目标侧均在阈值内；`False` 表示切状态失败、下发失败或等待超时。
- 关键说明：
  - `left_positions` 与 `right_positions` 不能同时为 `None`。
  - 发送前会自动调用 `switch_to_ocs2_state(robot)`，确保处于可控状态。
  - 实际下发后，只有在 `wait_until_complete=True` 时才执行阻塞等待。

### 4.5 夹爪函数

夹爪对外推荐从包根导入 `gripper_control_blocking`；完整 API 定义于 `src/gripper_skills.py`：

```python
# pip install 后（推荐）
from fa_robot_skills import gripper_control_blocking

# 本地开发（与 examples 一致）
from src.gripper_skills import gripper_control_blocking
```

<a id="fn-gripper-control"></a>
## `gripper_control_blocking(robot=None, gripper_types=None, left_gripper_target_position=1.0, right_gripper_target_position=1.0, wait_until_complete=True, max_timeout=2.0, position_threshold=None)`

- 用途：单/双夹爪位置下发，并可阻塞等待全部目标侧到位。
- 典型场景：抓取前夹爪同步开合、单侧夹爪控制。
- 入参：
  - `robot`（`Optional[ROS2RobotInterface]`）：机器人实例。
  - `gripper_types`（`Optional[List[str]]`）：侧别列表，支持 `"left"` / `"right"`；为 `None` 时默认 `["left", "right"]`。
  - `left_gripper_target_position` / `right_gripper_target_position`（`float`）：左右夹爪目标位置（行程值，超出范围时由底层 clamp）。
  - `wait_until_complete`（`bool`）：是否等待到位后返回。
  - `max_timeout`（`float`）：全部夹爪到位的最大等待时间（秒）。
  - `position_threshold`（`Optional[float]`）：位置距离阈值；`None` 时使用 `config.gripper_position_threshold`。
- 入参示例：

```python
from fa_robot_skills import gripper_control_blocking

# 见 examples/test_gripper_skills.py
gripper_params = {
    "gripper_types": ["left", "right"],
    "left_gripper_target_position": 0.0384,
    "right_gripper_target_position": 0.0384,
    "wait_until_complete": True,
    "max_timeout": 2.0,
}
ok = gripper_control_blocking(robot=robot, **gripper_params)

# 单夹爪
ok = gripper_control_blocking(
    robot=robot,
    gripper_types=["left"],
    left_gripper_target_position=0.0,
    max_timeout=2.0,
)
```

- 出参：
  - `bool`：`True` 表示下发成功且（若等待）全部目标侧均已到位；`False` 表示连接/handler/参数错误或等待超时。
- 关键说明：
  - 多侧运动时先批量 `send_joint_positions`，再统一轮询 `GripperHandler.check_arrival`。
  - 关闭时除距离阈值外，还支持位置历史稳定判定（夹持物体时适用）。
  - `robot is None` 时返回 `False`；`gripper_types` 为空、侧别非法、未连接或 handler 未初始化时返回 `False`。
