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

These details have not been verified by PyPI

Project description

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`
未从包根导出的函数（见对照表）	优先改用包根函数；确需低层接口时：`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. 文件结构与功能说明

文件/目录	功能说明
`__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.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`	在 `pose_params` 中仅填 `left_pose_6d`（及可选 `left_pose_delta`）
`right_move_pose_stamped_blocking`	`src/robot_skills.py`	`moveL_robot`	在 `pose_params` 中仅填 `right_pose_6d`（及可选 `right_pose_delta`）
`send_dual_arm_stamped_blocking`	`src/robot_skills.py`	`moveL_robot`	双臂同步时在 `pose_params` 中同时填 `left_pose_6d` 与 `right_pose_6d`
`left_joint_positions_blocking`	`src/robot_skills.py`	`moveJ_robot`	`joint_params={"left_joints": [...]}`
`right_joint_positions_blocking`	`src/robot_skills.py`	`moveJ_robot`	`joint_params={"right_joints": [...]}`
`dual_arm_joint_positions_blocking`	`src/robot_skills.py`	`moveJ_robot`	`joint_params={"left_joints": [...], "right_joints": [...]}`
`send_head_joint_positions_blocking`	`src/robot_skills.py`	`moveJ_robot`	`joint_params={"head_joints": [...]}`
`send_body_joint_positions_blocking`	`src/robot_skills.py`	`moveJ_robot` 或 `adjust_body_height`	精确关节角用前者；按比例升降用后者
`send_left_hand_joint_positions`	`src/dexhand_skills.py`	`send_dual_hand_joint_positions`	`left_positions=..., right_positions=None`
`send_right_hand_joint_positions`	`src/dexhand_skills.py`	`send_dual_hand_joint_positions`	`left_positions=None, right_positions=...`
`send_left_hand_joint_positions_blocking`	`src/dexhand_skills.py`	`send_dual_hand_joint_positions_blocking`	`left_positions=..., right_positions=None`
`send_right_hand_joint_positions_blocking`	`src/dexhand_skills.py`	`send_dual_hand_joint_positions_blocking`	`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` 文件

python examples/test_robot_basic.py

可选参数：

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

使用流程

init_robot(...)：创建机器人接口实例
connect_robot(robot)：建立连接并切换到 HOLD 可用状态
get_joint(robot)：读取当前关节状态
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 确认，避免误触发运动。

使用流程

init_robot(...)：创建机器人接口实例
connect_robot(robot)：建立连接并切换到 HOLD 可用状态
moveJ_robot(...)：执行全身关节阻塞运动
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），避免误触发运动。

使用流程

init_robot(...)：创建机器人接口实例
connect_robot(robot)：建立连接并切换到 HOLD 可用状态
switch_to_ocs2_state(robot)：切换到 OCS2 状态
left_get_pose / right_get_pose：读取当前末端位姿（可选）
moveL_robot(...)：执行笛卡尔位姿运动（仅左或仅右 6D 为单臂；同时提供左右 6D 为双臂同步）
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：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 确认。

使用流程

init_robot(...)：创建机器人接口实例
connect_robot(robot)：建立连接并切换到 HOLD 可用状态
switch_to_ocs2_state(robot)：切换到 OCS2 状态
moveL_path_robot_by_pose_list(...) 或 moveL_path_robot_by_delta_list(...)：执行路径运动
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，避免误触发运动。

使用流程

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

关键说明

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

代码示例

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 确认，避免误触发运动。

使用流程

init_robot(...)：创建机器人接口实例
connect_robot(robot)：建立连接并切换到 HOLD 可用状态
send_dual_hand_joint_positions_blocking：下发目标并可阻塞等待到位（单手时另一侧传 None）
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 状态。

代码示例

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 确认）。

使用流程

init_robot(...)：创建机器人接口实例
connect_robot(robot)：建立连接并切换到 HOLD 可用状态
gripper_control_blocking(...)：批量下发目标并统一等待到位
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：打开时按距离阈值；关闭时距离达标或位置历史稳定即视为到位。

