fa-robot-skills 0.2.6

ROS 2 robot skills library for arm, dexhand, and gripper control

fa_robot_skills 使用说明

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

目录

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

0. 安装

环境要求：Python 3.12

从 PyPI 安装

pip install fa-robot-skills

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

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 的包根目录下，可直接运行示例：

python examples/test_robot_basic.py

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

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 字段维护版本号（当前单一来源）。

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

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

# 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。

# 可编辑安装
pip install -e .

# 构建 wheel / sdist
python -m build

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

安装后同时可用：

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）
仓库根目录运行 examples/	同上；示例脚本会自动将包根目录加入 sys.path
未从包根导出的函数（见下节）	PyPI：from fa_robot_skills.src.robot_skills import moveJ_path_robot；本地：from src.robot_skills import moveJ_path_robot
灵巧手 / 夹爪	PyPI：from fa_robot_skills.src.dexhand_skills import ...；本地：from src.dexhand_skills import ...

fa_robot_skills 包根（__init__.py）导出的函数见 1. 文件结构 中的说明；未导出的函数需从对应子模块导入，功能不受影响。

1. 文件结构与功能说明

文件/目录	功能说明
__init__.py	包根统一导出入口：从 src/robot_skills.py 及 src/robot_skills_utils.py 再导出常用 skill 与位姿工具（见下方「包根导出范围」）
src/	基础源码目录，包含核心实现与工具模块
src/robot_skills.py	核心 skill 实现，包含连接、状态切换、MoveJ/MoveL/路径、单双臂目标下发、腰部升降等
src/dexhand_skills.py	灵巧手 skill 实现，包含左/右/双手关节目标下发与阻塞等待
src/gripper_skills.py	夹爪 skill 实现，包含单/双夹爪位置下发与阻塞等待到位
src/robot_skills_utils.py	工具函数，包含位姿构建、位姿增量处理、可达性判断等
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.md	使用文档（当前文件）
README_ROS2.md	ROS2 topic/node 调用说明与 JSON 指令示例

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

已导出（可直接从包根导入）：init_robot、connect_robot、disconnect_robot、robot_go_home、状态切换（switch_to_*）、关节/位姿读取、moveJ_robot、moveL_robot、moveL_path_robot_by_pose_list、moveL_path_robot_by_delta_list、单双臂阻塞运动、adjust_body_height，以及 update_pose_with_delta、update_pose_with_self_delta、is_pose_reachable。

未从包根导出（需从子模块导入，见 导入方式）：

模块	典型函数
src/robot_skills.py	moveJ_path_robot、move_waist_lifting_relative_blocking
src/dexhand_skills.py	send_*_hand_joint_positions* 系列
src/gripper_skills.py	gripper_control_blocking

2. 快速使用

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

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

examples 目录下的 test_robot_basic.py 文件

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)：流程结束后安全断线

代码示例

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 文件

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)：流程结束后安全断线

代码示例

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 部分）

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；底层经 send_dual_arm_stamped_blocking 同步下发，参考系以 left_frame_id 为准（默认 arm_base）。
• 若需多点路径，建议优先使用：
  • moveL_path_robot_by_pose_list：pose_list + 单一 delta
  • moveL_path_robot_by_delta_list：单一 pose + delta_list

代码示例（单臂 MoveL）

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）

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 文件

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 风格）

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 文件

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（或 fa_robot_skills.src.robot_skills），见 包根导出范围。

代码示例

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 文件

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_left_hand_joint_positions_blocking / send_right_hand_joint_positions_blocking / send_dual_hand_joint_positions_blocking：下发目标并可阻塞等待到位
4. disconnect_robot(robot)：流程结束后安全断线

关键说明

• 灵巧手函数从 src.dexhand_skills 导入（未纳入 __init__.py 包根导出，见 包根导出范围）。
• 灵巧手目标值使用归一化范围 0.0-1.0（0.0=最小位置，1.0=最大位置）。
• 若只需控制单手，可将另一侧参数传 None（例如 right_positions=None）。
• 阻塞接口发送前会自动尝试切换到 OCS2 状态。

代码示例

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 文件

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)：流程结束后安全断线

关键说明

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

代码示例

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 函数简略清单

基础函数	功能
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	获取右臂末端位姿

