mirror of
https://github.com/deepmodeling/Uni-Lab-OS
synced 2026-03-24 08:54:17 +00:00
1316 lines
59 KiB
Markdown
1316 lines
59 KiB
Markdown
# Uni-Lab-OS MoveIt2 集成架构总结
|
||
|
||
## 概览
|
||
|
||
Uni-Lab-OS 通过三个核心文件实现了对 MoveIt2（ROS 2 运动规划框架）的深度集成，形成了从**底层运动规划接口** → **业务逻辑封装** → **场景构建与启动**的完整链路。
|
||
|
||
```
|
||
┌──────────────────────────────────────────────────────────────────────┐
|
||
│ resource_visalization.py │
|
||
│ 场景构建层：URDF/SRDF 生成、MoveIt2 节点启动 │
|
||
│ ros2_control_node / move_group / robot_state_publisher / rviz2 │
|
||
└────────────────────────────┬─────────────────────────────────────────┘
|
||
│ 提供 MoveIt2 运行环境（Planning Scene、控制器）
|
||
▼
|
||
┌──────────────────────────────────────────────────────────────────────┐
|
||
│ moveit_interface.py │
|
||
│ 业务逻辑层：pick_and_place、set_position、set_status │
|
||
│ 管理多个 MoveGroup，提供 FK/IK 计算、资源 TF 更新 │
|
||
└────────────────────────────┬─────────────────────────────────────────┘
|
||
│ 调用 MoveIt2 Python API
|
||
▼
|
||
┌──────────────────────────────────────────────────────────────────────┐
|
||
│ moveit2.py │
|
||
│ 底层接口层：MoveIt2 ROS 2 Python 客户端实现 │
|
||
│ Action Client / Service Client / Collision Scene / 轨迹执行 │
|
||
└──────────────────────────────────────────────────────────────────────┘
|
||
```
|
||
|
||
---
|
||
|
||
## 1. `moveit2.py` — MoveIt2 底层 Python 接口
|
||
|
||
**路径**: `unilabos/devices/ros_dev/moveit2.py`
|
||
**行数**: ~2443 行
|
||
**角色**: 对 MoveIt2 ROS 2 接口的完整 Python 封装，是整个运动控制的基础。
|
||
|
||
### 1.1 核心类：`MoveIt2`
|
||
|
||
对 MoveIt2 框架的 ROS 2 通信协议进行了全面的 Python 封装，提供了规划、执行、运动学计算和碰撞管理的统一接口。
|
||
|
||
#### 1.1.1 ROS 2 通信拓扑
|
||
|
||
`MoveIt2` 类在初始化时创建了丰富的 ROS 2 通信端点：
|
||
|
||
| 类型 | 名称 | 用途 |
|
||
|------|------|------|
|
||
| **Subscriber** | `/joint_states` | 实时获取关节状态（BEST_EFFORT QoS） |
|
||
| **Action Client** | `/move_action` | 通过 MoveGroup Action 一体化规划+执行 |
|
||
| **Action Client** | `/execute_trajectory` | 独立的轨迹执行 |
|
||
| **Service Client** | `/plan_kinematic_path` | 关节空间/笛卡尔空间运动规划 |
|
||
| **Service Client** | `/compute_cartesian_path` | 笛卡尔路径规划（直线插补） |
|
||
| **Service Client** | `/compute_fk` | 正运动学计算 |
|
||
| **Service Client** | `/compute_ik` | 逆运动学计算 |
|
||
| **Service Client** | `/get_planning_scene` | 获取当前规划场景 |
|
||
| **Service Client** | `/apply_planning_scene` | 应用修改后的规划场景 |
|
||
| **Publisher** | `/collision_object` | 发布碰撞物体 |
|
||
| **Publisher** | `/attached_collision_object` | 发布附着碰撞物体 |
|
||
| **Publisher** | `/trajectory_execution_event` | 发送轨迹取消指令 |
|
||
|
||
#### 1.1.2 运动规划与执行
|
||
|
||
提供两种执行模式：
|
||
|
||
- **MoveGroup Action 模式** (`use_move_group_action=True`)：规划和执行合并在一个 Action 调用中完成，由 MoveIt2 内部管理整个流程。
|
||
- **Plan + Execute 模式**：先通过 Service 调用进行路径规划（`/plan_kinematic_path` 或 `/compute_cartesian_path`），再通过 ExecuteTrajectory Action 执行。
|
||
|
||
关键方法：
|
||
|
||
| 方法 | 说明 |
|
||
|------|------|
|
||
| `move_to_pose()` | 移动到目标位姿（位置 + 四元数姿态），支持笛卡尔直线规划 |
|
||
| `move_to_configuration()` | 移动到目标关节配置 |
|
||
| `plan()` / `plan_async()` | 异步路径规划，返回 `JointTrajectory` |
|
||
| `execute()` | 执行规划好的轨迹 |
|
||
| `wait_until_executed()` | 阻塞等待运动完成，返回成功/失败 |
|
||
| `cancel_execution()` | 取消当前运动 |
|
||
|
||
#### 1.1.3 目标约束系统
|
||
|
||
支持多层次的目标设定：
|
||
|
||
- **位置约束** (`set_position_goal`): 以球形区域定义目标位置的容差
|
||
- **姿态约束** (`set_orientation_goal`): 支持 Euler 角和旋转向量两种参数化方式
|
||
- **关节约束** (`set_joint_goal`): 直接指定目标关节角度
|
||
- **复合约束** (`set_pose_goal`): 位置 + 姿态的组合
|
||
- **路径约束** (`set_path_joint_constraint`, `set_path_position_constraint`, `set_path_orientation_constraint`): 对整条运动路径施加约束
|
||
- **多目标组** (`create_new_goal_constraint`): 支持同时设置多组目标约束
|
||
|
||
#### 1.1.4 运动学服务
|
||
|
||
- **正运动学 (FK)**: `compute_fk()` / `compute_fk_async()` — 给定关节角求末端位姿
|
||
- **逆运动学 (IK)**: `compute_ik()` / `compute_ik_async()` — 给定目标位姿求关节角，支持传入约束和避碰选项
|
||
|
||
#### 1.1.5 碰撞场景管理
|
||
|
||
提供完整的 Planning Scene 管理接口：
|
||
|
||
| 方法 | 说明 |
|
||
|------|------|
|
||
| `add_collision_box()` | 添加长方体碰撞体 |
|
||
| `add_collision_sphere()` | 添加球形碰撞体 |
|
||
| `add_collision_cylinder()` | 添加圆柱碰撞体 |
|
||
| `add_collision_cone()` | 添加锥形碰撞体 |
|
||
| `add_collision_mesh()` | 添加三角网格碰撞体（依赖 trimesh） |
|
||
| `move_collision()` | 移动已有碰撞体 |
|
||
| `remove_collision_object()` | 移除碰撞体 |
|
||
| `attach_collision_object()` | 将碰撞体附着到机器人 link 上 |
|
||
| `detach_collision_object()` | 从机器人上分离碰撞体 |
|
||
| `allow_collisions()` | 修改 Allowed Collision Matrix，允许/禁止特定碰撞 |
|
||
| `clear_all_collision_objects()` | 清空所有碰撞物体 |
|
||
|
||
#### 1.1.6 状态管理
|
||
|
||
通过 `MoveIt2State` 枚举和线程锁实现并发安全的状态管理：
|
||
|
||
- `IDLE`: 空闲
|
||
- `REQUESTING`: 已发送运动请求，等待接受
|
||
- `EXECUTING`: 正在执行轨迹
|
||
|
||
配合 `ignore_new_calls_while_executing` 标志，防止运动冲突。
|
||
|
||
#### 1.1.7 辅助工具函数
|
||
|
||
文件末尾提供模块级工具函数：
|
||
|
||
- `init_joint_state()`: 构造 `JointState` 消息
|
||
- `init_execute_trajectory_goal()`: 构造 `ExecuteTrajectory.Goal` 消息
|
||
- `init_dummy_joint_trajectory_from_state()`: 构造用于重置控制器的虚拟轨迹
|
||
|
||
---
|
||
|
||
## 2. `moveit_interface.py` — 业务逻辑封装层
|
||
|
||
**路径**: `unilabos/devices/ros_dev/moveit_interface.py`
|
||
**行数**: 385 行
|
||
**角色**: 将 `MoveIt2` 底层接口封装为实验室场景中可用的高级操作（pick/place、状态切换、资源管理）。
|
||
|
||
### 2.1 核心类：`MoveitInterface`
|
||
|
||
`MoveitInterface` 是连接实验室业务逻辑与 MoveIt2 运动规划的桥梁。它不直接继承 `MoveIt2`，而是通过**组合**的方式管理多个 `MoveIt2` 实例（每个 MoveGroup 一个），并在其上构建抓取放置、状态切换等实验室级操作。
|
||
|
||
---
|
||
|
||
#### 2.1.1 类属性与实例属性
|
||
|
||
```python
|
||
class MoveitInterface:
|
||
_ros_node: BaseROS2DeviceNode # ROS 2 节点引用（post_init 注入）
|
||
tf_buffer: Buffer # TF2 坐标变换缓冲区
|
||
tf_listener: TransformListener # TF2 变换监听器
|
||
```
|
||
|
||
**构造函数参数：**
|
||
|
||
| 参数 | 类型 | 说明 |
|
||
|------|------|------|
|
||
| `moveit_type` | `str` | 设备类型标识，用于定位 `device_mesh/devices/{moveit_type}/config/move_group.json` 配置文件 |
|
||
| `joint_poses` | `dict` | 预定义关节位姿字典，结构为 `{move_group: {status_name: [joint_values...]}}` |
|
||
| `rotation` | `optional` | 设备旋转参数（预留） |
|
||
| `device_config` | `optional` | 设备自定义配置 |
|
||
|
||
**实例属性：**
|
||
|
||
| 属性 | 类型 | 初始值 | 说明 |
|
||
|------|------|--------|------|
|
||
| `data_config` | `dict` | 从 JSON 加载 | Move Group 配置，结构为 `{group_name: {base_link_name, end_effector_name, joint_names}}` |
|
||
| `arm_move_flag` | `bool` | `False` | 机械臂运动标志（预留） |
|
||
| `move_option` | `list` | `["pick", "place", "side_pick", "side_place"]` | 支持的抓取/放置动作类型 |
|
||
| `joint_poses` | `dict` | 构造函数传入 | 预定义关节位姿查找表 |
|
||
| `cartesian_flag` | `bool` | `False` | 当前是否使用笛卡尔直线规划（在 `pick_and_place` 执行过程中动态切换） |
|
||
| `mesh_group` | `list` | `["reactor", "sample", "beaker"]` | 碰撞网格分组类别 |
|
||
| `moveit2` | `dict` | `{}` | MoveIt2 实例字典，键为 Move Group 名称 |
|
||
| `resource_action` | `str/None` | `None` | 发现的 `tf_update` Action Server 名称 |
|
||
| `resource_client` | `ActionClient/None` | `None` | 用于发送资源 TF 更新请求的 Action Client |
|
||
| `resource_action_ok` | `bool` | `False` | `tf_update` Action Server 是否就绪 |
|
||
|
||
---
|
||
|
||
#### 2.1.2 初始化流程：`__init__` + `post_init`
|
||
|
||
采用**两阶段初始化**设计：
|
||
|
||
**阶段一：`__init__`（纯数据初始化，不依赖 ROS 2）**
|
||
|
||
```
|
||
__init__(moveit_type, joint_poses, ...)
|
||
│
|
||
├── 加载 move_group.json 配置文件
|
||
│ 路径: device_mesh/devices/{moveit_type}/config/move_group.json
|
||
│ 内容: { "arm": { "base_link_name": "base_link",
|
||
│ "end_effector_name": "tool0",
|
||
│ "joint_names": ["joint1", "joint2", ...] } }
|
||
│
|
||
├── 初始化状态标志 (arm_move_flag, cartesian_flag, resource_action_ok)
|
||
└── 记录 joint_poses 预定义位姿
|
||
```
|
||
|
||
**阶段二：`post_init(ros_node)`（ROS 2 依赖初始化）**
|
||
|
||
```
|
||
post_init(ros_node)
|
||
│
|
||
├── 保存 ROS 节点引用 → self._ros_node
|
||
│
|
||
├── 创建 TF2 基础设施
|
||
│ ├── Buffer() → self.tf_buffer
|
||
│ └── TransformListener(buffer, node) → self.tf_listener
|
||
│
|
||
├── 遍历 data_config 中的每个 Move Group
|
||
│ │
|
||
│ │ 对于每个 {move_group, config}:
|
||
│ │
|
||
│ ├── 生成带设备前缀的名称
|
||
│ │ ├── base_link_name = "{device_id}_{config.base_link_name}"
|
||
│ │ ├── end_effector = "{device_id}_{config.end_effector_name}"
|
||
│ │ └── joint_names = ["{device_id}_{name}" for name in config.joint_names]
|
||
│ │
|
||
│ └── 创建 MoveIt2 实例
|
||
│ self.moveit2[move_group] = MoveIt2(
|
||
│ node=ros_node,
|
||
│ joint_names=...,
|
||
│ base_link_name=...,
|
||
│ end_effector_name=...,
|
||
│ group_name="{device_id}_{move_group}", ← MoveIt2 Planning Group 名
|
||
│ callback_group=ros_node.callback_group, ← 共享回调组
|
||
│ use_move_group_action=True, ← 使用 MoveGroup Action 模式
|
||
│ ignore_new_calls_while_executing=True ← 防止运动冲突
|
||
│ )
|
||
│ .allowed_planning_time = 3.0 ← 规划超时 3 秒
|
||
│
|
||
└── 创建定时器 (1秒间隔)
|
||
→ wait_for_resource_action() ← 异步等待 tf_update Action Server
|
||
```
|
||
|
||
**设备名前缀机制**：所有关节名、link 名、group 名都加上 `{device_id}_` 前缀。这是多设备共存的关键——当多台机械臂在同一 ROS 2 网络中运行时，前缀保证名称唯一，MoveIt2 能正确区分不同设备的规划组。
|
||
|
||
---
|
||
|
||
#### 2.1.3 资源 TF 更新服务发现
|
||
|
||
`MoveitInterface` 需要在运行时动态更新实验室资源（如烧杯、样品瓶）的 TF 父链接——当机械臂抓取物体时，物体的 TF 从 `world` 切换到末端执行器；放置时切换回 `world`。
|
||
|
||
```
|
||
wait_for_resource_action() [定时器回调，1秒间隔]
|
||
│
|
||
├── 若 resource_action_ok 已为 True → 直接返回
|
||
│
|
||
├── 轮询 check_tf_update_actions()
|
||
│ │
|
||
│ ├── 遍历所有 ROS 2 Topic
|
||
│ ├── 查找 topic_type == "action_msgs/msg/GoalStatusArray" 的 Topic
|
||
│ ├── 从 Topic 名称中提取 Action 名（去掉 "/_action/status" 后缀）
|
||
│ └── 检查最后一段是否为 "tf_update" → 返回 Action 名称
|
||
│
|
||
├── 创建 ActionClient(SendCmd, action_name)
|
||
├── 等待 Action Server 就绪 (timeout=5s，循环等待)
|
||
└── 设置 resource_action_ok = True
|
||
```
|
||
|
||
**为什么用 Topic 发现而非硬编码？**
|
||
`tf_update` Action Server 由 `resource_mesh_manager` 节点提供，其命名空间可能随部署配置变化。通过 Topic 自动发现机制，`MoveitInterface` 能自适应不同的部署环境。
|
||
|
||
---
|
||
|
||
#### 2.1.4 底层运动方法：`moveit_task`
|
||
|
||
```python
|
||
def moveit_task(self, move_group, position, quaternion,
|
||
speed=1, retry=10, cartesian=False,
|
||
target_link=None, offsets=[0,0,0])
|
||
```
|
||
|
||
这是笛卡尔空间运动的核心封装，所有高级方法最终都通过它调用 `MoveIt2.move_to_pose()`。
|
||
|
||
**执行流程：**
|
||
|
||
```
|
||
moveit_task(move_group, position, quaternion, speed, retry, ...)
|
||
│
|
||
├── 速度限幅: speed_ = clamp(speed, 0.1, 1.0)
|
||
├── 设置 MoveIt2 速度:
|
||
│ ├── moveit2[group].max_velocity = speed_
|
||
│ └── moveit2[group].max_acceleration = speed_
|
||
│
|
||
├── 计算最终位置: pose_result = position + offsets (逐元素相加)
|
||
│
|
||
└── 重试循环 (最多 retry+1 次):
|
||
│
|
||
├── moveit2[group].move_to_pose(
|
||
│ target_link=target_link, ← 可指定非默认末端 link
|
||
│ position=pose_result, ← 目标 [x, y, z]
|
||
│ quat_xyzw=quaternion, ← 目标 [qx, qy, qz, qw]
|
||
│ cartesian=cartesian, ← 笛卡尔直线 or 自由空间
|
||
│ cartesian_max_step=0.01, ← 笛卡尔插补步长 1cm
|
||
│ weight_position=1.0 ← 位置约束权重
|
||
│ )
|
||
│
|
||
├── re_ = moveit2[group].wait_until_executed() ← 阻塞等待
|
||
│
|
||
└── 若 re_ 为 True → 返回成功; 否则 retry -= 1 继续
|
||
```
|
||
|
||
**参数说明：**
|
||
|
||
| 参数 | 说明 | MoveIt2 对应 |
|
||
|------|------|-------------|
|
||
| `position` | 目标位置 `[x, y, z]`（米） | `move_to_pose(position=...)` |
|
||
| `quaternion` | 目标姿态四元数 `[x, y, z, w]` | `move_to_pose(quat_xyzw=...)` |
|
||
| `speed` | 速度因子 0.1~1.0，同时控制速度和加速度 | `max_velocity` + `max_acceleration` |
|
||
| `retry` | 规划失败时的最大重试次数 | N/A（应用层重试） |
|
||
| `cartesian` | 是否使用笛卡尔直线规划 | `move_to_pose(cartesian=...)` |
|
||
| `target_link` | 目标 link（默认末端执行器） | `move_to_pose(target_link=...)` |
|
||
| `offsets` | 位置偏移量 `[dx, dy, dz]` | 叠加到 `position` |
|
||
|
||
---
|
||
|
||
#### 2.1.5 底层运动方法：`moveit_joint_task`
|
||
|
||
```python
|
||
def moveit_joint_task(self, move_group, joint_positions,
|
||
joint_names=None, speed=1, retry=10)
|
||
```
|
||
|
||
关节空间运动的核心封装，调用 `MoveIt2.move_to_configuration()`。
|
||
|
||
**执行流程：**
|
||
|
||
```
|
||
moveit_joint_task(move_group, joint_positions, joint_names, speed, retry)
|
||
│
|
||
├── 关节角度转 float: joint_positions_ = [float(x) for x in joint_positions]
|
||
├── 速度限幅: speed_ = clamp(speed, 0.1, 1.0)
|
||
├── 设置 MoveIt2 速度
|
||
│
|
||
└── 重试循环:
|
||
│
|
||
├── moveit2[group].move_to_configuration(
|
||
│ joint_positions=joint_positions_,
|
||
│ joint_names=joint_names ← None 时使用 MoveIt2 默认关节名
|
||
│ )
|
||
│
|
||
├── re_ = moveit2[group].wait_until_executed()
|
||
│
|
||
├── 打印 FK 结果 (调试用):
|
||
│ compute_fk(joint_positions) → 显示对应的末端位姿
|
||
│
|
||
└── 若成功 → 返回; 否则 retry -= 1
|
||
```
|
||
|
||
**与 `moveit_task` 的区别：**
|
||
|
||
- `moveit_task`：目标是笛卡尔空间位姿（位置 + 姿态），MoveIt2 自动进行 IK 求解
|
||
- `moveit_joint_task`：目标直接是关节角度，无需 IK 计算，确定性更高
|
||
- 每次循环后调用 `compute_fk` 输出当前末端位姿，便于调试
|
||
|
||
---
|
||
|
||
#### 2.1.6 资源 TF 管理：`resource_manager`
|
||
|
||
```python
|
||
def resource_manager(self, resource, parent_link)
|
||
```
|
||
|
||
通过 `SendCmd` Action 向 `tf_update` 服务发送 TF 父链接更新请求。
|
||
|
||
```
|
||
resource_manager("beaker_1", "tool0")
|
||
│
|
||
├── 构造 SendCmd.Goal:
|
||
│ goal.command = '{"beaker_1": "tool0"}' ← JSON 格式: {资源名: 新父link}
|
||
│
|
||
└── resource_client.send_goal(goal) ← 异步发送，不等待结果
|
||
```
|
||
|
||
**在 pick/place 流程中的角色：**
|
||
|
||
- **pick 时**：`resource_manager(resource, end_effector_name)` — 资源跟随末端执行器
|
||
- **pick 且有 target 时**：`resource_manager(resource, target)` — 资源挂到指定 link
|
||
- **place 时**：`resource_manager(resource, "world")` — 资源释放到世界坐标系
|
||
|
||
---
|
||
|
||
#### 2.1.7 直接位姿控制：`set_position`
|
||
|
||
```python
|
||
def set_position(self, command: str)
|
||
```
|
||
|
||
最简单的运动接口，解析 JSON 指令后直接委托给 `moveit_task`。
|
||
|
||
**JSON 指令格式：**
|
||
|
||
```json
|
||
{
|
||
"position": [0.3, 0.0, 0.5],
|
||
"quaternion": [0.0, 0.0, 0.0, 1.0],
|
||
"move_group": "arm",
|
||
"speed": 0.5,
|
||
"retry": 10
|
||
}
|
||
```
|
||
|
||
**调用链：**
|
||
|
||
```
|
||
set_position(command_json)
|
||
│
|
||
├── JSON 解析 (替换单引号为双引号)
|
||
└── moveit_task(**cmd_dict)
|
||
└── MoveIt2.move_to_pose(...)
|
||
```
|
||
|
||
---
|
||
|
||
#### 2.1.8 预定义状态切换：`set_status`
|
||
|
||
```python
|
||
def set_status(self, command: str)
|
||
```
|
||
|
||
将机械臂移动到预定义的关节配置（如 home 位、准备位等），关节角度从 `self.joint_poses` 查找表中获取。
|
||
|
||
**JSON 指令格式：**
|
||
|
||
```json
|
||
{
|
||
"status": "home",
|
||
"move_group": "arm",
|
||
"speed": 0.8,
|
||
"retry": 5
|
||
}
|
||
```
|
||
|
||
**调用链：**
|
||
|
||
```
|
||
set_status(command_json)
|
||
│
|
||
├── JSON 解析
|
||
├── 查找预定义关节角: joint_poses[move_group][status]
|
||
│ 例: joint_poses["arm"]["home"] → [0.0, -1.57, 1.57, 0.0, 0.0, 0.0]
|
||
│
|
||
└── moveit_joint_task(move_group, joint_positions, speed, retry)
|
||
└── MoveIt2.move_to_configuration(...)
|
||
```
|
||
|
||
**`joint_poses` 查找表结构：**
|
||
|
||
```python
|
||
{
|
||
"arm": {
|
||
"home": [0.0, -1.57, 1.57, 0.0, 0.0, 0.0],
|
||
"ready": [0.0, -0.78, 1.57, 0.0, 0.78, 0.0],
|
||
"pick_A1": [0.5, -1.2, 1.0, 0.0, 0.5, 0.3],
|
||
...
|
||
},
|
||
"gripper": {
|
||
"open": [0.04],
|
||
"close": [0.0],
|
||
...
|
||
}
|
||
}
|
||
```
|
||
|
||
---
|
||
|
||
#### 2.1.9 核心方法详解：`pick_and_place`
|
||
|
||
```python
|
||
def pick_and_place(self, command: str)
|
||
```
|
||
|
||
这是 `MoveitInterface` 最复杂的方法，实现了完整的抓取-放置工作流。它动态构建一个**有序函数列表** (`function_list`)，然后顺序执行。
|
||
|
||
**JSON 指令格式（完整参数）：**
|
||
|
||
```json
|
||
{
|
||
"option": "pick", // *必须: pick/place/side_pick/side_place
|
||
"move_group": "arm", // *必须: MoveIt2 规划组名
|
||
"status": "pick_station_A", // *必须: 在 joint_poses 中的目标状态名
|
||
"resource": "beaker_1", // 要操作的资源名称
|
||
"target": "custom_link", // pick 时资源附着的目标 link (默认末端执行器)
|
||
"lift_height": 0.05, // 抬升高度 (米)
|
||
"x_distance": 0.1, // X 方向水平移动距离 (米)
|
||
"y_distance": 0.0, // Y 方向水平移动距离 (米)
|
||
"speed": 0.5, // 运动速度因子 (0.1~1.0)
|
||
"retry": 10, // 规划失败重试次数
|
||
"constraints": [0, 0, 0, 0.5, 0, 0] // 各关节约束容差 (>0 时生效)
|
||
}
|
||
```
|
||
|
||
##### 阶段 1：指令解析与动作类型判定
|
||
|
||
```
|
||
pick_and_place(command_json)
|
||
│
|
||
├── JSON 解析
|
||
├── 动作类型判定:
|
||
│ move_option = ["pick", "place", "side_pick", "side_place"]
|
||
│ 0 1 2 3
|
||
│ option_index = move_option.index(cmd["option"])
|
||
│ place_flag = option_index % 2 ← 0=pick类, 1=place类
|
||
│
|
||
├── 提取运动参数:
|
||
│ config = {speed, retry, move_group} ← 从 cmd_dict 中按需提取
|
||
│
|
||
└── 获取目标关节位姿:
|
||
joint_positions_ = joint_poses[move_group][status]
|
||
```
|
||
|
||
##### 阶段 2：构建资源 TF 更新动作
|
||
|
||
```
|
||
根据 place_flag 决定资源 TF 操作:
|
||
|
||
if pick 类 (place_flag == 0):
|
||
if "target" 已指定:
|
||
function_list += [resource_manager(resource, target)] ← 挂到自定义 link
|
||
else:
|
||
function_list += [resource_manager(resource, end_effector)] ← 挂到末端执行器
|
||
|
||
if place 类 (place_flag == 1):
|
||
function_list += [resource_manager(resource, "world")] ← 释放到世界坐标
|
||
```
|
||
|
||
##### 阶段 3：构建关节约束
|
||
|
||
```
|
||
if "constraints" 存在于指令中:
|
||
for i, tolerance in enumerate(constraints):
|
||
if tolerance > 0:
|
||
JointConstraint(
|
||
joint_name = moveit2[group].joint_names[i],
|
||
position = joint_positions_[i], ← 约束中心 = 目标关节角
|
||
tolerance_above = tolerance,
|
||
tolerance_below = tolerance,
|
||
weight = 1.0
|
||
)
|
||
```
|
||
|
||
约束的作用：限制 IK 求解的搜索空间，确保机械臂在抬升/移动过程中保持特定关节（如肘关节）在安全范围内。
|
||
|
||
##### 阶段 4A：有 `lift_height` 的完整流程
|
||
|
||
这是最复杂的场景，涉及 FK/IK 计算和多段运动拼接：
|
||
|
||
```
|
||
if "lift_height" 存在:
|
||
│
|
||
├── Step 1: FK 计算 → 获取目标关节配置对应的末端位姿
|
||
│ retval = compute_fk(joint_positions_) ← 可能需要重试
|
||
│ pose = [retval.position.x, .y, .z]
|
||
│ quaternion = [retval.orientation.x, .y, .z, .w]
|
||
│
|
||
├── Step 2: 构建"下降到目标点"动作
|
||
│ function_list = [moveit_task(position=pose, ...)] + function_list
|
||
│ 注：此时 function_list 已包含 resource_manager，它被插入到中间
|
||
│
|
||
├── Step 3: 构建"从目标点抬升"动作
|
||
│ pose[2] += lift_height ← Z 轴抬升
|
||
│ function_list += [moveit_task(position=pose_lifted, ...)]
|
||
│
|
||
├── Step 4 (可选): 水平移动
|
||
│ if "x_distance":
|
||
│ deep_pose = copy(pose_lifted)
|
||
│ deep_pose[0] += x_distance
|
||
│ function_list = [moveit_task(pose_lifted)] + function_list
|
||
│ function_list += [moveit_task(deep_pose)]
|
||
│ elif "y_distance":
|
||
│ 类似处理 Y 方向
|
||
│
|
||
├── Step 5: IK 预计算 → 将末端位姿转换为安全的关节配置
|
||
│ retval_ik = compute_ik(
|
||
│ position = end_pose, ← 最终抬升/移动后的位姿
|
||
│ quat_xyzw = quaternion,
|
||
│ constraints = Constraints(joint_constraints=constraints)
|
||
│ )
|
||
│ position_ = 从 IK 结果提取各关节角度
|
||
│
|
||
└── Step 6: 构建"关节空间移动到起始位"动作
|
||
function_list = [moveit_joint_task(position_)] + function_list
|
||
```
|
||
|
||
##### 阶段 4B：无 `lift_height` 的简单流程
|
||
|
||
```
|
||
else (无 lift_height):
|
||
│
|
||
└── 直接关节运动到目标位姿
|
||
function_list = [moveit_joint_task(joint_positions_)] + function_list
|
||
```
|
||
|
||
##### 阶段 5：顺序执行动作列表
|
||
|
||
```
|
||
for i, func in enumerate(function_list):
|
||
│
|
||
├── 设置规划模式:
|
||
│ i == 0: cartesian_flag = False ← 第一步用自由空间规划（大范围移动）
|
||
│ i > 0: cartesian_flag = True ← 后续用笛卡尔直线规划（精确控制）
|
||
│
|
||
├── result = func() ← 执行动作
|
||
│
|
||
└── if not result:
|
||
return failure ← 任一步骤失败即中止
|
||
```
|
||
|
||
##### 完整 pick 流程示例（含 lift_height + x_distance）
|
||
|
||
假设指令为：pick beaker_1 from station_A, lift 5cm, move 10cm in X
|
||
|
||
```
|
||
最终 function_list 执行顺序:
|
||
┌─────────────────────────────────────────────────────────────────────┐
|
||
│ Step 0: moveit_joint_task → IK 求解的关节角 │
|
||
│ [cartesian=False, 自由空间规划] │
|
||
│ 机械臂从当前位置移动到抬升位 │
|
||
├─────────────────────────────────────────────────────────────────────┤
|
||
│ Step 1: moveit_task → 水平移动后的抬升位 (pose_lifted) │
|
||
│ [cartesian=True, 笛卡尔直线] │
|
||
│ 对齐到 station_A 正上方 │
|
||
├─────────────────────────────────────────────────────────────────────┤
|
||
│ Step 2: moveit_task → station_A 目标位姿 (FK 计算的位置) │
|
||
│ [cartesian=True, 笛卡尔直线] │
|
||
│ 末端执行器下降到目标点 │
|
||
├─────────────────────────────────────────────────────────────────────┤
|
||
│ Step 3: resource_manager("beaker_1", end_effector) │
|
||
│ 资源 TF 附着到末端执行器 │
|
||
├─────────────────────────────────────────────────────────────────────┤
|
||
│ Step 4: moveit_task → 抬升位 (z + lift_height) │
|
||
│ [cartesian=True, 笛卡尔直线] │
|
||
│ 抓取后垂直抬升 │
|
||
├─────────────────────────────────────────────────────────────────────┤
|
||
│ Step 5: moveit_task → 水平偏移位 (x + x_distance) │
|
||
│ [cartesian=True, 笛卡尔直线] │
|
||
│ 抬升后水平移开，避免碰撞 │
|
||
└─────────────────────────────────────────────────────────────────────┘
|
||
```
|
||
|
||
##### 完整 place 流程（同结构，反向操作）
|
||
|
||
与 pick 相同的运动轨迹，但 `resource_manager` 调用变为 `resource_manager(resource, "world")`——在 Step 3 处将资源从末端执行器释放到世界坐标系。
|
||
|
||
---
|
||
|
||
#### 2.1.10 MoveIt2 API 调用汇总
|
||
|
||
`MoveitInterface` 使用的 `MoveIt2` 接口及其调用位置：
|
||
|
||
| MoveIt2 方法 | 调用位置 | 用途 |
|
||
|-------------|---------|------|
|
||
| `MoveIt2(...)` 构造 | `post_init` L51-60 | 为每个 MoveGroup 创建实例 |
|
||
| `.allowed_planning_time = 3.0` | `post_init` L61 | 设置规划超时时间 |
|
||
| `.max_velocity = speed_` | `moveit_task` L119, `moveit_joint_task` L151 | 动态设置最大速度缩放因子 |
|
||
| `.max_acceleration = speed_` | `moveit_task` L120, `moveit_joint_task` L152 | 动态设置最大加速度缩放因子 |
|
||
| `.move_to_pose(...)` | `moveit_task` L129-137 | 笛卡尔空间运动规划与执行 |
|
||
| `.wait_until_executed()` | `moveit_task` L138, `moveit_joint_task` L157 | 阻塞等待运动完成 |
|
||
| `.move_to_configuration(...)` | `moveit_joint_task` L156 | 关节空间运动规划与执行 |
|
||
| `.compute_fk(...)` | `pick_and_place` L244, `moveit_joint_task` L160 | 正运动学：关节角 → 末端位姿 |
|
||
| `.compute_ik(...)` | `pick_and_place` L298-300 | 逆运动学：末端位姿 → 关节角（含约束） |
|
||
| `.end_effector_name` | `pick_and_place` L218 | 获取末端执行器 link 名 |
|
||
| `.joint_names` | `pick_and_place` L232, L308, L313 | 获取关节名列表 |
|
||
|
||
---
|
||
|
||
#### 2.1.11 错误处理策略
|
||
|
||
| 场景 | 处理方式 |
|
||
|------|---------|
|
||
| FK 计算失败 | 最多重试 `retry` 次（每次间隔 0.1s），超时返回 `result.success = False` |
|
||
| IK 计算失败 | 同上 |
|
||
| 运动规划失败 | 在 `moveit_task` / `moveit_joint_task` 中最多重试 `retry+1` 次 |
|
||
| 动作序列中任一步失败 | `pick_and_place` 立即中止并返回 `result.success = False` |
|
||
| 未知异常 | `pick_and_place` 和 `set_status` 捕获 Exception，重置 `cartesian_flag`，返回失败 |
|
||
|
||
---
|
||
|
||
#### 2.1.12 `cartesian_flag` 状态机
|
||
|
||
`cartesian_flag` 控制 `moveit_task` 中是否使用笛卡尔直线规划。在 `pick_and_place` 执行过程中，它被动态切换：
|
||
|
||
```
|
||
执行前: cartesian_flag = (上次残留状态)
|
||
|
||
动作序列执行:
|
||
Step 0: cartesian_flag ← False (自由空间规划，适合大范围移动)
|
||
Step 1: cartesian_flag ← True (笛卡尔直线，适合精确操作)
|
||
Step 2: cartesian_flag ← True
|
||
...
|
||
Step N: cartesian_flag ← True
|
||
|
||
异常时: cartesian_flag ← False (重置，防止残留影响后续操作)
|
||
```
|
||
|
||
这种设计的考量：第一步通常是从安全位移动到工作区附近（距离远、可能需要绕障），使用自由空间规划更灵活；后续步骤是在工作区内的精确操作（下降、抬升、平移），笛卡尔直线规划确保路径可预测。
|
||
|
||
---
|
||
|
||
#### 2.1.13 数据流总览
|
||
|
||
```
|
||
外部系统 (base_device_node)
|
||
│
|
||
│ JSON 指令字符串
|
||
▼
|
||
┌── MoveitInterface ──────────────────────────────────────────────────┐
|
||
│ │
|
||
│ set_position(cmd) ──→ moveit_task() ──→ MoveIt2.move_to_pose() │
|
||
│ │
|
||
│ set_status(cmd) ──→ moveit_joint_task() ──→ MoveIt2.move_to_config│
|
||
│ │
|
||
│ pick_and_place(cmd) │
|
||
│ │ │
|
||
│ ├─ MoveIt2.compute_fk() ─── /compute_fk service ──→ move_group │
|
||
│ ├─ MoveIt2.compute_ik() ─── /compute_ik service ──→ move_group │
|
||
│ ├─ moveit_task() ─── /move_action ──→ move_group │
|
||
│ ├─ moveit_joint_task() ─── /move_action ──→ move_group │
|
||
│ └─ resource_manager() ─── SendCmd Action ──→ tf_update │
|
||
│ │
|
||
│ 内部状态: │
|
||
│ joint_poses ← 预定义位姿查找表 │
|
||
│ moveit2{} ← MoveIt2 实例池 (per MoveGroup) │
|
||
│ tf_buffer ← TF2 坐标变换缓存 │
|
||
│ cartesian_flag ← 规划模式状态机 │
|
||
│ │
|
||
└─────────────────────────────────────────────────────────────────────┘
|
||
```
|
||
|
||
---
|
||
|
||
## 3. `resource_visalization.py` — 场景构建与 MoveIt2 启动
|
||
|
||
**路径**: `unilabos/device_mesh/resource_visalization.py`
|
||
**行数**: 429 行
|
||
**角色**: 根据实验室设备/资源配置，动态生成 URDF/SRDF，并启动 MoveIt2 所需的全部 ROS 2 节点。
|
||
|
||
### 3.1 核心类：`ResourceVisualization`
|
||
|
||
#### 3.1.1 模型加载与 URDF 生成
|
||
|
||
初始化阶段完成以下工作：
|
||
|
||
1. **遍历设备注册表**：区分 `resource`（静态资源如 plate/deck）和 `device`（可动设备如机械臂）
|
||
2. **Resource 类型**：记录 mesh 路径和 TF 变换信息，后续用于碰撞场景
|
||
3. **Device 类型**：通过 `xacro` 宏注入动态参数：
|
||
- 设备名前缀（`device_name`）
|
||
- 位置/旋转（从配置中提取）
|
||
- 自定义设备参数（`device_config`）
|
||
4. **MoveIt 设备**（`class` 包含 `moveit.`）：额外加载：
|
||
- `ros2_control` xacro 宏 → 生成 `ros2_control` 硬件接口描述
|
||
- SRDF xacro 宏 → 生成运动学语义描述（move group 定义、自碰撞矩阵等）
|
||
|
||
#### 3.1.2 MoveIt2 配置初始化 (`moveit_init`)
|
||
|
||
对每个 MoveIt 设备，加载并合并以下配置文件（均加上设备名前缀）：
|
||
|
||
| 配置文件 | 生成目标 | 作用 |
|
||
|----------|----------|------|
|
||
| `ros2_controllers.yaml` | `ros2_controllers_yaml` | 定义 ros2_control 控制器（JointTrajectoryController 等） |
|
||
| `moveit_controllers.yaml` | `moveit_controllers_yaml` | 定义 MoveIt 控制器映射 |
|
||
| `kinematics.yaml` | `moveit_nodes_kinematics` | 定义运动学求解器参数 |
|
||
|
||
所有关节名和控制器名都加上设备 ID 前缀，确保多设备共存时不冲突。
|
||
|
||
#### 3.1.3 ROS 2 Launch 节点启动
|
||
|
||
`create_launch_description()` 方法启动以下 ROS 2 节点：
|
||
|
||
| 节点 | 包 | 作用 |
|
||
|------|-----|------|
|
||
| `ros2_control_node` | `controller_manager` | ros2_control 硬件管理器，加载 URDF 和控制器配置 |
|
||
| `spawner` (per controller) | `controller_manager` | 激活各 JointTrajectoryController |
|
||
| `spawner` (joint_state_broadcaster) | `controller_manager` | 广播关节状态到 `/joint_states` |
|
||
| `robot_state_publisher` | `robot_state_publisher` | 根据 URDF 和关节状态发布 TF |
|
||
| `move_group` | `moveit_ros_move_group` | **MoveIt2 核心节点**，提供运动规划服务 |
|
||
| `rviz2` (可选) | `rviz2` | 3D 可视化 |
|
||
|
||
`move_group` 节点的参数配置包括：
|
||
|
||
- `robot_description`: 动态生成的 URDF
|
||
- `robot_description_semantic`: 动态生成的 SRDF
|
||
- `robot_description_kinematics`: 合并后的运动学配置
|
||
- `planning_pipelines`: 从 `moveit_configs_utils` 加载的规划器配置（默认 OMPL）
|
||
- `moveit_controllers_yaml`: 控制器映射配置
|
||
- `robot_description_planning`: 速度/加速度限制
|
||
|
||
---
|
||
|
||
## 4. 三个文件的协作关系
|
||
|
||
### 4.1 启动阶段
|
||
|
||
```
|
||
resource_visalization.py
|
||
│
|
||
├── 1. 解析设备/资源配置 → 生成 URDF + SRDF
|
||
├── 2. 合并控制器/运动学配置（带设备名前缀）
|
||
├── 3. 启动 ros2_control_node + controller spawners
|
||
├── 4. 启动 robot_state_publisher（发布 TF）
|
||
├── 5. 启动 move_group（提供规划服务）
|
||
└── 6. (可选) 启动 rviz2
|
||
```
|
||
|
||
### 4.2 运行阶段
|
||
|
||
```
|
||
外部调用（如 base_device_node）
|
||
│
|
||
▼
|
||
moveit_interface.py
|
||
│
|
||
├── pick_and_place() ──┬── compute_fk() ──→ moveit2.py → /compute_fk service
|
||
│ ├── compute_ik() ──→ moveit2.py → /compute_ik service
|
||
│ ├── move_to_pose() ──→ moveit2.py → /move_action or /plan + /execute
|
||
│ ├── move_to_config()──→ moveit2.py → /move_action or /plan + /execute
|
||
│ └── resource_manager()→ SendCmd Action → tf_update
|
||
│
|
||
├── set_position() ────→ moveit_task() ──→ moveit2.py → move_to_pose()
|
||
│
|
||
└── set_status() ──────→ moveit_joint_task()→ moveit2.py → move_to_configuration()
|
||
```
|
||
|
||
### 4.3 MoveIt2 消息类型使用一览
|
||
|
||
| 消息/服务/Action | 文件 | 用途 |
|
||
|-----------------|------|------|
|
||
| `moveit_msgs/action/MoveGroup` | moveit2.py | 一体化规划+执行 |
|
||
| `moveit_msgs/action/ExecuteTrajectory` | moveit2.py | 独立轨迹执行 |
|
||
| `moveit_msgs/srv/GetMotionPlan` | moveit2.py | 运动规划 |
|
||
| `moveit_msgs/srv/GetCartesianPath` | moveit2.py | 笛卡尔路径规划 |
|
||
| `moveit_msgs/srv/GetPositionFK` | moveit2.py | 正运动学 |
|
||
| `moveit_msgs/srv/GetPositionIK` | moveit2.py | 逆运动学 |
|
||
| `moveit_msgs/srv/GetPlanningScene` | moveit2.py | 获取规划场景 |
|
||
| `moveit_msgs/srv/ApplyPlanningScene` | moveit2.py | 应用规划场景 |
|
||
| `moveit_msgs/msg/CollisionObject` | moveit2.py | 碰撞物体管理 |
|
||
| `moveit_msgs/msg/AttachedCollisionObject` | moveit2.py | 附着碰撞物体 |
|
||
| `moveit_msgs/msg/Constraints` | moveit2.py, moveit_interface.py | 目标/路径约束 |
|
||
| `moveit_msgs/msg/JointConstraint` | moveit2.py, moveit_interface.py | 关节约束 |
|
||
| `moveit_msgs/msg/PlanningScene` | moveit2.py | 规划场景 |
|
||
|
||
---
|
||
|
||
## 5. 注册表中的 `model` 字段与 3D 模型加载
|
||
|
||
### 5.1 `model` 字段概述
|
||
|
||
在 Uni-Lab-OS 的设备/资源注册表 YAML 中，`model` 字段是连接**注册表定义**与**3D 可视化系统**的关键。它告诉 `ResourceVisualization` 如何为该设备或资源加载 3D 模型。
|
||
|
||
以 `arm_slider` 为例：
|
||
|
||
```yaml
|
||
# unilabos/registry/devices/robot_arm.yaml (L355-358)
|
||
model:
|
||
mesh: arm_slider
|
||
path: https://uni-lab.oss-cn-zhangjiakou.aliyuncs.com/uni-lab/devices/arm_slider/macro_device.xacro
|
||
type: device
|
||
```
|
||
|
||
**三个字段的作用：**
|
||
|
||
| 字段 | 值 | 说明 |
|
||
|------|-----|------|
|
||
| `mesh` | `arm_slider` | 模型文件夹名，对应 `device_mesh/devices/arm_slider/` 目录 |
|
||
| `path` | `https://...macro_device.xacro` | 模型文件的远程下载地址（OSS），用于首次部署时下载模型资源 |
|
||
| `type` | `device` | 模型类型标识，决定 `ResourceVisualization` 的处理逻辑 |
|
||
|
||
### 5.2 `type` 字段的两种取值
|
||
|
||
`model.type` 决定了 `ResourceVisualization` 如何加载和处理 3D 模型，有两条完全不同的路径：
|
||
|
||
#### `type: device` — 动态设备（如机械臂、龙门架）
|
||
|
||
```python
|
||
# resource_visalization.py L121-163
|
||
if model_config['type'] == 'device':
|
||
# 1. 通过 xacro include 加载设备的 URDF 宏
|
||
# → device_mesh/devices/{mesh}/macro_device.xacro
|
||
|
||
# 2. 调用 xacro 宏，注入参数：
|
||
# parent_link, mesh_path, device_name, x/y/z, rx/ry/r, device_config...
|
||
|
||
# 3. 若设备 class 包含 "moveit."：
|
||
# → 额外加载 ros2_control xacro 和 SRDF xacro
|
||
# → 注册为 moveit_nodes，后续由 moveit_init() 加载控制器配置
|
||
```
|
||
|
||
**处理流程：**
|
||
|
||
```
|
||
注册表 model.mesh = "arm_slider"
|
||
│
|
||
├── 加载 URDF: device_mesh/devices/arm_slider/macro_device.xacro
|
||
│ → 生成完整的 link/joint 运动链，嵌入到全局 URDF 中
|
||
│
|
||
├── 注入位置参数: x, y, z, rx, ry, r (从节点配置 position/rotation 中读取, 单位 mm→m)
|
||
│
|
||
├── 注入设备参数: device_config 中的键值对作为 xacro 参数
|
||
│
|
||
└── 若 class 含 "moveit.":
|
||
├── 加载 ros2_control: config/macro.ros2_control.xacro
|
||
├── 加载 SRDF: config/macro.srdf.xacro
|
||
└── 记录到 moveit_nodes → moveit_init() 加载 controllers/kinematics
|
||
```
|
||
|
||
#### `type: resource` — 静态资源（如微孔板、试管架）
|
||
|
||
```python
|
||
# resource_visalization.py L111-120
|
||
if model_config['type'] == 'resource':
|
||
# 只记录 mesh 文件路径和 TF 偏移，用于碰撞场景
|
||
# 不注入到 URDF 运动链中
|
||
resource_model[node_id] = {
|
||
'mesh': f"device_mesh/resources/{mesh}",
|
||
'mesh_tf': model_config['mesh_tf']
|
||
}
|
||
```
|
||
|
||
Resource 类型的 `model` 结构更丰富，包含 TF 偏移和子物体：
|
||
|
||
```yaml
|
||
# 例: registry/resources/opentrons/plates.yaml
|
||
model:
|
||
mesh: tecan_nested_tip_rack/meshes/plate.stl # 主体 mesh（STL 文件）
|
||
mesh_tf: [0.064, 0.043, 0, -1.5708] # 位姿偏移 [x, y, z, rotation]
|
||
children_mesh: generic_labware_tube_10_75/meshes/0_base.stl # 子物体 mesh
|
||
children_mesh_tf: [0.0018, 0.0018, 0, -1.5708] # 子物体偏移
|
||
```
|
||
|
||
### 5.3 两种类型的对比
|
||
|
||
| 对比项 | `type: device` | `type: resource` |
|
||
|--------|---------------|-----------------|
|
||
| **模型格式** | xacro 宏（动态参数化 URDF） | STL 静态 mesh 文件 |
|
||
| **加载方式** | xacro include → 嵌入全局 URDF | 记录路径 → 后续作为碰撞体添加 |
|
||
| **位置来源** | 节点配置中的 position/rotation | `mesh_tf` 偏移数组 |
|
||
| **是否有关节** | 是（prismatic/revolute） | 否（纯静态） |
|
||
| **支持 MoveIt** | 是（通过 class 名中的 `moveit.` 触发） | 否 |
|
||
| **子物体** | 无（运动链本身定义了所有部件） | 可选 `children_mesh`（如管架中的管子） |
|
||
| **远程路径** | `path` 字段指向 OSS 下载地址 | 类似，`children_mesh_path` 指向子物体 |
|
||
| **存放目录** | `device_mesh/devices/{mesh}/` | `device_mesh/resources/{mesh}` |
|
||
| **实际示例** | arm_slider, toyo_xyz, elite_robot | 微孔板, tip rack, 试管架 |
|
||
|
||
### 5.4 `arm_slider` 注册表完整结构
|
||
|
||
`arm_slider` 的注册表键名 `robotic_arm.SCARA_with_slider.moveit.virtual` 本身就编码了重要信息：
|
||
|
||
```
|
||
robotic_arm → 设备大类（机械臂）
|
||
.SCARA_with_slider → 具体型号（SCARA 构型 + 线性滑轨）
|
||
.moveit → ★ 标记为 MoveIt 设备（class 名包含 "moveit."）
|
||
.virtual → 仿真/虚拟设备
|
||
```
|
||
|
||
**class 名中包含 `moveit.` 的关键作用**：`ResourceVisualization` 在 L151 通过 `node['class'].find('moveit.') != -1` 判断是否需要加载 MoveIt 配置。这是 **MoveIt 设备与普通 device 的唯一判别条件**——即使两者的 `model.type` 都是 `device`。
|
||
|
||
注册表中的关键部分：
|
||
|
||
```yaml
|
||
robotic_arm.SCARA_with_slider.moveit.virtual:
|
||
# 设备驱动类
|
||
class:
|
||
module: unilabos.devices.ros_dev.moveit_interface:MoveitInterface
|
||
type: python
|
||
action_value_mappings:
|
||
pick_and_place: ... # SendCmd Action（JSON 指令）
|
||
set_position: ... # SendCmd Action
|
||
set_status: ... # SendCmd Action
|
||
auto-moveit_task: ... # 自动发现的方法（UniLabJsonCommand）
|
||
auto-moveit_joint_task: ...
|
||
auto-resource_manager: ...
|
||
auto-post_init: ...
|
||
|
||
# 初始化参数
|
||
init_param_schema:
|
||
config:
|
||
properties:
|
||
moveit_type: ... # → 对应 device_mesh/devices/{moveit_type}/ 文件夹
|
||
joint_poses: ... # → 预定义关节位姿查找表
|
||
rotation: ...
|
||
device_config: ...
|
||
|
||
# ★ 3D 模型定义
|
||
model:
|
||
mesh: arm_slider # → device_mesh/devices/arm_slider/
|
||
path: https://... # → OSS 远程下载地址
|
||
type: device # → ResourceVisualization 按 device 逻辑加载
|
||
```
|
||
|
||
### 5.5 从注册表到 MoveIt2 的完整链路
|
||
|
||
```
|
||
注册表 YAML
|
||
│
|
||
│ model.mesh = "arm_slider"
|
||
│ model.type = "device"
|
||
│ class = "robotic_arm.SCARA_with_slider.moveit.virtual"
|
||
│ ^^^^^^
|
||
│ class 含 "moveit."
|
||
▼
|
||
ResourceVisualization.__init__()
|
||
│
|
||
├── model.type == "device"
|
||
│ └── xacro include: devices/arm_slider/macro_device.xacro → URDF
|
||
│
|
||
├── class 含 "moveit."
|
||
│ ├── xacro include: devices/arm_slider/config/macro.ros2_control.xacro → URDF
|
||
│ ├── xacro include: devices/arm_slider/config/macro.srdf.xacro → SRDF
|
||
│ └── moveit_nodes["device_id"] = "arm_slider"
|
||
│
|
||
▼
|
||
ResourceVisualization.moveit_init()
|
||
│
|
||
├── 加载 devices/arm_slider/config/ros2_controllers.yaml
|
||
├── 加载 devices/arm_slider/config/moveit_controllers.yaml
|
||
├── 加载 devices/arm_slider/config/kinematics.yaml
|
||
└── 合并到全局配置（带设备名前缀）
|
||
│
|
||
▼
|
||
ResourceVisualization.create_launch_description()
|
||
│
|
||
├── 启动 ros2_control_node（加载 URDF + 控制器配置）
|
||
├── 启动 controller spawners（激活 arm_controller、gripper_controller）
|
||
├── 启动 robot_state_publisher（发布 TF）
|
||
├── 启动 move_group（MoveIt2 核心，加载 SRDF + kinematics + planners）
|
||
└── (可选) 启动 rviz2
|
||
│
|
||
▼
|
||
MoveitInterface.post_init()
|
||
│
|
||
├── 读取 devices/arm_slider/config/move_group.json
|
||
├── 为 "arm" 组创建 MoveIt2 实例
|
||
└── 等待 tf_update Action Server 就绪
|
||
│
|
||
▼
|
||
运行时: pick_and_place / set_position / set_status
|
||
```
|
||
|
||
---
|
||
|
||
## 6. 设备模型文件夹结构：MoveIt 设备 vs 非 MoveIt 设备
|
||
|
||
`device_mesh/devices/` 下的每个子文件夹代表一种设备类型的 3D 模型和配置。根据设备是否需要 MoveIt2 运动规划，文件夹内容有**显著差异**。
|
||
|
||
### 5.1 非 MoveIt 设备示例：`slide_w140` / `hplc_station`
|
||
|
||
非 MoveIt 设备只需要 3D 可视化和简单的关节控制（由 `liquid_handler_joint_publisher` 等自定义节点直接控制），**不需要运动规划**。
|
||
|
||
```
|
||
slide_w140/ hplc_station/
|
||
├── macro_device.xacro ├── macro_device.xacro
|
||
├── joint_config.json ├── joint_config.json
|
||
├── param_config.json ├── param_config.json
|
||
└── meshes/ └── meshes/
|
||
└── *.STL └── *.STL
|
||
```
|
||
|
||
**仅 3 个配置文件：**
|
||
|
||
| 文件 | 作用 |
|
||
|------|------|
|
||
| `macro_device.xacro` | URDF 模型（xacro 宏），定义 link/joint/visual/collision |
|
||
| `joint_config.json` | 关节名与轴向信息，供自定义关节发布器使用 |
|
||
| `param_config.json` | 设备尺寸等可配参数（如轨道长度），注入到 xacro 宏参数中 |
|
||
|
||
**特点：**
|
||
- 没有 `config/` 子文件夹
|
||
- 没有 SRDF、ros2_control、MoveIt 控制器等配置
|
||
- 关节由应用层直接发布 `JointState`，不经过 ros2_control 和 MoveIt2
|
||
|
||
---
|
||
|
||
### 5.2 MoveIt 设备示例：`arm_slider`
|
||
|
||
MoveIt 设备需要完整的运动规划支持——从 ros2_control 硬件抽象到 MoveIt2 运动学求解和碰撞矩阵。
|
||
|
||
```
|
||
arm_slider/
|
||
├── macro_device.xacro ← URDF 模型（xacro 宏）
|
||
├── joint_limit.yaml ← 关节物理限制（effort/velocity/position）
|
||
├── meshes/ ← 3D 网格文件
|
||
│ ├── arm_slideway.STL
|
||
│ ├── arm_base.STL
|
||
│ ├── arm_link_1.STL
|
||
│ ├── arm_link_2.STL
|
||
│ ├── arm_link_3.STL
|
||
│ ├── gripper_base.STL
|
||
│ ├── gripper_right.STL
|
||
│ └── gripper_left.STL
|
||
│
|
||
└── config/ ← ★ MoveIt 设备独有的配置目录
|
||
├── macro.ros2_control.xacro ← ros2_control 硬件接口定义
|
||
├── macro.srdf.xacro ← SRDF 语义描述（Move Group + 碰撞矩阵）
|
||
├── move_group.json ← Move Group 定义（供 MoveitInterface 使用）
|
||
├── ros2_controllers.yaml ← ros2_control 控制器配置
|
||
├── moveit_controllers.yaml ← MoveIt ↔ 控制器映射
|
||
├── kinematics.yaml ← 运动学求解器配置
|
||
├── joint_limits.yaml ← MoveIt 用关节限制（速度/加速度缩放）
|
||
├── initial_positions.yaml ← 仿真初始关节位置
|
||
├── pilz_cartesian_limits.yaml ← Pilz 笛卡尔限制
|
||
└── moveit_planners.yaml ← 规划器配置
|
||
```
|
||
|
||
**比非 MoveIt 设备多出 10 个配置文件**，全部位于 `config/` 子目录。
|
||
|
||
---
|
||
|
||
### 5.3 `arm_slider` 各文件详解
|
||
|
||
#### 5.3.1 `macro_device.xacro` — URDF 运动链定义
|
||
|
||
定义了 arm_slider 的完整运动链，包含 8 个 link 和 7 个 joint：
|
||
|
||
```
|
||
world
|
||
└── [fixed] base_link_joint
|
||
└── device_link
|
||
└── [fixed] device_link_joint
|
||
└── arm_slideway (底座滑轨, 有 visual + collision mesh)
|
||
└── [prismatic, X轴] arm_base_joint (滑轨平移)
|
||
└── arm_base (机械臂底座)
|
||
└── [prismatic, Z轴] arm_link_1_joint (升降)
|
||
└── arm_link_1
|
||
└── [revolute, Z轴] arm_link_2_joint (旋转关节1)
|
||
└── arm_link_2
|
||
└── [revolute, Z轴] arm_link_3_joint (旋转关节2)
|
||
└── arm_link_3
|
||
└── [revolute, Z轴] gripper_base_joint (夹爪旋转)
|
||
└── gripper_base (夹爪底座)
|
||
├── [prismatic, X轴] gripper_right_joint
|
||
│ └── gripper_right
|
||
└── [prismatic, X轴, mimic] gripper_left_joint
|
||
└── gripper_left
|
||
```
|
||
|
||
**关键设计点：**
|
||
|
||
- **混合关节类型**：包含 prismatic（滑轨平移 + 升降 + 夹爪）和 revolute（旋转）关节
|
||
- **Mimic 关节**：`gripper_left_joint` 通过 `<mimic>` 标签跟随 `gripper_right_joint`，实现对称夹爪联动
|
||
- **参数化前缀**：所有 link/joint 名都带 `${station_name}${device_name}` 前缀，支持多实例
|
||
- **外部关节限制**：从 `joint_limit.yaml` 加载 effort/velocity/position 范围
|
||
- **完整物理属性**：每个 link 都有 `<inertial>`（质量、惯量矩阵）、`<visual>`（STL mesh）和 `<collision>`（碰撞体）
|
||
|
||
#### 5.3.2 `joint_limit.yaml` — 关节物理限制
|
||
|
||
定义每个关节的运动范围和动力学参数，被 `macro_device.xacro` 引用：
|
||
|
||
| 关节 | 类型 | 范围 | 说明 |
|
||
|------|------|------|------|
|
||
| `arm_base_joint` | prismatic | 0 ~ 1.5m | 滑轨水平行程 |
|
||
| `arm_link_1_joint` | prismatic | 0 ~ 0.6m | 升降行程 |
|
||
| `arm_link_2_joint` | revolute | -95° ~ 95° | 第一旋转关节 |
|
||
| `arm_link_3_joint` | revolute | -195° ~ 195° | 第二旋转关节 |
|
||
| `gripper_base_joint` | revolute | -95° ~ 95° | 夹爪旋转 |
|
||
| `gripper_right/left_joint` | prismatic | 0 ~ 0.03m | 夹爪开合 |
|
||
|
||
#### 5.3.3 `config/macro.ros2_control.xacro` — ros2_control 硬件接口
|
||
|
||
定义 ros2_control 硬件抽象层，将关节映射到控制接口：
|
||
|
||
- **硬件插件**：`mock_components/GenericSystem`（仿真模式，可替换为真实硬件驱动）
|
||
- **每个关节声明**：
|
||
- `command_interface: position` — 位置控制模式
|
||
- `state_interface: position` — 位置反馈（含 `initial_value` 从 `initial_positions.yaml` 加载）
|
||
- `state_interface: velocity` — 速度反馈
|
||
- **6 个关节**：`arm_base_joint` ~ `gripper_right_joint`（`gripper_left_joint` 因为是 mimic 关节，不需要独立控制接口）
|
||
|
||
#### 5.3.4 `config/macro.srdf.xacro` — SRDF 语义描述
|
||
|
||
MoveIt2 的语义机器人描述，定义了：
|
||
|
||
**Move Groups（规划组）：**
|
||
|
||
| 组名 | 类型 | 内容 |
|
||
|------|------|------|
|
||
| `{device_name}arm` | chain | `arm_slideway` → `gripper_base`（5 DOF 运动链） |
|
||
| `{device_name}arm_gripper` | joint | `gripper_right_joint`（夹爪控制） |
|
||
|
||
**Disable Collisions（自碰撞矩阵）：**
|
||
|
||
22 条 `<disable_collisions>` 规则，标记不可能碰撞的 link 对（Adjacent / Never），减少碰撞检测计算量。例如：
|
||
|
||
- `arm_base` ↔ `arm_slideway`：Adjacent（相邻 link，必然接触）
|
||
- `arm_link_1` ↔ `arm_link_3`：Never（物理上不可能碰撞）
|
||
- `gripper_left` ↔ `gripper_right`：Never
|
||
|
||
#### 5.3.5 `config/move_group.json` — MoveitInterface 配置
|
||
|
||
供 `MoveitInterface.post_init()` 使用，定义每个 Move Group 的关节和端点：
|
||
|
||
```json
|
||
{
|
||
"arm": {
|
||
"joint_names": ["arm_base_joint", "arm_link_1_joint",
|
||
"arm_link_2_joint", "arm_link_3_joint",
|
||
"gripper_base_joint"],
|
||
"base_link_name": "device_link",
|
||
"end_effector_name": "gripper_base"
|
||
}
|
||
}
|
||
```
|
||
|
||
`MoveitInterface` 读取此文件后，为 `"arm"` 组创建一个 `MoveIt2` 实例，自动加上设备名前缀。
|
||
|
||
#### 5.3.6 `config/ros2_controllers.yaml` — 控制器定义
|
||
|
||
定义两个 `JointTrajectoryController`：
|
||
|
||
| 控制器 | 控制的关节 | 说明 |
|
||
|--------|-----------|------|
|
||
| `arm_controller` | arm_base_joint ~ gripper_base_joint (5个) | 机械臂主体 |
|
||
| `gripper_controller` | gripper_right_joint (1个) | 夹爪 |
|
||
|
||
被 `resource_visalization.py` 的 `moveit_init()` 读取，加上设备名前缀后合并到全局 `ros2_controllers_yaml` 中。
|
||
|
||
#### 5.3.7 `config/moveit_controllers.yaml` — MoveIt ↔ 控制器映射
|
||
|
||
告诉 MoveIt2 的 `move_group` 节点如何将规划好的轨迹发送到 ros2_control 控制器：
|
||
|
||
- `arm_controller` → FollowJointTrajectory Action（5 个关节）
|
||
- `gripper_controller` → FollowJointTrajectory Action（1 个关节）
|
||
|
||
#### 5.3.8 `config/kinematics.yaml` — 运动学求解器
|
||
|
||
```yaml
|
||
arm:
|
||
kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin
|
||
kinematics_solver_search_resolution: 0.005
|
||
kinematics_solver_timeout: 0.005
|
||
```
|
||
|
||
使用 **LMA (Levenberg-Marquardt Algorithm)** 运动学求解器进行正/逆运动学计算。这是 MoveIt2 的通用 IK 求解器，适用于任意运动链拓扑。
|
||
|
||
#### 5.3.9 其他配置文件
|
||
|
||
| 文件 | 作用 |
|
||
|------|------|
|
||
| `initial_positions.yaml` | 仿真启动时各关节初始角度/位置（所有为 0，夹爪张开 0.03m） |
|
||
| `joint_limits.yaml` | MoveIt 层面的速度/加速度缩放限制（覆盖 URDF 中的值） |
|
||
| `pilz_cartesian_limits.yaml` | Pilz 工业运动规划器的笛卡尔速度/加速度限制 |
|
||
| `moveit_planners.yaml` | 可用规划器列表（`ompl_interface/OMPLPlanner`） |
|
||
|
||
---
|
||
|
||
### 5.4 对比：`toyo_xyz`（另一个 MoveIt 设备）
|
||
|
||
`toyo_xyz` 是一个三轴直线运动平台（XYZ 龙门），也是 MoveIt 设备。与 `arm_slider` 对比：
|
||
|
||
| 对比项 | `arm_slider` | `toyo_xyz` |
|
||
|--------|-------------|------------|
|
||
| **自由度** | 5 DOF (2 prismatic + 3 revolute) + 夹爪 | 3 DOF (3 prismatic) |
|
||
| **运动链** | 混合链（平移+旋转） | 纯直线链（全 prismatic） |
|
||
| **Move Group** | `arm` (5 joints) + `arm_gripper` (1 joint) | `toyo_xyz` (3 joints) |
|
||
| **末端执行器** | `gripper_base` | `slider3_link` |
|
||
| **独有文件** | `joint_limit.yaml` | `joint_config.json` + `param_config.json` |
|
||
| **xacro 参数** | 固定尺寸 | 可配长度 (`length1/2/3`) + mesh scale 缩放 |
|
||
| **IK 求解器** | LMA | LMA |
|
||
|
||
`toyo_xyz` 的额外特点：
|
||
- `param_config.json`：定义三轴行程和滑块尺寸，通过 xacro 参数动态缩放 STL 模型
|
||
- `joint_config.json`：简单的关节名→轴向映射，供非 MoveIt 的关节发布器使用
|
||
- `config/full_dev.urdf.xacro`：额外的完整 URDF 文件（独立调试用）
|
||
|
||
两者的 **MoveIt 配置文件结构完全一致**（SRDF、ros2_control、controllers、kinematics），说明 Uni-Lab-OS 的 MoveIt 设备遵循统一的模板。
|
||
|
||
---
|
||
|
||
### 5.5 MoveIt vs 非 MoveIt 设备文件对比总结
|
||
|
||
```
|
||
非 MoveIt 设备 (slide_w140) MoveIt 设备 (arm_slider)
|
||
─────────────────────────── ───────────────────────────
|
||
macro_device.xacro ✓ macro_device.xacro ✓
|
||
joint_config.json ✓ joint_limit.yaml ✓
|
||
param_config.json ✓
|
||
config/
|
||
├── macro.ros2_control.xacro ★ ros2_control 硬件接口
|
||
├── macro.srdf.xacro ★ SRDF (Move Group + 碰撞)
|
||
├── move_group.json ★ MoveitInterface 配置
|
||
├── ros2_controllers.yaml ★ 控制器定义
|
||
├── moveit_controllers.yaml ★ MoveIt↔控制器映射
|
||
├── kinematics.yaml ★ IK 求解器配置
|
||
├── joint_limits.yaml ★ MoveIt 关节限制
|
||
├── initial_positions.yaml ★ 仿真初始状态
|
||
├── pilz_cartesian_limits.yaml ★ 笛卡尔限制
|
||
└── moveit_planners.yaml ★ 规划器列表
|
||
|
||
文件数: 3 文件数: 12 (3 + 10 MoveIt 专用)
|
||
```
|
||
|
||
**核心区别：MoveIt 设备多出的 `config/` 目录下的 10 个文件，构成了 MoveIt2 运动规划所需的完整配置栈：**
|
||
|
||
1. **硬件层** (`macro.ros2_control.xacro`): 关节如何被控制
|
||
2. **语义层** (`macro.srdf.xacro`): 哪些关节组成规划组，哪些碰撞可以忽略
|
||
3. **规划层** (`kinematics.yaml`, `moveit_planners.yaml`, `pilz_cartesian_limits.yaml`): 如何求解和规划
|
||
4. **执行层** (`ros2_controllers.yaml`, `moveit_controllers.yaml`): 轨迹如何下发到控制器
|
||
5. **桥接层** (`move_group.json`): Uni-Lab-OS 的 `MoveitInterface` 如何连接到 MoveIt2
|
||
|
||
---
|
||
|
||
## 6. 设计特点
|
||
|
||
1. **多设备支持**：通过设备名前缀机制（`device_id_`），所有关节名、link 名、控制器名都是唯一的，支持在同一 ROS 2 环境中运行多台机械臂。
|
||
|
||
2. **动态场景构建**：`ResourceVisualization` 根据实验室配置动态生成 URDF/SRDF，无需手动编写或维护静态模型文件。
|
||
|
||
3. **规划/执行分离**：`MoveIt2` 类支持 MoveGroup Action（合并模式）和 Plan+Execute（分离模式），可根据场景灵活选择。
|
||
|
||
4. **线程安全**：`MoveIt2` 类通过 `threading.Lock` 保护关节状态和执行状态的并发访问。
|
||
|
||
5. **碰撞场景集成**：支持完整的碰撞物体生命周期管理（添加/移动/附着/分离/删除），可在运行时动态更新规划场景。
|
||
|
||
6. **资源 TF 动态更新**：`MoveitInterface` 通过 `resource_manager()` 在 pick/place 时动态更新资源的 TF 父 link，实现物体在机器人和环境之间的"跟随"效果。
|
||
|
||
7. **统一设备模板**：MoveIt 设备遵循统一的 `config/` 目录结构（SRDF、ros2_control、controllers、kinematics），新增设备只需按模板创建配置文件即可接入 MoveIt2 运动规划。
|