代码示例

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`	初始化机器人接口实例
`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`	左臂位姿下发并可阻塞等待〔未导出〕→ `moveL_robot`
`right_move_pose_stamped_blocking`	右臂位姿下发并可阻塞等待〔未导出〕→ `moveL_robot`
`send_dual_arm_stamped_blocking`	双臂 pose/path 下发并等待到位〔未导出〕→ `moveL_robot` / 路径函数
`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`	头部关节下发并可阻塞等待〔未导出〕→ `moveJ_robot`
`send_body_joint_positions_blocking`	身体关节下发并可阻塞等待〔未导出〕→ `moveJ_robot` / `adjust_body_height`
`left_joint_positions_blocking`	左臂关节阻塞运动〔未导出〕→ `moveJ_robot`
`right_joint_positions_blocking`	右臂关节阻塞运动〔未导出〕→ `moveJ_robot`
`dual_arm_joint_positions_blocking`	双臂关节阻塞运动〔未导出〕→ `moveJ_robot`
`adjust_body_height`	身体关节升降控制（按比例计算关节角）
`move_waist_lifting_relative_blocking`	腰部相对升降（米）并阻塞等待到位
`moveJ_robot`	全身关节阻塞运动入口
`moveJ_path_robot`	关节空间路径（waypoints）阻塞运动入口

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

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

4. skill 函数详细说明

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

4.1 基础函数

`init_robot(config=None, config_path=None)`

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

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。

`connect_robot(robot, hold_wait=0.1)`

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

from fa_robot_skills import connect_robot

ok = connect_robot(robot, hold_wait=0.1)

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

`disconnect_robot(robot)`

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

from fa_robot_skills import disconnect_robot

disconnect_robot(robot)

出参：
- 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 后等待时间（秒）。
入参示例：

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 行为一致。

`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 后等待时间（秒）。
入参示例：

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 表示连接或切换失败。

`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 后等待时间（秒）。
入参示例：

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 表示连接或切换失败。

`switch_to_hold_state(robot, hold_wait=0.1)`

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

from fa_robot_skills import switch_to_hold_state

ok = switch_to_hold_state(robot, hold_wait=0.1)

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

`get_joint(robot)`

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

from fa_robot_skills import get_joint

joint_state = get_joint(robot)

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

`left_get_pose(robot)` / `right_get_pose(robot)`

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

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 运动函数（笛卡尔空间运动）

`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）：是否开启可视化。
入参示例：

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 稳定联合到位检查。

`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：到位等待策略参数。
入参示例：

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。

`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 同上。
入参示例：

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。

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

〔未从包根导出〕 推荐改用 moveL_robot：左臂仅填 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 表示参数错误、下发失败或超时。

`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（单点双臂）或 moveL_path_robot_by_pose_list / moveL_path_robot_by_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 运动函数（关节空间运动）

`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）：各部位到位等待的最大时间（秒）。
入参示例：

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 表示前置或执行失败。

`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）：轨迹下发后的初始等待时间（秒），用于等待轨迹发布完成。
入参示例：

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 为准。

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

〔未从包根导出〕 推荐改用 moveJ_robot：
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 表示下发失败或超时。

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

〔未从包根导出〕 推荐改用 moveJ_robot：

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 表示下发失败或超时。

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

〔未从包根导出〕 推荐改用 moveJ_robot：joint_params={"head_joints": positions}。

用途：头部关节目标下发，并可阻塞等待。
典型场景：视野调整、传感器姿态归位。
入参：
- 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)`

〔未从包根导出〕 推荐改用：

精确关节角：moveJ_robot 的 joint_params={"body_joints": positions}

按比例升降：adjust_body_height 的 up_rate 参数

用途：身体关节目标下发，并可阻塞等待。
典型场景：机身高度调整、姿态准备。
入参：
- 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]。
入参示例：

from fa_robot_skills import adjust_body_height

adjust_body_height(robot, up_rate=0.5)  # 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 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）：最大等待时间（秒）。
入参示例：

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：

# 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

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

〔未从包根导出〕 推荐改用 send_dual_hand_joint_positions，另一侧传 None：

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 无效、未连接或发布器未创建。

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

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

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 表示所有请求下发的手均成功发布。

`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，另一侧传 None：
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 表示下发成功且（若等待）关节反馈在阈值内。

`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。
入参示例：

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：

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

# 本地开发（与 examples 一致）
from 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。
入参示例：

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。

Project details

These details have not been verified by PyPI

Release history Release notifications | RSS feed

0.2.9

Jun 9, 2026

0.2.8

Jun 9, 2026

This version

0.2.7

Jun 9, 2026

0.2.6

Jun 9, 2026

Download files

Download the file for your platform. If you're not sure which to choose, learn more about installing packages.

Source Distribution

fa_robot_skills-0.2.7.tar.gz (127.3 kB view details)

Uploaded Jun 9, 2026 Source