运动函数（笛卡尔空间运动）	功能
left_move_pose_stamped_blocking	左臂位姿下发并可阻塞等待
right_move_pose_stamped_blocking	右臂位姿下发并可阻塞等待
send_dual_arm_stamped_blocking	双臂 pose/path 下发并等待到位
moveL_robot	单臂/双臂笛卡尔位姿运动入口
moveL_path_robot_by_pose_list	路径运动（pose_list + 单一 delta）
moveL_path_robot_by_delta_list	路径运动（单一 pose + delta_list）

运动函数（关节空间运动）	功能
send_head_joint_positions_blocking	头部关节下发并可阻塞等待
send_body_joint_positions_blocking	身体关节下发并可阻塞等待
left_joint_positions_blocking	左臂关节阻塞运动
right_joint_positions_blocking	右臂关节阻塞运动
dual_arm_joint_positions_blocking	双臂关节阻塞运动
adjust_body_height	身体关节升降控制（按比例计算关节角）
move_waist_lifting_relative_blocking	腰部相对升降（米）并阻塞等待到位（未从包根导出）
moveJ_robot	全身关节阻塞运动入口
moveJ_path_robot	关节空间路径（waypoints）阻塞运动入口（未从包根导出，见 包根导出范围）

灵巧手函数（src/dexhand_skills.py）	功能
send_left_hand_joint_positions	左灵巧手目标下发（非阻塞）
send_right_hand_joint_positions	右灵巧手目标下发（非阻塞）
send_dual_hand_joint_positions	双手灵巧手目标下发（非阻塞，支持单手）
send_left_hand_joint_positions_blocking	左灵巧手目标下发并可阻塞等待
send_right_hand_joint_positions_blocking	右灵巧手目标下发并可阻塞等待
send_dual_hand_joint_positions_blocking	双手灵巧手目标下发并可阻塞等待（支持单手）

夹爪函数（src/gripper_skills.py）	功能
gripper_control_blocking	单/双夹爪位置下发并可阻塞等待到位（多侧同时运动）

4. skill 函数详细说明

4.1 基础函数

init_robot(config=None, config_path=None)

• 用途：初始化 ROS2RobotInterface 实例。
• 典型场景：程序启动时创建机器人控制对象。
• 入参：
  • config（Optional[Dict[str, Any]]）：动态配置字典。
  • config_path（Optional[str]）：YAML 配置文件路径。
• 出参：
  • Optional[ROS2RobotInterface]：成功返回实例，失败返回 None。

connect_robot(robot, hold_wait=0.1)

• 用途：连接机器人并切换到 HOLD 可用状态。
• 典型场景：所有动作执行前的连接与状态准备。
• 入参：
  • robot（Optional[ROS2RobotInterface]）：机器人实例。
  • hold_wait（float）：切换到 HOLD 后等待时间（秒）。
• 出参：
  • bool：True 表示成功；False 表示实例为空或连接失败。

disconnect_robot(robot)

• 用途：断开机器人连接。
• 典型场景：业务收尾、程序退出前。
• 入参：
  • robot（Optional[ROS2RobotInterface]）：机器人实例。
• 出参：
  • bool：True 表示成功；False 表示实例为空或断开失败。

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 后等待时间（秒）。
• 出参：
  • bool：True 表示流程完成；False 表示实例为空、未连接且重连失败，或其他前置条件不满足。
• 关键说明：
  • 若 robot.is_connected 为 False，会先尝试 connect_robot；重连失败则直接返回 False，与 switch_to_movej_state 行为一致。

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 后等待时间（秒）。
• 出参：
  • bool：True 表示切换成功；False 表示连接或切换失败。

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 后等待时间（秒）。
• 出参：
  • bool：True 表示切换成功；False 表示连接或切换失败。

switch_to_hold_state(robot, hold_wait=0.1)

• 用途：切换到 HOLD 安全状态。
• 典型场景：动作结束后停留、切状态前中间态。
• 入参：
  • robot（Optional[ROS2RobotInterface]）：机器人实例。
  • hold_wait（float）：切换 HOLD 后等待时间（秒）。
• 出参：
  • bool：True 表示切换成功；False 表示连接或切换失败。

get_joint(robot)

• 用途：获取机器人当前关节状态。
• 典型场景：动作前后状态采样、调试监控。
• 入参：
  • robot（Optional[ROS2RobotInterface]）：机器人实例。
• 出参：
  • Optional[Dict[str, Any]]：成功返回关节状态，失败返回 None。