Built Distribution

If you're not sure about the file name format, learn more about wheel file names.

The dropdown lists show the available interpreters, ABIs, and platforms. Enable javascript to be able to filter the list of wheel files.

fa_robot_skills-0.2.7-py3-none-any.whl (141.0 kB view details)

Uploaded Jun 9, 2026 Python 3

File details

Details for the file fa_robot_skills-0.2.7.tar.gz.

File metadata

Download URL: fa_robot_skills-0.2.7.tar.gz
Upload date: Jun 9, 2026
Size: 127.3 kB
Tags: Source
Uploaded using Trusted Publishing? No
Uploaded via: twine/6.2.0 CPython/3.12.3

File hashes

Hashes for fa_robot_skills-0.2.7.tar.gz
Algorithm	Hash digest
SHA256	`3a45e5b72cdec7dd5608f77000c3a7df081561ea027a8d80aea918642e78c09c`
MD5	`67e27ef396b18bf771aa7418880562f7`
BLAKE2b-256	`18cf3b905948c918fc7e198db76d934f4c693bdadb866d6577ca6a1065287e38`

See more details on using hashes here.

File details

Details for the file fa_robot_skills-0.2.7-py3-none-any.whl.

File metadata

Download URL: fa_robot_skills-0.2.7-py3-none-any.whl
Upload date: Jun 9, 2026
Size: 141.0 kB
Tags: Python 3
Uploaded using Trusted Publishing? No
Uploaded via: twine/6.2.0 CPython/3.12.3

File hashes

Hashes for fa_robot_skills-0.2.7-py3-none-any.whl
Algorithm	Hash digest
SHA256	`b87b62b5ab7a28d09074c054966714bb28064c86f09c6225494dc8d67260a236`
MD5	`a83df77d6b951d2fdbba4f6d61d975de`
BLAKE2b-256	`819321d60cbc983ccec25a12c8599a9911f758945967855e5cba1c076f3127f8`

See more details on using hashes here.

fa-robot-skills 0.2.7

Navigation

Verified details

Maintainers

Unverified details

Meta

Classifiers

Project description

fa_robot_skills 使用说明

目录

0. 安装

从 PyPI 安装

不 pip install（源码直接使用）

查看版本

本地开发与发布

导入方式

1. 文件结构与功能说明

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

2. 快速使用

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

examples 目录下的 test_robot_basic.py 文件

使用流程

代码示例

2.2 使用 MoveJ（关节空间）

examples 目录下的 test_robot_movej.py 文件

使用流程

代码示例

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

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

使用流程

代码示例（单臂 MoveL）

代码示例（双臂同步 MoveL）

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

examples 目录下的 test_robot_path_pose.py 文件

使用流程

代码示例（pose_list 风格）

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

examples 目录下的 test_robot_path_joint.py 文件

使用流程

关键说明

代码示例

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

examples 目录下的 test_dexhand_skills.py 文件

使用流程

关键说明

代码示例

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

examples 目录下的 test_gripper_skills.py 文件

使用流程

关键说明

代码示例

3. skill 函数简略清单

4. skill 函数详细说明

4.1 基础函数

init_robot(config=None, config_path=None)

connect_robot(robot, hold_wait=0.1)

disconnect_robot(robot)

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

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

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

switch_to_hold_state(robot, hold_wait=0.1)

get_joint(robot)

left_get_pose(robot) / right_get_pose(robot)

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)

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)

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)

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

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)

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

moveJ_robot(robot, joint_params=None, hold_wait=0.1, movej_wait=0.1, max_timeout=15.0)

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)

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

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

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

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

adjust_body_height(robot, up_rate=0.5)

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)

4.4 灵巧手函数

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

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

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

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

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

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

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

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

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

`init_robot(config=None, config_path=None)`

`connect_robot(robot, hold_wait=0.1)`

`disconnect_robot(robot)`

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

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

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

`switch_to_hold_state(robot, hold_wait=0.1)`

`get_joint(robot)`

`left_get_pose(robot)` / `right_get_pose(robot)`

`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)`

`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)`

`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)`

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

`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)`

`moveJ_robot(robot, joint_params=None, hold_wait=0.1, movej_wait=0.1, max_timeout=15.0)`

`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)`

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

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

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

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

`adjust_body_height(robot, up_rate=0.5)`

`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)`

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

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

`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(robot, left_positions=None, right_positions=None, wait_until_complete=True, max_timeout=15.0, position_threshold=None)`

`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)`