left_get_pose(robot) / right_get_pose(robot)

• 用途：获取左臂/右臂当前末端位姿。
• 典型场景：位姿闭环校验、记录当前 TCP 姿态。
• 入参：
  • robot（Optional[ROS2RobotInterface]）：机器人实例。
• 出参：
  • Optional[Pose]：成功返回末端位姿，失败返回 None。

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

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

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：到位等待策略参数。
• 出参：
  • Optional[Dict[str, List[Pose]]]：成功返回 {"left_poses": [...], "right_poses": [...]}；失败返回 None。

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 同上。
• 出参：
  • Optional[Dict[str, List[Pose]]]：成功返回 {"left_poses": [...], "right_poses": [...]}；失败返回 None。

left_move_pose_stamped_blocking(...) / right_move_pose_stamped_blocking(...)

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

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)

• 用途：双臂 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，请确保左右臂目标在同一参考坐标系下表达。

update_pose_with_delta / update_pose_with_self_delta / is_pose_reachable

• 用途：位姿工具函数（增量变换 + 工作空间检查）。
• 典型场景：在执行 moveL_robot 前做位姿预处理与边界过滤。
• 入参（核心）：
  • update_pose_with_delta(pose, center, rotate, ros2_format=False)：在参考系（如 arm_base）下加减平移、左乘旋转；支持 Pose / 4x4 / 6D / 7D。
  • update_pose_with_self_delta(pose, center, rotate, ros2_format=False)：在末端自身坐标系下施加增量（T_new = T_old @ T_delta），平移沿当前 TCP 轴，旋转绕当前 TCP 轴。
  • is_pose_reachable(rospose, x_range=None, y_range=None)：检查目标位姿 xy 是否在给定范围内。
• 出参：
  • 增量函数：np.ndarray（4×4）或 Pose（ros2_format=True）；
  • is_pose_reachable：bool。

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

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）：各部位到位等待的最大时间（秒）。
• 出参：
  • bool：True 表示动作完成；False 表示前置或执行失败。

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.src.robot_skills import moveJ_path_robot（或本地 from src.robot_skills import moveJ_path_robot）；未从包根 fa_robot_skills 导出。
• 入参：
  • 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）：轨迹下发后的初始等待时间（秒），用于等待轨迹发布完成。
• 出参：
  • bool：True 表示末路径点到位；False 表示前置或执行失败。
• 关键说明：
  • 执行前会自动切换到 MOVEJ 状态。
  • 到位判定以末路径点与当前 joint_states 的欧氏距离是否小于 robot.config.position_threshold 为准。

left_joint_positions_blocking(robot, positions, max_timeout=15.0) / right_joint_positions_blocking(robot, positions, max_timeout=15.0)

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

dual_arm_joint_positions_blocking(robot, left_positions, right_positions, max_timeout=15.0)

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

send_head_joint_positions_blocking(robot, positions, wait_until_complete=True, max_timeout=15.0)

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

send_body_joint_positions_blocking(robot, positions, wait_until_complete=True, max_timeout=15.0)

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

adjust_body_height(robot, up_rate=0.5)

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

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.src.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）：最大等待时间（秒）。
• 出参：
  • bool：True 表示到位；False 表示失败或超时。
• 关键说明：
  • 执行前会自动切换到 MOVEJ 状态，并通过 /body_joint_controller 设置 waist_lifting_duration。
  • 到位后自动切回 HOLD 状态；超时也会切回 HOLD。

4.4 灵巧手函数

灵巧手函数定义于 src/dexhand_skills.py，未从包根导出，需单独导入：

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

# pip install 后
from fa_robot_skills.src.dexhand_skills import send_dual_hand_joint_positions_blocking

send_left_hand_joint_positions(robot, positions) / send_right_hand_joint_positions(robot, positions)

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

send_dual_hand_joint_positions(robot, left_positions=None, right_positions=None)

• 用途：双手灵巧手关节目标下发（非阻塞），支持只下发单手。
• 入参：
  • left_positions / right_positions：传 None 表示跳过对应侧。
• 出参：
  • bool：True 表示所有请求下发的手均成功发布。

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(...)

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

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

4.5 夹爪函数

夹爪函数定义于 src/gripper_skills.py，未从包根导出，需单独导入：

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

# pip install 后
from fa_robot_skills.src.gripper_skills import gripper_control_blocking

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