Compare commits
19 Commits
develop
...
develop_ra
| Author | SHA1 | Date | |
|---|---|---|---|
|
64faf552bd | ||
|
|
773ce04f37 | ||
|
|
900869515c | ||
|
|
4da9895e4c | ||
|
|
5d2a300c85 | ||
|
|
87a3bf68f9 | ||
|
|
2be6713107 | ||
|
|
99b3d4a7a2 | ||
|
|
1a5309f9c1 | ||
|
|
3c1f82d1ae | ||
|
|
64ee485100 | ||
|
|
a721198fe0 | ||
|
|
efe2f9f87c | ||
|
|
3b3bdc77a5 | ||
|
|
84678115e2 | ||
|
|
d2824e1657 | ||
|
|
541f0dbd02 | ||
|
|
6c02271f49 | ||
|
|
29dbef1cd6 |
8
.gitignore
vendored
8
.gitignore
vendored
@@ -1,8 +0,0 @@
|
||||
# 忽略所有层级的 build 文件夹及其内容
|
||||
**/build/
|
||||
|
||||
# 忽略所有层级的 install 文件夹及其内容
|
||||
**/install/
|
||||
|
||||
# 忽略所有层级的 log 文件夹及其内容
|
||||
**/log/
|
||||
18
.vscode/c_cpp_properties.json
vendored
18
.vscode/c_cpp_properties.json
vendored
@@ -1,18 +0,0 @@
|
||||
{
|
||||
"configurations": [
|
||||
{
|
||||
"name": "Linux",
|
||||
"includePath": [
|
||||
"${workspaceFolder}/**",
|
||||
"/opt/ros/humble/include/**",
|
||||
"/usr/include/**"
|
||||
],
|
||||
"defines": [],
|
||||
"compilerPath": "/usr/bin/gcc",
|
||||
"cStandard": "c17",
|
||||
"cppStandard": "gnu++17",
|
||||
"intelliSenseMode": "linux-gcc-x64"
|
||||
}
|
||||
],
|
||||
"version": 4
|
||||
}
|
||||
83
.vscode/settings.json
vendored
83
.vscode/settings.json
vendored
@@ -1,83 +0,0 @@
|
||||
{
|
||||
"files.associations": {
|
||||
"*.launch": "python",
|
||||
"*.world": "xml",
|
||||
"*.xacro": "xml",
|
||||
"cctype": "cpp",
|
||||
"clocale": "cpp",
|
||||
"cmath": "cpp",
|
||||
"csignal": "cpp",
|
||||
"cstdarg": "cpp",
|
||||
"cstddef": "cpp",
|
||||
"cstdio": "cpp",
|
||||
"cstdlib": "cpp",
|
||||
"cstring": "cpp",
|
||||
"ctime": "cpp",
|
||||
"cwchar": "cpp",
|
||||
"cwctype": "cpp",
|
||||
"any": "cpp",
|
||||
"array": "cpp",
|
||||
"atomic": "cpp",
|
||||
"strstream": "cpp",
|
||||
"bit": "cpp",
|
||||
"bitset": "cpp",
|
||||
"chrono": "cpp",
|
||||
"codecvt": "cpp",
|
||||
"compare": "cpp",
|
||||
"complex": "cpp",
|
||||
"concepts": "cpp",
|
||||
"condition_variable": "cpp",
|
||||
"cstdint": "cpp",
|
||||
"deque": "cpp",
|
||||
"forward_list": "cpp",
|
||||
"list": "cpp",
|
||||
"map": "cpp",
|
||||
"set": "cpp",
|
||||
"string": "cpp",
|
||||
"unordered_map": "cpp",
|
||||
"unordered_set": "cpp",
|
||||
"vector": "cpp",
|
||||
"exception": "cpp",
|
||||
"algorithm": "cpp",
|
||||
"functional": "cpp",
|
||||
"iterator": "cpp",
|
||||
"memory": "cpp",
|
||||
"memory_resource": "cpp",
|
||||
"numeric": "cpp",
|
||||
"optional": "cpp",
|
||||
"random": "cpp",
|
||||
"ratio": "cpp",
|
||||
"regex": "cpp",
|
||||
"string_view": "cpp",
|
||||
"system_error": "cpp",
|
||||
"tuple": "cpp",
|
||||
"type_traits": "cpp",
|
||||
"utility": "cpp",
|
||||
"fstream": "cpp",
|
||||
"future": "cpp",
|
||||
"initializer_list": "cpp",
|
||||
"iomanip": "cpp",
|
||||
"iosfwd": "cpp",
|
||||
"iostream": "cpp",
|
||||
"istream": "cpp",
|
||||
"limits": "cpp",
|
||||
"mutex": "cpp",
|
||||
"new": "cpp",
|
||||
"numbers": "cpp",
|
||||
"ostream": "cpp",
|
||||
"semaphore": "cpp",
|
||||
"shared_mutex": "cpp",
|
||||
"sstream": "cpp",
|
||||
"stdexcept": "cpp",
|
||||
"stop_token": "cpp",
|
||||
"streambuf": "cpp",
|
||||
"thread": "cpp",
|
||||
"cfenv": "cpp",
|
||||
"cinttypes": "cpp",
|
||||
"typeindex": "cpp",
|
||||
"typeinfo": "cpp",
|
||||
"variant": "cpp",
|
||||
"dense": "cpp",
|
||||
"filesystem": "cpp"
|
||||
}
|
||||
}
|
||||
73
LICENSE
73
LICENSE
@@ -1,73 +0,0 @@
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a file or class name and description of purpose be included on the same "printed page" as the copyright notice for easier identification within third-party archives.
|
||||
|
||||
Copyright 2025 HiveCoreRD
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
107
README.md
107
README.md
@@ -1,2 +1,109 @@
|
||||
# hivecore_robot_motion
|
||||
|
||||
`hivecore_robot_motion` 是 HiveCore 机器人 OS1 的运动控制模块，基于 ROS 2（Humble）和 MoveIt 2，提供双臂运动规划与执行、底盘/腰部/腿部控制、以及硬件与安全相关服务。
|
||||
|
||||
## 仓库结构
|
||||
|
||||
```text
|
||||
hivecore_robot_motion/
|
||||
├── src/
|
||||
│ ├── robot_control/ # 核心运动控制包（节点、Action、Service、Controller）
|
||||
│ ├── dual_arm_description/ # 双臂机器人 URDF/描述文件
|
||||
│ └── dual_arm_moveit_config/ # MoveIt 配置与启动文件
|
||||
└── docs/ # 设计文档（PTP/LIN/轨迹融合等）
|
||||
```
|
||||
|
||||
`robot_control` 采用分层设计：
|
||||
|
||||
- `actions/`：Action 服务端，负责协调任务流程
|
||||
- `controllers/`：底层控制器，封装硬件控制逻辑
|
||||
- `services/`：对外提供 ROS Service 能力
|
||||
- `core/`：统一管理控制器/服务/动作实例
|
||||
- `utils/`：轨迹与工具函数
|
||||
|
||||
## 主要功能
|
||||
|
||||
- 双臂运动控制（MoveIt 规划与执行）
|
||||
- 底盘、腿部、腰部基础运动控制
|
||||
- 运动学、硬件、状态、安全等服务接口
|
||||
- 轨迹插补与平滑（梯形/S 曲线/轨迹融合）
|
||||
|
||||
## 环境依赖
|
||||
|
||||
建议环境：
|
||||
|
||||
- Ubuntu + ROS 2 Humble
|
||||
- colcon / ament_cmake
|
||||
- MoveIt 2（`moveit_core`、`moveit_ros_planning_interface` 等）
|
||||
|
||||
`robot_control` 包内依赖可参考：
|
||||
|
||||
- `src/robot_control/package.xml`
|
||||
- `src/robot_control/CMakeLists.txt`
|
||||
|
||||
## 构建
|
||||
|
||||
在工作空间根目录（例如 `hivecore_robot_os1/`）执行：
|
||||
|
||||
```bash
|
||||
colcon build --symlink-install --packages-select dual_arm_description dual_arm_moveit_config robot_control
|
||||
source install/setup.bash
|
||||
```
|
||||
|
||||
如果只编译本仓库包，也可以在当前目录执行：
|
||||
|
||||
```bash
|
||||
colcon build --symlink-install --base-paths src
|
||||
source install/setup.bash
|
||||
```
|
||||
|
||||
## 运行示例
|
||||
|
||||
### 1) 启动 robot_control（自动拉起 move_group）
|
||||
|
||||
```bash
|
||||
ros2 launch robot_control robot_control.launch.py
|
||||
```
|
||||
|
||||
### 2) 启动 MoveIt（带 RViz）
|
||||
|
||||
```bash
|
||||
ros2 launch dual_arm_moveit_config move_group_with_rviz.launch.py
|
||||
```
|
||||
|
||||
### 3) 仅查看机器人模型（description + RViz）
|
||||
|
||||
```bash
|
||||
ros2 launch dual_arm_description display.launch.py
|
||||
```
|
||||
|
||||
### 4) 右臂关节测试节点
|
||||
|
||||
```bash
|
||||
ros2 launch robot_control right_arm_joint_test.launch.py
|
||||
```
|
||||
|
||||
可选参数（来自 launch 文件）：
|
||||
|
||||
- `use_action`：`true/false`，是否使用 FollowJointTrajectory action
|
||||
- `action_name`：默认 `/right_arm_controller/follow_joint_trajectory`
|
||||
|
||||
## 开发说明
|
||||
|
||||
- C++ 标准：`C++17`
|
||||
- 主可执行文件：`robot_control_node`
|
||||
- 测试可执行文件：`right_arm_joint_test`
|
||||
- 设计文档位于 `docs/`
|
||||
|
||||
推荐遵循以下约束：
|
||||
|
||||
- Action 层不直接操作硬件
|
||||
- Controller 层不依赖 Action 实现
|
||||
- 统一使用 `RCLCPP_*` 日志，避免 `std::cout/printf`
|
||||
- 对外接口保持清晰的成功/失败返回语义
|
||||
|
||||
## 常见问题
|
||||
|
||||
- **找不到 MoveIt 相关依赖**：确认已安装 ROS 2 Humble 对应 MoveIt 2 组件，并已 `source /opt/ros/humble/setup.bash`。
|
||||
- **launch 失败提示包不存在**：确认已完成 `colcon build` 并正确 `source install/setup.bash`。
|
||||
- **运行无模型显示**：优先检查 `dual_arm_description` 包是否已成功安装，以及 URDF 路径是否可读。
|
||||
|
||||
516
docs/MC-S-002_点到点运动（PTP）设计方案4.md
Normal file
516
docs/MC-S-002_点到点运动（PTP）设计方案4.md
Normal file
@@ -0,0 +1,516 @@
|
||||
# MC-S-002\_点到点运动（PTP）设计方案
|
||||
|
||||
# 双臂运动控制设计文档（MoveIt 规划 + Action 下发）
|
||||
|
||||
| 版本 | 日期 | 作者 | 描述 |
|
||||
| --- | --- | --- | --- |
|
||||
| v1.0.0 | 2026/02/25 | Ray | 文档新建 |
|
||||
| v1.0.1 | 2026/02/28 | Ray | 完善对外接口部分 |
|
||||
|
||||
## 1. 概述
|
||||
|
||||
机械臂需要快速、平滑地从一个位置运动到另一个位置。点到点运动（PTP）在关节空间进行规划：基于 **MoveIt 2** 生成轨迹，并通过左右臂各自的 `**FollowJointTrajectory**` action 下发到 **ros2\_control** 的 `JointTrajectoryController` 执行。目前支持单臂运动和双臂协同PTP运动类型。
|
||||
|
||||
## 2. 功能需求
|
||||
|
||||
* 在PTP运动规划过程中，系统应生成平滑的运动轨迹，速度曲线，加速度曲线在所有时间点连续，无阶跃。
|
||||
|
||||
* 在PTP运动执行过程中，系统应在轨迹规划失败时返回详细的错误原因，帮助上层应用诊断问题。
|
||||
|
||||
* 在PTP执行过程中，系统应在运动过程中检测到急停或错误时，立即停止运动，返回中断状态。
|
||||
|
||||
* 在PTP执行过程中，系统应在执行运动前检测碰撞风险，发现碰撞风险时拒绝执行，返回碰撞信息。
|
||||
|
||||
* 在PTP运动执行过程中，系统应在执行运动前验证目标位置是否在工作空间内，超出工作空间时返回错误，不执行运动。
|
||||
|
||||
* 在PTP运动规划过程中，系统应在规定时间内完成轨迹规划，确保实时响应，规划时间 ≤ 50ms。
|
||||
|
||||
* 在PTP运动指令规划执行过程中，系统应严格控制实际运动速度，确保不超过设定的速度限制，支持速度比例参数（1%-100%），可动态调整
|
||||
|
||||
* 在PTP运动执行过程中，系统应在运动完成后通过Action Result通知上层应用，包含最终位置和状态信息，运动执行结果：SUCCESS（成功）、FAILED（失败）、CANCELLED（取消）。
|
||||
|
||||
* 在PTP运动规划中，系统应确保机器人到达目标位置时无振荡，到达目标后，位置波动范围 ≤ ±0.05mm。
|
||||
|
||||
### 2.1 状态反馈 SMART 需求（PTP）
|
||||
|
||||
| 编号 | SMART 需求 |
|
||||
| --- | --- |
|
||||
| PTP-FB-01 | 系统必须通过 ROS 2 话题持续发布运动状态，发布频率不低于 **100Hz**（目标值）；当前实现基线为 `robot_arm_status_period_ms=10~20ms`（50~100Hz），需通过参数与性能优化收敛到 100Hz 稳态。 |
|
||||
| PTP-FB-02 | 每次 PTP 任务结束（成功/失败/取消）时，必须通过 Action Result 返回 `success/final_progress/message`，并补充统一错误码语义（当前为 message 文本，后续结构化）。 |
|
||||
| PTP-FB-03 | 状态信息必须具备完整性：关节空间（名称/位置/使能/错误码）+ 双臂末端笛卡尔位姿；base 与腿末端笛卡尔状态作为扩展字段进入后续版本。 |
|
||||
| PTP-FB-04 | 状态消息必须携带准确时间戳并与系统时钟同步；跨 topic 对齐误差超过阈值时需上报诊断信息。 |
|
||||
| PTP-FB-05 | 状态话题需支持多订阅者并发消费（建议基线 10 个）；订阅者异常断开或慢消费不得阻塞发布与运动执行链路。 |
|
||||
|
||||
### 2.2 状态反馈验收映射（PTP）
|
||||
|
||||
| 验收项 | 对应需求 |
|
||||
| --- | --- |
|
||||
| 实时状态：发布频率 ≥100Hz | PTP-FB-01 |
|
||||
| 完成通知：Result 含成功/失败与错误信息 | PTP-FB-02 |
|
||||
| 数据完整性：关节空间 + 笛卡尔空间 | PTP-FB-03 |
|
||||
| 时间戳准确：与系统时钟同步 | PTP-FB-04 |
|
||||
| 多订阅者支持与断开不阻塞 | PTP-FB-05 |
|
||||
|
||||
|
||||
## 3. 依赖
|
||||
|
||||
* **ROS 2 / rclcpp**
|
||||
|
||||
* **MoveIt 2**：`move_group`、`MoveGroupInterface`
|
||||
|
||||
* **ros2\_control**：`controller_manager`、`JointTrajectoryController`
|
||||
|
||||
* **控制接口**：`FollowJointTrajectory` action
|
||||
|
||||
|
||||
---
|
||||
|
||||
## 4. 内部结构设计（按当前代码）
|
||||
|
||||
### 4.1 总体架构
|
||||
|
||||
```mermaid
|
||||
flowchart LR
|
||||
subgraph App[上层应用]
|
||||
A1[DualArm Action Client]
|
||||
A2["Service Client<br/>IK/Enable/ClearError"]
|
||||
A3[Status Subscriber]
|
||||
end
|
||||
|
||||
subgraph RC[robot_control]
|
||||
N[robot_control_node]
|
||||
AM[ActionManager]
|
||||
DA[DualArmAction]
|
||||
RCM[RobotControlManager]
|
||||
AC[ArmControl]
|
||||
KS[KinematicsService]
|
||||
AHS[ArmHardwareService]
|
||||
ASS[ArmStatusService]
|
||||
end
|
||||
|
||||
subgraph MoveIt[MoveIt 2]
|
||||
MGI["MoveGroupInterface<br/>arm_left/arm_right/dual_arm"]
|
||||
RS[RobotState IK/FK]
|
||||
end
|
||||
|
||||
subgraph Ctrl[ros2_control]
|
||||
L[left_arm_controller]
|
||||
R[right_arm_controller]
|
||||
G1[left_arm_gpio_controller]
|
||||
G2[right_arm_gpio_controller]
|
||||
end
|
||||
|
||||
A1 --> DA
|
||||
A2 --> KS
|
||||
A2 --> AHS
|
||||
A3 --> ASS
|
||||
|
||||
N --> AM --> DA
|
||||
DA --> RCM --> AC --> MGI
|
||||
KS --> RCM --> AC --> RS
|
||||
AHS --> G1
|
||||
AHS --> G2
|
||||
ASS --> RCM
|
||||
DA --> L
|
||||
DA --> R
|
||||
```
|
||||
|
||||
### 4.2 分层职责
|
||||
|
||||
#### `robot_control_node`（编排层）
|
||||
|
||||
- 负责节点生命周期、订阅/发布器/service/action server 的创建与绑定。
|
||||
- 保持“薄节点”原则，不承载具体业务算法。
|
||||
- 启动时激活左右臂控制器并初始化 MoveIt。
|
||||
|
||||
#### `ActionManager` + `DualArmAction`（PTP主链路）
|
||||
|
||||
- 对外提供 `DualArm` action。
|
||||
- 负责单臂/双臂 PTP 目标校验、规划调用、轨迹导出、拆分与并发下发。
|
||||
- 负责执行期反馈、取消处理和结果回传。
|
||||
|
||||
#### `RobotControlManager`（领域能力聚合）
|
||||
|
||||
- 聚合运动学、状态映射、控制器状态等核心能力。
|
||||
- 提供 `PlanArmMotion` / `PlanDualArmJointMotion`、轨迹导出、关节状态查询等统一接口。
|
||||
- 提供 IK 与 FK 路径：
|
||||
- IK：调用 `ArmControl::SolveInverseKinematics`（纯 IK，`setFromIK`）
|
||||
- FK：使用当前 `jointPositions_` 计算末端位姿。
|
||||
|
||||
#### `ArmControl`（MoveIt 适配层）
|
||||
|
||||
- 管理 `arm_left` / `arm_right` / `dual_arm` 的 `MoveGroupInterface`。
|
||||
- 轨迹规划、时间参数化、轨迹缓存导出。
|
||||
- 纯 IK 与基于给定关节值的 FK 计算。
|
||||
|
||||
#### 新增服务层（`services/`）
|
||||
|
||||
- `KinematicsService`：对外逆解服务入口，负责请求/响应封装与错误语义。
|
||||
- `ArmHardwareService`：对外使能与清错服务，负责 GPIO 命令下发和缓存更新。
|
||||
- `ArmStatusService`：解析 `DynamicJointState`，定时发布 `RobotArmStatus`。
|
||||
|
||||
> 结论：第4版设计从“节点内聚”调整为“节点编排 + 业务服务分层”，便于维护与扩展。
|
||||
|
||||
---
|
||||
|
||||
## 5. 配置项设计
|
||||
|
||||
### 5.1 控制器与MoveIt配置
|
||||
|
||||
- `left_arm_controller`、`right_arm_controller`：6轴 `JointTrajectoryController`。
|
||||
- `controller_manager/switch_controller`：启动时由节点激活。
|
||||
- MoveIt 使用三个 group：`arm_left`、`arm_right`、`dual_arm`。
|
||||
|
||||
### 5.2 关键运行参数
|
||||
|
||||
- `robot_arm_status_period_ms`：状态发布周期，范围 `[10, 20]` ms，默认 `20` ms。
|
||||
- `velocity_scaling`：Action 目标速度比例（兼容 1~100 输入语义）。
|
||||
- 状态反馈目标频率：`>=100Hz`（建议将周期参数收敛到 `10ms` 并验证 CPU 余量）。
|
||||
|
||||
### 5.3 主题与服务约定
|
||||
|
||||
- GPIO命令：
|
||||
- `/left_arm_gpio_controller/commands`
|
||||
- `/right_arm_gpio_controller/commands`
|
||||
- 状态输入：
|
||||
- `/joint_states`
|
||||
- `/dynamic_joint_states`
|
||||
- 状态输出：
|
||||
- `/robot_control/arm_status`
|
||||
- 对外服务：
|
||||
- `/robot_control/inverse_kinematics`
|
||||
- `/robot_control/set_arm_enable`
|
||||
- `/robot_control/clear_arm_error`
|
||||
- `/robot_control/reset_estop`
|
||||
|
||||
---
|
||||
|
||||
## 6. 接口定义（当前实现）
|
||||
|
||||
### 6.1 PTP执行接口（Action）
|
||||
|
||||
- `interfaces/action/DualArm`（当前代码）
|
||||
|
||||
```plaintext
|
||||
# DualArm.action
|
||||
|
||||
# 状态常量定义
|
||||
uint8 STATUS_PLANNING = 0 # 状态: 规划中
|
||||
uint8 STATUS_EXECUTING = 1 # 状态: 执行中
|
||||
uint8 STATUS_DONE = 2 # 状态: 完成
|
||||
|
||||
# Action Goal: 双臂控制目标
|
||||
interfaces/ArmMotionParams[] arm_motion_params # 每条手臂的运动参数 (包含目标位姿、轨迹等)
|
||||
int32 velocity_scaling # 速度百分比系数 [1, 100] (100表示全速)
|
||||
|
||||
---
|
||||
|
||||
# Action Result: 执行结果
|
||||
bool success # 执行是否成功 (true=成功, false=失败)
|
||||
string message # 结果描述信息
|
||||
float64 final_progress # 最终进度 [0.0, 1.0]
|
||||
|
||||
---
|
||||
|
||||
# Action Feedback: 实时反馈
|
||||
float64 progress # 实时总体进度 [0.0, 1.0]
|
||||
uint8 status # 实时状态 (0:规划中, 1:执行中, 2:完成)
|
||||
float64[] joints_left # 左臂当前关节角 [单位: 弧度]
|
||||
float64[] joints_right # 右臂当前关节角 [单位: 弧度]
|
||||
```
|
||||
|
||||
### 6.2 运动学与硬件服务
|
||||
|
||||
- `interfaces/srv/InverseKinematics`（当前代码）
|
||||
|
||||
```plaintext
|
||||
int32 arm_id #0-左臂，1-右臂
|
||||
geometry_msgs/Pose pose #目标位姿
|
||||
---
|
||||
int32 result #结果：0-成功，非0-失败
|
||||
float32[] joint_angles #解算出来的关节角
|
||||
```
|
||||
|
||||
- `interfaces/srv/SetArmEnable`（当前代码）
|
||||
|
||||
```plaintext
|
||||
# SetArmEnable.srv
|
||||
# 机械臂关节上下使能服务
|
||||
#
|
||||
# 用途：
|
||||
# - 对单关节进行使能/失能
|
||||
# - 对单臂全部关节进行使能/失能
|
||||
# - 对双臂全部关节（或同编号关节）进行使能/失能
|
||||
#
|
||||
# 参数组合说明：
|
||||
# - arm_id=0, joint_num=0: 左臂全部关节
|
||||
# - arm_id=1, joint_num=0: 右臂全部关节
|
||||
# - arm_id=2, joint_num=0: 双臂全部关节
|
||||
# - arm_id=0/1, joint_num=1~6: 指定单臂的指定关节
|
||||
# - arm_id=2, joint_num=1~6: 双臂的同编号关节（如左右第3关节）
|
||||
#
|
||||
# 注意：
|
||||
# - joint_num=0 表示“该作用范围内的全部关节”
|
||||
# - 关节编号按 1 开始计数
|
||||
|
||||
int8 arm_id # 机械臂ID: 0=左臂, 1=右臂, 2=双臂
|
||||
int8 joint_num # 关节编号: 1~6=指定关节, 0=全部关节
|
||||
bool enable # 目标状态: true=使能, false=失能
|
||||
---
|
||||
bool success # true=操作成功, false=操作失败
|
||||
string message # 执行结果描述（失败原因/调试信息）
|
||||
```
|
||||
|
||||
- `interfaces/srv/ClearArmError`（当前代码）
|
||||
|
||||
```plaintext
|
||||
# ClearArmError.srv
|
||||
# 清除机械臂错误服务
|
||||
|
||||
int8 arm_id # 机械臂ID (例如: 0=左臂, 1=右臂, 2=双臂)
|
||||
int8 joint_num # 关节编号 (1~7表示第1个到第7个关节, 0表示所有关节)
|
||||
---
|
||||
bool success # 操作是否成功
|
||||
string message # 结果信息
|
||||
```
|
||||
|
||||
- `interfaces/srv/ResetEStop`（当前代码）
|
||||
|
||||
```plaintext
|
||||
# ResetEStop.srv
|
||||
# 急停复位服务（用于清除上层软件锁存态）
|
||||
#
|
||||
# 注意：
|
||||
# - 该服务仅表达“复位请求”，是否允许复位由安全链路策略决定
|
||||
# - 硬件安全回路未恢复时，应返回 success=false
|
||||
|
||||
bool reset # true=请求复位，false=无效请求
|
||||
---
|
||||
bool success # true=复位成功，false=复位失败
|
||||
string message # 结果描述（失败原因/诊断信息）
|
||||
```
|
||||
|
||||
### 6.3 状态发布接口
|
||||
|
||||
- `interfaces/msg/RobotArmStatus`（topic: `/robot_control/arm_status`，当前代码）
|
||||
|
||||
```plaintext
|
||||
builtin_interfaces/Time stamp
|
||||
string[] joint_names
|
||||
float64[] joint_positions
|
||||
bool[] joint_enabled
|
||||
int32[] joint_error_codes
|
||||
bool estop_latched
|
||||
uint32 estop_generation
|
||||
geometry_msgs/Pose left_arm_pose
|
||||
geometry_msgs/Pose right_arm_pose
|
||||
```
|
||||
|
||||
> 末端位姿采用 `jointPositions_ + FK` 计算，确保与同帧关节值一致。
|
||||
> 当前版本对外稳定字段为“关节空间 + 双臂末端位姿”；base 与腿末端笛卡尔状态定义为下一版本扩展项，以保持接口兼容演进。
|
||||
|
||||
---
|
||||
|
||||
## 7. 行为与流程设计
|
||||
|
||||
### 7.1 单臂 PTP 主流程
|
||||
|
||||
```mermaid
|
||||
sequenceDiagram
|
||||
participant C as Client
|
||||
participant DA as DualArmAction
|
||||
participant RCM as RobotControlManager
|
||||
participant AC as ArmControl
|
||||
participant FJT as JointTrajectoryController
|
||||
|
||||
C->>DA: Goal(单臂MOVEJ)
|
||||
DA->>DA: 参数/状态校验
|
||||
DA->>RCM: PlanArmMotion()
|
||||
RCM->>AC: PlanJointMotion()
|
||||
AC-->>RCM: 规划轨迹缓存
|
||||
DA->>RCM: ExportArmPlannedTrajectory()
|
||||
DA->>FJT: async_send_goal()
|
||||
FJT-->>DA: result/feedback
|
||||
DA-->>C: result
|
||||
```
|
||||
|
||||
### 7.2 双臂 PTP 主流程
|
||||
|
||||
```mermaid
|
||||
sequenceDiagram
|
||||
participant C as Client
|
||||
participant DA as DualArmAction
|
||||
participant RCM as RobotControlManager
|
||||
participant AC as ArmControl(dual_arm)
|
||||
participant L as left FJT
|
||||
participant R as right FJT
|
||||
|
||||
C->>DA: Goal(双臂)
|
||||
DA->>RCM: PlanDualArmJointMotion()
|
||||
RCM->>AC: PlanJointMotion(arm_id=2)
|
||||
AC-->>RCM: 12轴轨迹
|
||||
DA->>RCM: ExportDualArmPlannedTrajectory()
|
||||
DA->>DA: 拆分左右轨迹
|
||||
par left
|
||||
DA->>L: send goal
|
||||
and right
|
||||
DA->>R: send goal
|
||||
end
|
||||
L-->>DA: result
|
||||
R-->>DA: result
|
||||
DA-->>C: result
|
||||
```
|
||||
|
||||
### 7.3 服务链路流程
|
||||
|
||||
- `InverseKinematics`：`KinematicsService -> RobotControlManager -> ArmControl(setFromIK)`
|
||||
- `SetArmEnable/ClearArmError`：`ArmHardwareService -> GPIO controllers`
|
||||
- `RobotArmStatus`：`ArmStatusService` 定时汇总发布（状态输入来自 `joint_states` + `dynamic_joint_states`）
|
||||
|
||||
---
|
||||
|
||||
## 8. 时序与性能设计
|
||||
|
||||
### 8.1 PTP链路性能目标
|
||||
|
||||
- 规划阶段：以“稳定优先”，不再文档强约束固定 50ms。
|
||||
- 执行反馈：依赖 action feedback 周期与控制器状态更新频率。
|
||||
- 状态发布：10~20ms 周期（参数可调，默认20ms）。
|
||||
|
||||
### 8.2 时序一致性策略
|
||||
|
||||
- 关节位置：来自 `joint_states` 映射后的 `jointPositions_`。
|
||||
- 末端位姿：基于同一时刻 `jointPositions_` 进行 FK。
|
||||
- 使能/错误：优先由 `dynamic_joint_states` 更新，服务操作会同步更新缓存。
|
||||
|
||||
### 8.3 取消与停止
|
||||
|
||||
- 取消沿用 `DualArmAction` 既有策略（含软停逻辑，按当前实现执行）。
|
||||
- 硬急停状态机作为后续增强项，不在本次服务化重构内改变执行协议。
|
||||
|
||||
### 8.4 急停分层链路与状态机
|
||||
|
||||
#### 分层链路（与当前代码对齐）
|
||||
|
||||
```mermaid
|
||||
flowchart LR
|
||||
IO[外部急停IO] --> HAL[HAL安全输入]
|
||||
HAL --> Driver[驱动层安全状态]
|
||||
Driver --> Topic["/safety/estop_state"]
|
||||
Topic --> SS[SafetyService]
|
||||
SS --> DA[DualArmAction]
|
||||
SS --> ASS[ArmStatusService]
|
||||
DA --> Stop1["Stop1减速停车<br/>MakeStopTrajectory + cancel"]
|
||||
ASS --> Status["/robot_control/arm_status<br/>estop_latched/generation"]
|
||||
```
|
||||
|
||||
- `SafetyService` 订阅 `/safety/estop_state`，维护 `latched/generation` 原子状态。
|
||||
- `SafetyService` 提供 `/robot_control/reset_estop`：
|
||||
- 必须已收到至少一次 `/safety/estop_state`（完成驱动侧状态同步）；
|
||||
- 且当前 `latched=false`（硬件链路已释放）；
|
||||
- 否则返回 `success=false`，拒绝复位请求。
|
||||
- `DualArmAction` 在 **接收目标前** 与 **执行循环中** 检查急停锁存：
|
||||
- 锁存态下拒绝新目标；
|
||||
- 执行中触发时，执行 Stop1 风格减速停车并返回中断结果（`E_STOP_STOP1`）。
|
||||
- `ArmStatusService` 将 `estop_latched/estop_generation` 合并进 `RobotArmStatus`，便于上层统一观测执行态与安全态。
|
||||
|
||||
#### 急停状态机（软件视角）
|
||||
|
||||
```mermaid
|
||||
stateDiagram-v2
|
||||
[*] --> RUNNING
|
||||
RUNNING --> E_STOP_LATCHED: estop_latched=true
|
||||
E_STOP_LATCHED --> STOPPING: 执行中检测到急停
|
||||
STOPPING --> ABORTED: Stop1完成/Action中止
|
||||
E_STOP_LATCHED --> REJECT_NEW_GOAL: 新目标到达
|
||||
REJECT_NEW_GOAL --> E_STOP_LATCHED
|
||||
ABORTED --> WAIT_RESET: 等待安全链路复位
|
||||
WAIT_RESET --> RUNNING: latched=false 且允许复位
|
||||
```
|
||||
|
||||
- `generation` 用于区分“同一锁存态”与“新触发事件”，上层可据此做边沿判定与告警去重。
|
||||
- `ResetEStop` 服务仅作为复位请求入口，复位成功前提是“驱动状态已同步 + 锁存已释放”，不允许绕过硬件安全链路。
|
||||
|
||||
---
|
||||
|
||||
## 9. 错误处理与恢复策略
|
||||
|
||||
### 9.1 错误分类
|
||||
|
||||
- 请求参数错误：`arm_id`/`joint_num` 越界、目标维度不匹配。
|
||||
- 状态错误：未初始化完成、控制器不可用、资源冲突。
|
||||
- 规划错误：MoveIt 无解/规划失败。
|
||||
- 执行错误：FJT 执行失败、超时、被取消。
|
||||
|
||||
### 9.2 上报语义
|
||||
|
||||
- Action：`result.success=false + message`。
|
||||
- Service：`success/result + message` 明确失败原因。
|
||||
- 日志：在 service/action 层保留关键错误日志，便于定位。
|
||||
|
||||
### 9.3 恢复策略
|
||||
|
||||
- 参数错误：立即失败返回，不进入执行链路。
|
||||
- 规划失败：保持控制器状态不变，允许重试。
|
||||
- 执行失败：由上层决定重试、回零或清错。
|
||||
- 清错与使能：通过 service 明确触发，避免隐式副作用。
|
||||
|
||||
---
|
||||
|
||||
## 10. 可观测性与调试支持
|
||||
|
||||
- 统一状态topic：`/robot_control/arm_status` 作为运维与调试主观测口。
|
||||
- 建议录包：`joint_states`、`dynamic_joint_states`、`robot_control/arm_status`、Action feedback/result。
|
||||
- 调试逆解：调用 `/robot_control/inverse_kinematics` 并与 FK结果做闭环比对。
|
||||
- 建议新增监控项：状态发布时间抖动、Action Result 延迟、订阅者数量与发布阻塞告警。
|
||||
|
||||
---
|
||||
|
||||
## 11. 安全性与可靠性
|
||||
|
||||
### 11.1 安全性
|
||||
|
||||
- 关节限位在规划入口前校验。
|
||||
- 使能/清错接口显式化，避免业务层绕过硬件控制语义。
|
||||
- 双臂/单臂关节范围通过 `arm_id + joint_num` 严格限定。
|
||||
|
||||
### 11.2 可靠性
|
||||
|
||||
- 节点瘦身 + 服务分层后，修改影响面更小。
|
||||
- 状态发布与执行链路解耦，降低互相阻塞风险。
|
||||
- IK 与 FK 语义清晰，避免“规划末点冒充逆解”的歧义。
|
||||
|
||||
---
|
||||
|
||||
## 12. 测试设计
|
||||
|
||||
### 12.1 功能测试
|
||||
|
||||
- 单臂/双臂 PTP 成功路径（MOVEJ）。
|
||||
- `SetArmEnable`：
|
||||
- `arm_id=0/1/2`
|
||||
- `joint_num=0`（全部）与 `1~6`（单关节）
|
||||
- `ClearArmError`：单关节、单臂全部、双臂全部。
|
||||
- `InverseKinematics`：左/右臂典型位姿、不可达位姿。
|
||||
|
||||
### 12.2 一致性测试
|
||||
|
||||
- 校验 `RobotArmStatus.joint_positions` 与实际 `joint_states` 对齐。
|
||||
- 校验 `RobotArmStatus` 中 pose 是否与同帧关节值 FK 一致。
|
||||
- 校验 service 调用后使能/错误缓存刷新行为。
|
||||
|
||||
### 12.3 稳定性测试
|
||||
|
||||
- 连续PTP请求压力（含取消）。
|
||||
- 状态topic长时间发布（10ms 周期）稳定性与CPU占用。
|
||||
- 控制器重启后的恢复行为（switch_controller + service/action 重新可用）。
|
||||
- 多订阅者压力（建议 10+ 并发订阅）与慢订阅者/断开场景不阻塞验证。
|
||||
|
||||
---
|
||||
|
||||
## 13. 变更影响分析
|
||||
|
||||
- **架构层面**：从“node内聚实现”升级为“service分层实现”，可维护性提升。
|
||||
- **兼容性**：对外 action/service/topic 名称保持不变，调用方无感。
|
||||
- **性能影响**：新增服务对象几乎无额外开销；状态发布逻辑迁移不改变频率与数据结构。
|
||||
- **风险点**：需关注缓存一致性（dynamic state 与 service写入并存场景）；建议通过录包回归验证。
|
||||
362
docs/MC-S-003-直线运动（LIN）设计方案.md
Normal file
362
docs/MC-S-003-直线运动（LIN）设计方案.md
Normal file
@@ -0,0 +1,362 @@
|
||||
# MC-S-003_直线运动（LIN）设计方案
|
||||
|
||||
# 双臂 LIN 运动控制设计文档（当前实现）
|
||||
|
||||
| 版本 | 日期 | 作者 | 描述 |
|
||||
| --- | --- | --- | --- |
|
||||
| v1.0.0 | 2026/03/03 | Ray | 初版，基于当前代码实现 |
|
||||
|
||||
## 1. 概述
|
||||
|
||||
机械臂直线运动（LIN）用于要求末端执行器沿笛卡尔空间直线路径到达目标位姿的场景。
|
||||
当前系统在 `DualArm.action` 中通过 `ArmMotionParams.motion_type = MOVEL` 表示 LIN 请求。
|
||||
|
||||
当前实现目标：
|
||||
|
||||
- 单臂 LIN：基于 MoveIt `computeCartesianPath` 生成直线路径；
|
||||
- 双臂 LIN：左右臂分别规划后做统一时间轴同步，保证同起同止；
|
||||
- 兼容现有执行链路：左右臂 `FollowJointTrajectory` 下发。
|
||||
|
||||
---
|
||||
|
||||
## 2. 用户故事与 SMART 需求
|
||||
|
||||
### 2.1 用户故事（场景描述）
|
||||
|
||||
1. 上层应用需要机械臂末端沿直线路径运动到目标位置。
|
||||
2. LIN 运动在笛卡尔空间规划，确保末端轨迹为直线。
|
||||
3. 姿态可保持固定或做平滑插值。
|
||||
4. 典型应用包括焊接、涂胶、装配等对路径精度敏感的任务。
|
||||
5. 系统需支持速度比例可调，并提供参数配置与逆解等服务能力。
|
||||
|
||||
### 2.2 SMART 需求条目
|
||||
|
||||
| 编号 | SMART 需求 |
|
||||
| --- | --- |
|
||||
| LIN-SR-01 | 系统在接收 `MOVEL` 目标后，**50ms 内**必须完成“成功规划”或“失败返回”；失败时返回结构化错误信息（`code + message + stage`）。 |
|
||||
| LIN-SR-02 | 单臂 LIN 规划必须使用笛卡尔直线路径生成机制（`computeCartesianPath`），且路径完整度 `fraction >= 0.999` 才允许进入执行阶段。 |
|
||||
| LIN-SR-03 | 系统必须支持两种姿态策略：`FIXED`（姿态保持）与 `SLERP`（四元数球面插值）；每条 LIN 指令必须可选其一。 |
|
||||
| LIN-SR-04 | 在 `SLERP` 模式下，姿态轨迹应连续无突变；相邻采样点不得出现翻转跳变（按四元数同向性约束处理）。 |
|
||||
| LIN-SR-05 | 执行期末端实际速度不得超过设定笛卡尔速度上限；当超限持续超过设定窗口（建议 100ms）时，系统必须停止并上报。 |
|
||||
| LIN-SR-06 | 系统必须支持 `velocity_scaling` 在 `1%~100%` 可调，并保证速度上限按比例缩放，缩放行为在当前任务内生效。 |
|
||||
| LIN-SR-07 | 到达目标后不得出现超调振荡；在到位判定窗口内（建议 500ms）末端位置应稳定在允许误差带内。 |
|
||||
| LIN-SR-08 | 若直线路径上任一点 IK 无解，规划阶段必须立即失败并返回错误（例如 `IK_NO_SOLUTION_ON_PATH`），不得下发执行轨迹。 |
|
||||
| LIN-SR-09 | 若规划起始点处于奇异区，系统必须拒绝规划并返回错误（例如 `START_SINGULARITY`）。 |
|
||||
| LIN-SR-10 | 若执行过程中检测到路径进入奇异区，系统必须停止当前任务并返回错误（例如 `EXEC_SINGULARITY_ABORT`）。 |
|
||||
| LIN-SR-11 | 双臂 LIN 必须使用统一时间轴；左右臂开始与结束偏差应满足同步阈值（建议 ≤20ms）。 |
|
||||
| LIN-SR-12 | 系统必须提供 LIN 参数配置服务与逆解服务，支持在线读取/更新关键参数（具体参数集可后续冻结）。 |
|
||||
|
||||
### 2.3 验收标准映射
|
||||
|
||||
| 验收项 | 对应需求 |
|
||||
| --- | --- |
|
||||
| 姿态平滑（SLERP，无突变） | LIN-SR-03, LIN-SR-04 |
|
||||
| 速度控制（不超过笛卡尔上限） | LIN-SR-05, LIN-SR-06 |
|
||||
| 位置精度与无超调振荡 | LIN-SR-07 |
|
||||
| 规划时间 ≤ 50ms | LIN-SR-01 |
|
||||
| 路径点 IK 不可达返回错误 | LIN-SR-08 |
|
||||
| 起始点奇异返回错误 | LIN-SR-09 |
|
||||
| 执行中经过奇异点返回错误并停止 | LIN-SR-10 |
|
||||
| 速度比例可调 | LIN-SR-06 |
|
||||
| 参数配置服务/逆解服务 | LIN-SR-12 |
|
||||
|
||||
### 2.4 状态反馈用户故事（场景描述）
|
||||
|
||||
1. 上层应用需要实时了解机械臂运动状态，包括当前位置、速度、运动进度等。
|
||||
2. 状态反馈通过 ROS 2 话题实时发布，并支持多个订阅者同时订阅。
|
||||
3. 运动完成后通过 Action Result 通知执行结果和最终状态。
|
||||
4. 适用于在线监控、任务编排、异常追踪等场景。
|
||||
|
||||
### 2.5 状态反馈 SMART 需求条目
|
||||
|
||||
| 编号 | SMART 需求 |
|
||||
| --- | --- |
|
||||
| LIN-FB-01 | 系统必须通过 ROS 2 话题持续发布运动状态，发布频率不低于 **100Hz**；消息需包含关节空间与笛卡尔空间状态字段（手臂末端、base、腿末端），以及运动类型、运动状态与进度。 |
|
||||
| LIN-FB-02 | 每次 LIN 任务结束（成功/失败/取消）时，系统必须在 Action Result 返回执行结果、错误码和最终状态摘要；Result 发送延迟应小于 **100ms**（相对任务终止时刻）。 |
|
||||
| LIN-FB-03 | 系统必须支持多订阅者并发订阅状态话题（建议基线 10 个），且任一订阅者断开或消费变慢时，发布端持续发布、不阻塞控制链路。 |
|
||||
|
||||
### 2.6 状态反馈验收标准映射
|
||||
|
||||
| 验收项 | 对应需求 |
|
||||
| --- | --- |
|
||||
| 实时状态：发布频率 ≥100Hz | LIN-FB-01 |
|
||||
| 完成通知：Result 含成功/失败状态与错误码 | LIN-FB-02 |
|
||||
| 数据完整性：包含关节空间 + 笛卡尔空间字段 | LIN-FB-01 |
|
||||
| 时间戳准确：与系统时钟同步 | LIN-FB-01 |
|
||||
| 多订阅者支持：并发订阅不影响性能 | LIN-FB-03 |
|
||||
| 订阅者断开不阻塞系统 | LIN-FB-03 |
|
||||
|
||||
### 2.7 PTP/LIN 状态反馈统一需求矩阵
|
||||
|
||||
| 维度 | PTP（MC-S-002） | LIN（本方案） | 统一结论 |
|
||||
| --- | --- | --- | --- |
|
||||
| 发布通道 | `/robot_control/arm_status` + Action feedback/result | `/robot_control/arm_status` + Action feedback/result | 统一 |
|
||||
| 发布频率目标 | >=100Hz（目标），当前参数 10~20ms | >=100Hz（目标） | 统一为 >=100Hz，按性能分阶段收敛 |
|
||||
| 数据范围 | 关节空间 + 双臂末端位姿（当前） | 关节空间 + 笛卡尔状态（当前重点手臂） | 统一扩展到“关节+手臂/base/腿末端” |
|
||||
| 完成通知 | Action Result（success/message/final_progress） | Action Result（success/message/final_progress） | 统一错误码语义（后续结构化） |
|
||||
| 多订阅者鲁棒性 | 设计要求支持并发与断开不阻塞 | 设计要求支持并发与断开不阻塞 | 统一压测基线 10+ 订阅者 |
|
||||
| 时间戳一致性 | 需与系统时钟对齐 | 需与系统时钟对齐 | 统一校验策略 |
|
||||
|
||||
---
|
||||
|
||||
## 3. 依赖
|
||||
|
||||
- **ROS 2 / rclcpp**
|
||||
- **MoveIt 2**：`MoveGroupInterface`、`computeCartesianPath`
|
||||
- **ros2_control**：`JointTrajectoryController`
|
||||
- **control_msgs/action/FollowJointTrajectory**
|
||||
- **interfaces/action/DualArm**
|
||||
- **interfaces/msg/ArmMotionParams**
|
||||
|
||||
---
|
||||
|
||||
## 4. 内部结构设计（按当前代码）
|
||||
|
||||
### 4.1 总体架构
|
||||
|
||||
```mermaid
|
||||
flowchart LR
|
||||
Client[上层应用<br/>DualArm Action Client] --> DA[DualArmAction]
|
||||
DA --> RCM[RobotControlManager]
|
||||
RCM --> AC[ArmControl]
|
||||
AC --> MGI_L[MoveGroup arm_left]
|
||||
AC --> MGI_R[MoveGroup arm_right]
|
||||
DA --> TU[trajectory_utils]
|
||||
DA --> LFJT[left_arm_controller FJT]
|
||||
DA --> RFJT[right_arm_controller FJT]
|
||||
```
|
||||
|
||||
### 4.2 分层职责
|
||||
|
||||
#### `DualArmAction`（业务编排层）
|
||||
|
||||
- LIN 目标校验与执行状态机；
|
||||
- 单臂/双臂分支调度；
|
||||
- 双臂 LIN 同步重采样调用；
|
||||
- FJT 下发、反馈发布、取消和异常处理。
|
||||
|
||||
#### `RobotControlManager`（领域能力聚合）
|
||||
|
||||
- 提供 `PlanArmMotion` / 轨迹导出等统一能力；
|
||||
- 将 `MOVEL` 请求路由至 `ArmControl::PlanCartesianMotion`。
|
||||
|
||||
#### `ArmControl`（MoveIt 适配层）
|
||||
|
||||
- `PlanCartesianMotion` 调用 `computeCartesianPath`；
|
||||
- 轨迹时间参数化并写入缓存；
|
||||
- 导出单臂轨迹供执行层使用。
|
||||
|
||||
#### `trajectory_utils`（轨迹工具层）
|
||||
|
||||
- 起点时间修正；
|
||||
- 双臂时间轴同步重采样；
|
||||
- S 型进度映射；
|
||||
- 速度/加速度回填。
|
||||
|
||||
---
|
||||
|
||||
## 5. 配置项设计（LIN 相关）
|
||||
|
||||
### 5.1 同步重采样参数
|
||||
|
||||
| 参数名 | 默认值 | 说明 |
|
||||
| --- | --- | --- |
|
||||
| `dual_arm.movel_sync.use_s_curve_progress` | `true` | 是否启用 S 型进度映射 |
|
||||
| `dual_arm.movel_sync.min_dt_sec` | `0.01` | 同步轨迹采样的最小时间分辨率 |
|
||||
| `dual_arm.movel_sync.max_samples` | `500` | 同步轨迹最大采样点数 |
|
||||
|
||||
### 5.2 轨迹执行参数
|
||||
|
||||
- `velocity_scaling`：来自 `DualArm.goal`（1~100 映射为 [0,1]）；
|
||||
- `acceleration_scaling`：当前执行链路固定使用内部配置值（与 PTP 一致）。
|
||||
|
||||
---
|
||||
|
||||
## 6. 接口定义（当前实现）
|
||||
|
||||
### 6.1 LIN 目标定义
|
||||
|
||||
```plaintext
|
||||
# ArmMotionParams.msg (节选)
|
||||
uint8 MOVEJ = 0
|
||||
uint8 MOVEL = 1
|
||||
uint8 arm_id
|
||||
uint8 motion_type
|
||||
geometry_msgs/Pose target_pose
|
||||
float64[] target_joints
|
||||
```
|
||||
|
||||
说明：
|
||||
|
||||
- `MOVEL` 仅使用 `target_pose`；
|
||||
- `MOVEJ` 仅使用 `target_joints`。
|
||||
|
||||
### 6.2 执行入口
|
||||
|
||||
- 统一通过 `DualArm.action` 触发；
|
||||
- 单臂：`arm_motion_params.size() == 1`；
|
||||
- 双臂：`arm_motion_params.size() == 2`（需包含 left/right）。
|
||||
|
||||
---
|
||||
|
||||
## 7. LIN 行为与流程设计
|
||||
|
||||
### 7.1 单臂 LIN 规划流程
|
||||
|
||||
```mermaid
|
||||
sequenceDiagram
|
||||
participant C as Client
|
||||
participant DA as DualArmAction
|
||||
participant RCM as RobotControlManager
|
||||
participant AC as ArmControl
|
||||
participant FJT as Arm FJT
|
||||
|
||||
C->>DA: Goal(单臂, MOVEL)
|
||||
DA->>RCM: PlanArmMotion(MOVEL)
|
||||
RCM->>AC: PlanCartesianMotion
|
||||
AC->>AC: computeCartesianPath(current->target)
|
||||
AC-->>RCM: 轨迹缓存
|
||||
RCM-->>DA: ExportArmPlannedTrajectory
|
||||
DA->>FJT: async_send_goal(traj)
|
||||
FJT-->>DA: result
|
||||
DA-->>C: Action Result
|
||||
```
|
||||
|
||||
### 7.2 双臂 LIN 规划与同步流程
|
||||
|
||||
```mermaid
|
||||
sequenceDiagram
|
||||
participant C as Client
|
||||
participant DA as DualArmAction
|
||||
participant RCM as RobotControlManager
|
||||
participant TU as trajectory_utils
|
||||
participant LFJT as Left FJT
|
||||
participant RFJT as Right FJT
|
||||
|
||||
C->>DA: Goal(双臂, 含MOVEL)
|
||||
DA->>RCM: PlanArmMotion(left)
|
||||
DA->>RCM: PlanArmMotion(right)
|
||||
DA->>RCM: ExportArmPlannedTrajectory(left/right)
|
||||
DA->>TU: synchronize_dual_arm_trajectory_timebase
|
||||
TU-->>DA: left_sync/right_sync
|
||||
DA->>LFJT: send left_sync
|
||||
DA->>RFJT: send right_sync
|
||||
LFJT-->>DA: result
|
||||
RFJT-->>DA: result
|
||||
DA-->>C: Action Result
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 8. 直线性与同步性设计说明
|
||||
|
||||
### 8.1 单臂直线性保证
|
||||
|
||||
单臂 `MOVEL` 直线性由 `computeCartesianPath` 保证，关键点：
|
||||
|
||||
- 使用当前位姿与目标位姿构造 waypoints；
|
||||
- 采用固定 `eef_step` 离散；
|
||||
- 要求 `fraction` 接近 1（当前阈值为 `>= 0.999`）。
|
||||
|
||||
### 8.2 双臂同步策略
|
||||
|
||||
当前双臂 LIN 不做“联合笛卡尔约束规划”，而是：
|
||||
|
||||
1. 左右臂各自直线规划；
|
||||
2. 统一时间轴重采样；
|
||||
3. 并发下发 FJT。
|
||||
|
||||
该策略可在保持各臂 LIN 路径特征的同时，提高双臂执行同步性。
|
||||
|
||||
### 8.3 平滑性策略
|
||||
|
||||
同步重采样时支持：
|
||||
|
||||
- S 型进度映射（quintic smoothstep）；
|
||||
- 重采样后回填 `vel/acc`；
|
||||
- 配合非零起点时间处理，降低起步冲击。
|
||||
|
||||
---
|
||||
|
||||
## 9. 取消、急停与异常处理
|
||||
|
||||
### 9.1 取消
|
||||
|
||||
- 检测到 cancel 请求后：
|
||||
- 发送减速/保持轨迹；
|
||||
- 取消当前 FJT goal；
|
||||
- Action 返回 canceled。
|
||||
|
||||
### 9.2 急停
|
||||
|
||||
- 执行循环内持续检查急停锁存；
|
||||
- 触发后立即停止并 abort。
|
||||
|
||||
### 9.3 规划失败与参数非法
|
||||
|
||||
- 无法生成笛卡尔路径；
|
||||
- 目标位姿非法（NaN/Inf/四元数异常）；
|
||||
- 导出轨迹失败；
|
||||
- 控制器不可用或 goal 被拒绝。
|
||||
均通过 Action Result 的 `success=false` 与 `message` 返回。
|
||||
|
||||
---
|
||||
|
||||
## 10. 可观测性与调试支持
|
||||
|
||||
- Action Feedback：
|
||||
- `status`：规划/执行/完成；
|
||||
- `progress`：执行进度估算值。
|
||||
- 统一状态 topic：
|
||||
- `/robot_control/arm_status` 提供关节空间与笛卡尔状态观测；
|
||||
- 建议与 `joint_states`、`dynamic_joint_states` 同步录包做一致性比对。
|
||||
- 轨迹文件导出：
|
||||
- 可保存左右臂下发轨迹（CSV）用于离线分析。
|
||||
- 关键日志：
|
||||
- LIN 规划成功率、同步重采样参数、FJT 接受/执行结果。
|
||||
- 状态发布频率、Action Result 延迟、多订阅者阻塞告警。
|
||||
|
||||
---
|
||||
|
||||
## 11. 测试设计
|
||||
|
||||
### 11.1 功能测试
|
||||
|
||||
- 左臂/右臂单臂 LIN 到达测试；
|
||||
- 双臂 LIN 同步执行测试；
|
||||
- `MOVEJ + MOVEL` 混合输入测试。
|
||||
|
||||
### 11.2 直线性测试
|
||||
|
||||
- 记录末端实际轨迹并做线段拟合；
|
||||
- 评估最大偏离与平均偏离。
|
||||
|
||||
### 11.3 同步性测试
|
||||
|
||||
- 比较左右臂完成时间差；
|
||||
- 比较相同归一化时刻的状态一致性。
|
||||
|
||||
### 11.4 稳定性测试
|
||||
|
||||
- 长时间连续 LIN；
|
||||
- 随机注入 cancel/急停；
|
||||
- 重复启动/停止场景回归。
|
||||
- 多订阅者并发 + 慢订阅者/断开场景下的非阻塞验证。
|
||||
|
||||
---
|
||||
|
||||
## 12. 边界与后续优化
|
||||
|
||||
### 12.1 当前边界
|
||||
|
||||
- 双臂 LIN 采用“单臂规划 + 同步重采样”，并非全局联合最优；
|
||||
- 时长差异较大时，快臂会被拉伸到慢臂时长；
|
||||
- 动态品质仍受控制器参数影响。
|
||||
|
||||
### 12.2 后续优化建议
|
||||
|
||||
- 引入双臂联合笛卡尔约束规划；
|
||||
- 增加在线同步偏差监控指标；
|
||||
- 将同步重采样参数纳入任务级可配置输入；
|
||||
- 增加 LIN 段间 blending 设计。
|
||||
|
||||
75
docs/TRAJECTORY_BLENDING.md
Normal file
75
docs/TRAJECTORY_BLENDING.md
Normal file
@@ -0,0 +1,75 @@
|
||||
# 两段轨迹之间的 Blending 实现说明
|
||||
|
||||
## 当前流程简述
|
||||
|
||||
在 `hivecore_robot_motion` 中，臂控流程为：
|
||||
|
||||
1. **规划**：通过 MoveIt（`PlanJointMotion` / `PlanCartesianMotion` / `PlanDualArmJointMotion`）得到一条关节轨迹。
|
||||
2. **时间参数化**：在 `ArmControl::timeParameterizeAndStore_` 中对轨迹做 S 型曲线或梯形时间参数化，得到带 `time_from_start`、`positions/velocities/accelerations` 的 `JointTrajectory`。
|
||||
3. **下发**：`DualArmAction` 将规划结果导出为 `JointTrajectory`，按左/右臂拆分为两条轨迹，通过 **FollowJointTrajectory (FJT)** action 分别发给 `left_arm_controller` 和 `right_arm_controller`。
|
||||
|
||||
目前每个 DualArm goal 只包含 **一条** 运动（单臂 1 个目标，双臂 2 个目标），不存在“两段轨迹”的概念，因此也没有段与段之间的 blending。
|
||||
|
||||
---
|
||||
|
||||
## 两段轨迹 Blending 的两种实现思路
|
||||
|
||||
### 思路一：在 motion 层合并两段轨迹再下发（推荐）
|
||||
|
||||
在应用层得到“两段”轨迹后，在 **下发前** 将两段轨迹在关节空间做平滑过渡（blend），合并成 **一条** `JointTrajectory`，再通过 FJT 一次性下发。这样：
|
||||
|
||||
- 不需要底层 FJT/controller 支持 blend；
|
||||
- 与现有“单条轨迹 → FJT”的流程一致；
|
||||
- 实现集中在 `robot_control` 内，易于维护。
|
||||
|
||||
**实现位置**：已提供工具函数 `BlendTwoTrajectories()`（见 `include/utils/trajectory_blend.hpp`），输入两段 `JointTrajectory` 和 `blend_ratio`，输出合并后的轨迹。
|
||||
|
||||
**如何得到“两段”轨迹**（需扩展规划/action）：
|
||||
|
||||
- **方式 A**：支持“从指定关节状态规划到目标”的接口（例如 `PlanJointMotionFromState(arm_id, start_joints, target_joints, ...)`），内部用 `move_group->setStartState(...)` 设置起点后再 `plan()`。
|
||||
- 第一段：当前状态 → 中间点（PlanJointMotion 现有逻辑）；
|
||||
- 第二段：中间点 → 终点（调用 PlanJointMotionFromState）；
|
||||
- 对两段结果调用 `BlendTwoTrajectories(seg1, seg2, blend_ratio, blend_point_count, &merged)`，再将 `merged` 导出并下发。
|
||||
- **方式 B**：扩展 DualArm goal，支持每个臂多个路点（如 `arm_motion_params` 变为多段，或新增 `waypoints` 字段）。对每个臂依次规划段 1、段 2、…，再按段顺序两两 blend 成一条轨迹后下发。
|
||||
|
||||
接口里已有 `ArmMotionParams.blend_radius`（0–100），可映射为 `BlendRadiusToRatio(blend_radius)` 作为 `blend_ratio` 传入 `BlendTwoTrajectories`。
|
||||
|
||||
### 思路二：控制器侧 Blending（若硬件/驱动支持）
|
||||
|
||||
若底层 FJT 或关节控制器支持“多段轨迹 + 每段 blend 半径”（类似部分工业机械臂），则可以在 **不合并轨迹** 的前提下，连续下发多段轨迹，由控制器在段与段之间做 blend。当前 ROS 2 常用的 `joint_trajectory_controller` **一般不**支持该能力，需要自定义 controller 或厂商驱动支持。
|
||||
|
||||
---
|
||||
|
||||
## 已提供的工具用法
|
||||
|
||||
### 1. `BlendTwoTrajectories`
|
||||
|
||||
- **声明**：`include/utils/trajectory_blend.hpp`
|
||||
- **作用**：将两段 `JointTrajectory` 在连接处按比例做关节空间平滑混合，输出一条合并轨迹。
|
||||
- **约束**：两段轨迹的 `joint_names` 必须一致；traj1 末点与 traj2 首点越接近，blend 越自然。
|
||||
- **参数**：
|
||||
- `blend_ratio`：两端参与混合的比例，例如 `0.2` 表示两段各约 20% 参与过渡；`0` 表示不混合、仅时间连续拼接。
|
||||
- `blend_point_count`：blend 区插点数，建议 5–20。
|
||||
|
||||
### 2. `BlendRadiusToRatio`
|
||||
|
||||
- 将接口层的 `blend_radius`（0–100）转为内部 `blend_ratio`（0 表示不混合）。
|
||||
|
||||
### 3. 与现有流程的衔接
|
||||
|
||||
- **规划**：若支持“从指定状态规划”，则先规划段 1（当前→中间点），再规划段 2（中间点→终点）。
|
||||
- **Blend**：对左臂/右臂分别：`BlendTwoTrajectories(seg1_left, seg2_left, BlendRadiusToRatio(blend_radius), 10, &left_merged)`，右臂同理。
|
||||
- **下发**：与现在一致，将 `left_merged` / `right_merged` 通过 FJT 下发给左右臂 controller，无需改 FJT 或底层控制器。
|
||||
|
||||
---
|
||||
|
||||
## 小结
|
||||
|
||||
| 项目 | 说明 |
|
||||
|----------------|------|
|
||||
| 当前是否支持两段 blend | 否；当前每个 goal 仅一条轨迹。 |
|
||||
| 推荐实现方式 | 在 motion 层用 `BlendTwoTrajectories` 合并两段轨迹，再通过 FJT 单条下发。 |
|
||||
| 接口预留 | `ArmMotionParams.blend_radius` 已存在，可映射为 blend 比例。 |
|
||||
| 规划扩展 | 需支持“从指定关节状态规划”（如 `PlanJointMotionFromState`）或“多路点/多段”goal，才能得到两段轨迹并做 blend。 |
|
||||
|
||||
在实现多段 goal 或“从指定状态规划”后，在导出并下发轨迹前调用 `BlendTwoTrajectories` 即可完成两段轨迹之间的 blending。
|
||||
18
src/dual_arm_description/CMakeLists.txt
Normal file
18
src/dual_arm_description/CMakeLists.txt
Normal file
@@ -0,0 +1,18 @@
|
||||
cmake_minimum_required(VERSION 3.10)
|
||||
project(dual_arm_description)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# Find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(xacro REQUIRED)
|
||||
find_package(urdf REQUIRED)
|
||||
|
||||
# Install directories
|
||||
install(DIRECTORY launch meshes urdf
|
||||
DESTINATION share/${PROJECT_NAME})
|
||||
|
||||
ament_package()
|
||||
|
||||
202
src/dual_arm_description/LICENSE
Normal file
202
src/dual_arm_description/LICENSE
Normal file
@@ -0,0 +1,202 @@
|
||||
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright [yyyy] [name of copyright owner]
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
1
src/dual_arm_description/config/joint_names_arms.yaml
Normal file
1
src/dual_arm_description/config/joint_names_arms.yaml
Normal file
@@ -0,0 +1 @@
|
||||
controller_joint_names: ['', 'left_arm_joint_1', 'left_arm_joint_2', 'left_arm_joint_3', 'left_arm_joint_4', 'left_arm_joint_5', 'left_arm_joint_6', 'right_arm_joint_1', 'right_arm_joint_2', 'right_arm_joint_3', 'right_arm_joint_4', 'right_arm_joint_5', 'right_arm_joint_6', ]
|
||||
51
src/dual_arm_description/launch/display.launch.py
Normal file
51
src/dual_arm_description/launch/display.launch.py
Normal file
@@ -0,0 +1,51 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
import os
|
||||
|
||||
def generate_launch_description():
|
||||
# 找到包的路径
|
||||
package_share_dir = FindPackageShare(package='dual_arm_description').find('dual_arm_description')
|
||||
|
||||
# 声明启动参数
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
|
||||
urdf_path = os.path.join(package_share_dir, 'urdf', 'dual_arm.urdf')
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time',
|
||||
default_value='false',
|
||||
description='Use simulation (Gazebo) clock if true'
|
||||
),
|
||||
|
||||
# 启动joint_state_publisher_gui
|
||||
Node(
|
||||
package='joint_state_publisher_gui',
|
||||
executable='joint_state_publisher_gui',
|
||||
name='joint_state_publisher_gui',
|
||||
output='screen'
|
||||
),
|
||||
|
||||
# 启动robot_state_publisher
|
||||
Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'robot_description': open(urdf_path).read()
|
||||
}]
|
||||
),
|
||||
|
||||
# 启动RViz2
|
||||
Node(
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
name='rviz2',
|
||||
output='screen',
|
||||
parameters=[{'use_sim_time': use_sim_time}]
|
||||
)
|
||||
])
|
||||
20
src/dual_arm_description/launch/gazebo.launch
Normal file
20
src/dual_arm_description/launch/gazebo.launch
Normal file
@@ -0,0 +1,20 @@
|
||||
<launch>
|
||||
<include
|
||||
file="$(find gazebo_ros)/launch/empty_world.launch" />
|
||||
<node
|
||||
name="tf_footprint_base"
|
||||
pkg="tf"
|
||||
type="static_transform_publisher"
|
||||
args="0 0 0 0 0 0 base_link base_footprint 40" />
|
||||
<node
|
||||
name="spawn_model"
|
||||
pkg="gazebo_ros"
|
||||
type="spawn_model"
|
||||
args="-file $(find arms)/urdf/arms.urdf -urdf -model arms"
|
||||
output="screen" />
|
||||
<node
|
||||
name="fake_joint_calibration"
|
||||
pkg="rostopic"
|
||||
type="rostopic"
|
||||
args="pub /calibrated std_msgs/Bool true" />
|
||||
</launch>
|
||||
BIN
src/dual_arm_description/meshes/base_link.STL
Normal file
BIN
src/dual_arm_description/meshes/base_link.STL
Normal file
Binary file not shown.
BIN
src/dual_arm_description/meshes/left_arm_link_1.STL
Normal file
BIN
src/dual_arm_description/meshes/left_arm_link_1.STL
Normal file
Binary file not shown.
BIN
src/dual_arm_description/meshes/left_arm_link_2.STL
Normal file
BIN
src/dual_arm_description/meshes/left_arm_link_2.STL
Normal file
Binary file not shown.
BIN
src/dual_arm_description/meshes/left_arm_link_3.STL
Normal file
BIN
src/dual_arm_description/meshes/left_arm_link_3.STL
Normal file
Binary file not shown.
BIN
src/dual_arm_description/meshes/left_arm_link_4.STL
Normal file
BIN
src/dual_arm_description/meshes/left_arm_link_4.STL
Normal file
Binary file not shown.
BIN
src/dual_arm_description/meshes/left_arm_link_5.STL
Normal file
BIN
src/dual_arm_description/meshes/left_arm_link_5.STL
Normal file
Binary file not shown.
BIN
src/dual_arm_description/meshes/left_arm_link_6.STL
Normal file
BIN
src/dual_arm_description/meshes/left_arm_link_6.STL
Normal file
Binary file not shown.
BIN
src/dual_arm_description/meshes/right_arm_link_1.STL
Normal file
BIN
src/dual_arm_description/meshes/right_arm_link_1.STL
Normal file
Binary file not shown.
BIN
src/dual_arm_description/meshes/right_arm_link_2.STL
Normal file
BIN
src/dual_arm_description/meshes/right_arm_link_2.STL
Normal file
Binary file not shown.
BIN
src/dual_arm_description/meshes/right_arm_link_3.STL
Normal file
BIN
src/dual_arm_description/meshes/right_arm_link_3.STL
Normal file
Binary file not shown.
BIN
src/dual_arm_description/meshes/right_arm_link_4.STL
Normal file
BIN
src/dual_arm_description/meshes/right_arm_link_4.STL
Normal file
Binary file not shown.
BIN
src/dual_arm_description/meshes/right_arm_link_5.STL
Normal file
BIN
src/dual_arm_description/meshes/right_arm_link_5.STL
Normal file
Binary file not shown.
BIN
src/dual_arm_description/meshes/right_arm_link_6.STL
Normal file
BIN
src/dual_arm_description/meshes/right_arm_link_6.STL
Normal file
Binary file not shown.
20
src/dual_arm_description/package.xml
Normal file
20
src/dual_arm_description/package.xml
Normal file
@@ -0,0 +1,20 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>dual_arm_description</name>
|
||||
<version>1.0.0</version>
|
||||
<description>hivecore dual arm robot description package</description>
|
||||
<maintainer email="Ray@hivecore.cn">Ray Lv</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>xacro</buildtool_depend>
|
||||
|
||||
<depend>urdf</depend>
|
||||
<depend>joint_state_publisher_gui</depend>
|
||||
<depend>rviz2</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
14
src/dual_arm_description/urdf/dual_arm.csv
Normal file
14
src/dual_arm_description/urdf/dual_arm.csv
Normal file
@@ -0,0 +1,14 @@
|
||||
Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
|
||||
base_link,0.00298057124503031,0.000515801514565793,0.0143479775720482,0,0,0,2.38613737991974,0.0300134006922132,-0.000102411277041058,0.00180787987056327,0.0140254000198246,1.62272105684866E-05,0.0277791757738099,0,0,0,0,0,0,package://arms/meshes/base_link.STL,0.498039215686275,0.498039215686275,0.498039215686275,1,0,0,0,0,0,0,package://arms/meshes/base_link.STL,,外壳2-1;右臂-1/大臂法兰1-1;左臂-1/大臂法兰1-1;后盖2-1,base_link,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
|
||||
left_arm_link_1,-0.00121014292843515,-0.00607985090076274,2.30717195142055E-06,0,0,0,1.01438144565655,0.00177944959424869,-7.74281812735368E-06,1.41839525869718E-08,0.00147694157046928,2.16095534381193E-07,0.00159565402432639,0,0,0,0,0,0,package://arms/meshes/left_arm_link_1.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://arms/meshes/left_arm_link_1.STL,,左臂-1/大臂法兰2-1;左臂-1/eRob90H××I-XX-18EX_V6_MC2E.DX.XX-2;左臂-1/肩部轴承法兰1-1,left_arm_1,A_left_arm_1,left_arm_joint_1,revolute,0,0.2645,0,0,0,0,base_link,0,-1,0,30,1,-3.14,3.14,,,,,,,,
|
||||
left_arm_link_2,0.0473073990020662,0.000240467230209984,-0.124896766185207,0,0,0,3.44815930076977,0.0205425911387401,1.08497424985243E-07,1.58751090790631E-05,0.0221557550613541,3.11670312449268E-05,0.00629782892914115,0,0,0,0,0,0,package://arms/meshes/left_arm_link_2.STL,0.898039215686275,0.917647058823529,0.929411764705882,1,0,0,0,0,0,0,package://arms/meshes/left_arm_link_2.STL,,左臂-1/肩部周转法兰3-镜像-1;左臂-1/肩部周转法兰4-1;左臂-1/肩部线缆遮盖-1;左臂-1/肩关节端盖-1;左臂-1/eRob80H××I-XX-18EX_V6_MC2E.DX.XX-1;左臂-1/肘关节盖1-1,left_arm_2,A_left_arm_2,left_arm_joint_2,revolute,0,0,0,0,0,0,left_arm_link_1,1,0,0,30,1,-3.14,3.14,,,,,,,,
|
||||
left_arm_link_3,-0.0244216591365906,1.57026866504095E-05,-0.0644017249622112,0,0,0,0.872590806848245,0.00194830010675522,2.54189746127937E-07,0.000582315993376883,0.00211386368849458,-3.77171097146813E-07,0.00113934274062373,0,0,0,0,0,0,package://arms/meshes/left_arm_link_3.STL,1,1,1,1,0,0,0,0,0,0,package://arms/meshes/left_arm_link_3.STL,,左臂-1/eRob70H××I-XX-18EX_V5_MC2D.DX.XX-1;左臂-1/肘部展折法兰2-1;左臂-1/肘关节盖2-1,left_arm_3,A_left_arm_3,left_arm_joint_3,revolute,0,0,-0.256,0,0,0,left_arm_link_2,-1,0,0,30,1,-3.14,3.14,,,,,,,,
|
||||
left_arm_link_4,0.0298883078018623,3.2586123593914E-07,-0.0634039016642955,0,0,0,0.554170739785688,0.000791642003681768,1.82737472933534E-09,0.000163616251225181,0.000796515292662639,-5.09665937788319E-09,0.000529269504583201,0,0,0,0,0,0,package://arms/meshes/left_arm_link_4.STL,0.898039215686275,0.917647058823529,0.929411764705882,1,0,0,0,0,0,0,package://arms/meshes/left_arm_link_4.STL,,左臂-1/肘部旋转法兰2-1;左臂-1/腕关节盖2-3,left_arm_4,A_left_arm_4,left_arm_joint_4,revolute,0,0,-0.14,0,0,0,left_arm_link_3,0,0,1,30,1,-3.14,3.14,,,,,,,,
|
||||
left_arm_link_5,0.000905144911766077,4.98476898669331E-06,-0.0452958049045105,0,0,0,0.878688377705966,0.00139023048760559,3.08179807647102E-08,-4.43955406467986E-05,0.00135925380280776,-3.78717391068107E-07,0.000904056015468609,0,0,0,0,0,0,package://arms/meshes/left_arm_link_5.STL,0.498039215686275,0.498039215686275,0.498039215686275,1,0,0,0,0,0,0,package://arms/meshes/left_arm_link_5.STL,,左臂-1/腕部防护盖-1;左臂-1/eRob70H××I-XX-18EX_V5_MC2D.DX.XX-3;左臂-1/腕关节盖1-1;左臂-1/腕部翻转法兰2-1;左臂-1/eRob70H××I-XX-18EX_V5_MC2D.DX.XX-2,left_arm_5,A_left_arm_5,left_arm_joint_5,revolute,0,0,-0.08,0,0,0,left_arm_link_4,-1,0,0,30,1,-3.14,3.14,,,,,,,,
|
||||
left_arm_link_6,-0.000101887716633362,0.015114071240723,-0.0652860140985222,0,0,0,0.999964109118212,0.000777334341902839,-2.05506130408708E-07,-3.61838630971151E-07,0.00110371590937852,2.93487288693614E-06,0.000974801992678963,0,0,0,0,0,0,package://arms/meshes/left_arm_link_6.STL,0.898039215686275,0.917647058823529,0.929411764705882,1,0,0,0,0,0,0,package://arms/meshes/left_arm_link_6.STL,,左臂-1/手臂相机法兰-1;左臂-1/M12堵头-1;左臂-1/奥比支架-1;左臂-1/奥比支架后盖-1;左臂-1/Gemini 335L_20240318-1-1;左臂-1/摄像头前挡板-1;左臂-1/钧舵RG75-300-1,left_arm_6,A_left_arm_6,left_arm_joint_6,revolute,0,0,-0.125,0,0,0,left_arm_link_5,0,0,-1,30,1,-3.14,3.14,,,,,,,,
|
||||
right_arm_link_1,-0.00121014259703951,0.00608867124860621,-1.29848221117196E-05,0,0,0,1.01438145710903,0.00177944960401512,8.0553144995043E-06,-3.5960361781722E-07,0.00147672077346284,-1.54892249181004E-07,0.00159587483827316,0,0,0,0,0,0,package://arms/meshes/right_arm_link_1.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://arms/meshes/right_arm_link_1.STL,,右臂-1/大臂法兰2-1;右臂-1/eRob90H××I-XX-18EX_V6_MC2E.DX.XX-2;右臂-1/肩部轴承法兰1-1,right_arm_1,A_right_arm_1,right_arm_joint_1,revolute,0,-0.264499999954925,0,0,0,0,base_link,0,-1,0,30,1,-3.14,3.14,,,,,,,,
|
||||
right_arm_link_2,-0.00050247965429728,-0.00021546614906065,-0.12491520176963,0,0,0,2.06685139029607,0.0108037096742892,-4.71664207475744E-06,-0.0018060013988364,0.0114661489578501,-1.52948974000445E-05,0.0033764723014604,0,0,0,0,0,0,package://arms/meshes/right_arm_link_2.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://arms/meshes/right_arm_link_2.STL,,右臂-1/肩部周转法兰3-1;右臂-1/肩部线缆遮盖-1;右臂-1/肩部周转法兰4-1;右臂-1/肩关节端盖-1;右臂-1/eRob80H××I-XX-18EX_V6_MC2E.DX.XX-1;右臂-1/肘关节盖1-1,right_arm_2,A_right_arm_2,right_arm_joint_2,revolute,0,0,0,0,0,0,right_arm_link_1,-1,0,0,30,1,-3.14,3.14,,,,,,,,
|
||||
right_arm_link_3,-0.0244230615717252,1.69165931350745E-05,-0.0644017249622173,0,0,0,0.87259080684837,0.00194827035859605,3.14401107436852E-07,0.000582351277048671,0.00211389343665385,-4.11685281008291E-07,0.00113934274062382,0,0,0,0,0,0,package://arms/meshes/right_arm_link_3.STL,0.898039215686275,0.917647058823529,0.929411764705882,1,0,0,0,0,0,0,package://arms/meshes/right_arm_link_3.STL,,右臂-1/肘关节盖2-2;右臂-1/肘部展折法兰2-1;右臂-1/eRob70H××I-XX-18EX_V5_MC2D.DX.XX-1,right_arm_3,A_right_arm_3,right_arm_joint_3,revolute,0,0,-0.256,0,0,0,right_arm_link_2,-1,0,0,30,1,-3.14,3.14,,,,,,,,
|
||||
right_arm_link_4,0.0298883078018621,3.25861235550562E-07,-0.0634039016642952,0,0,0,0.554170739785691,0.000791642003681773,1.82737472901328E-09,0.000163616251225182,0.000796515292662645,-5.09665937775888E-09,0.000529269504583203,0,0,0,0,0,0,package://arms/meshes/right_arm_link_4.STL,0.898039215686275,0.917647058823529,0.929411764705882,1,0,0,0,0,0,0,package://arms/meshes/right_arm_link_4.STL,,右臂-1/肘部旋转法兰2-1;右臂-1/腕关节盖2-1,right_arm_4,A_right_arm_4,right_arm_joint_4,revolute,0,0,-0.14,0,0,0,right_arm_link_3,0,0,1,30,1,-3.14,3.14,,,,,,,,
|
||||
right_arm_link_5,0.000901888305542433,1.25876753304666E-05,-0.0452702924877855,0,0,0,0.878688258547313,0.0013901168763223,2.17076373482529E-07,-4.36500837199152E-05,0.00135937763962618,-4.32056596179085E-07,0.000904045240570538,0,0,0,0,0,0,package://arms/meshes/right_arm_link_5.STL,0.498039215686275,0.498039215686275,0.498039215686275,1,0,0,0,0,0,0,package://arms/meshes/right_arm_link_5.STL,,右臂-1/腕部防护盖-1;右臂-1/eRob70H××I-XX-18EX_V5_MC2D.DX.XX-2;右臂-1/eRob70H××I-XX-18EX_V5_MC2D.DX.XX-3;右臂-1/腕部翻转法兰2-1;右臂-1/腕关节盖1-1,right_arm_5,A_right_arm_5,right_arm_joint_5,revolute,0,0,-0.0799999999999815,0,0,0,right_arm_link_4,-1,0,0,30,1,-3.14,3.14,,,,,,,,
|
||||
right_arm_link_6,-0.000111720946994739,-0.015114065193529,-0.0652860148982759,0,0,0,0.999964129684861,0.00077733434761381,-2.05512635353002E-07,-4.5647112991376E-07,0.00110371592448829,-2.93548237473146E-06,0.000974802009102366,0,0,0,0,0,0,package://arms/meshes/right_arm_link_6.STL,0.498039215686275,0.498039215686275,0.498039215686275,1,0,0,0,0,0,0,package://arms/meshes/right_arm_link_6.STL,,右臂-1/奥比支架后盖-1;右臂-1/奥比支架-1;右臂-1/Gemini 335L_20240318-1-1;右臂-1/摄像头前挡板-1;右臂-1/手臂相机法兰-1;右臂-1/M12堵头-1;右臂-1/钧舵RG75-300-1,right_arm_6,A_right_arm_6,right_arm_joint_6,revolute,0,0,-0.125,0,0,0,right_arm_link_5,0,0,-1,30,1,-3.14,3.14,,,,,,,,
|
||||
|
853
src/dual_arm_description/urdf/dual_arm.urdf
Normal file
853
src/dual_arm_description/urdf/dual_arm.urdf
Normal file
@@ -0,0 +1,853 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
|
||||
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
|
||||
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
|
||||
<robot
|
||||
name="dual_arm">
|
||||
|
||||
<link
|
||||
name="base_link">
|
||||
<!-- <inertial>
|
||||
<origin
|
||||
xyz="0.00298057124503031 0.000515801514565793 0.0143479775720482"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="2.38613737991974" />
|
||||
<inertia
|
||||
ixx="0.01841"
|
||||
ixy="0.000143"
|
||||
ixz="0.000002"
|
||||
iyy="0.050619"
|
||||
iyz="-0.001358"
|
||||
izz="0.052099" />
|
||||
</inertial> -->
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.498039215686275 0.498039215686275 0.498039215686275 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="left_arm_link_1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.00121014292843515 -0.00607985090076274 2.30717195142055E-06"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.01438144565655" />
|
||||
<inertia
|
||||
ixx="0.001976"
|
||||
ixy="-0.000032"
|
||||
ixz="-0.000001"
|
||||
iyy="0.001842"
|
||||
iyz="0.000001"
|
||||
izz="0.002157" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/left_arm_link_1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/left_arm_link_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_arm_joint_1"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0.2645 0"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="left_arm_link_1" />
|
||||
<axis
|
||||
xyz="0 -1 0" />
|
||||
<limit
|
||||
lower="-3.14"
|
||||
upper="3.14"
|
||||
effort="30.0"
|
||||
velocity="3.0" />
|
||||
</joint>
|
||||
<link
|
||||
name="left_arm_link_2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.0473073990020662 0.000240467230209984 -0.124896766185207"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="3.44815930076977" />
|
||||
<inertia
|
||||
ixx="0.045011"
|
||||
ixy="0.000018"
|
||||
ixz="0.000032"
|
||||
iyy="0.029756"
|
||||
iyz="-0.001086"
|
||||
izz="0.01994" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/left_arm_link_2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/left_arm_link_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_arm_joint_2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="left_arm_link_1" />
|
||||
<child
|
||||
link="left_arm_link_2" />
|
||||
<axis
|
||||
xyz="1 0 0" />
|
||||
<limit
|
||||
lower="-3.14"
|
||||
upper="3.14"
|
||||
effort="30.0"
|
||||
velocity="3.0" />
|
||||
</joint>
|
||||
<link
|
||||
name="left_arm_link_3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0244216591365906 1.57026866504095E-05 -0.0644017249622112"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.872590806848245" />
|
||||
<inertia
|
||||
ixx="0.002933"
|
||||
ixy="-0.000001"
|
||||
ixz="0.000001"
|
||||
iyy="0.00252"
|
||||
iyz="-0.00095"
|
||||
izz="0.001386" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/left_arm_link_3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/left_arm_link_3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_arm_joint_3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 -0.256"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="left_arm_link_2" />
|
||||
<child
|
||||
link="left_arm_link_3" />
|
||||
<axis
|
||||
xyz="-1 0 0" />
|
||||
<limit
|
||||
lower="-3.14"
|
||||
upper="3.14"
|
||||
effort="30.0"
|
||||
velocity="3.0" />
|
||||
</joint>
|
||||
<link
|
||||
name="left_arm_link_4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.0298883078018623 3.2586123593914E-07 -0.0634039016642955"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.554170739785688" />
|
||||
<inertia
|
||||
ixx="0.001529"
|
||||
ixy="-0.000001"
|
||||
ixz="0.000001"
|
||||
iyy="0.001395"
|
||||
iyz="0.000066"
|
||||
izz="0.000659" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/left_arm_link_4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/left_arm_link_4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_arm_joint_4"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 -0.14"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="left_arm_link_3" />
|
||||
<child
|
||||
link="left_arm_link_4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.14"
|
||||
upper="3.14"
|
||||
effort="30.0"
|
||||
velocity="3.0" />
|
||||
</joint>
|
||||
<link
|
||||
name="left_arm_link_5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000905144911766077 4.98476898669331E-06 -0.0452958049045105"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.878688377705965" />
|
||||
<inertia
|
||||
ixx="0.002476"
|
||||
ixy="-0.000001"
|
||||
ixz="0.000001"
|
||||
iyy="0.002441"
|
||||
iyz="0.000035"
|
||||
izz="0.000970" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/left_arm_link_5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.498039215686275 0.498039215686275 0.498039215686275 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/left_arm_link_5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_arm_joint_5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 -0.08"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="left_arm_link_4" />
|
||||
<child
|
||||
link="left_arm_link_5" />
|
||||
<axis
|
||||
xyz="-1 0 0" />
|
||||
<limit
|
||||
lower="-3.14"
|
||||
upper="3.14"
|
||||
effort="30.0"
|
||||
velocity="3.0" />
|
||||
</joint>
|
||||
<link
|
||||
name="left_arm_link_6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000101887716633362 0.015114071240723 -0.0652860140985222"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.908709" />
|
||||
<inertia
|
||||
ixx="0.006191"
|
||||
ixy="0.000018"
|
||||
ixz="-0.001104"
|
||||
iyy="0.006720"
|
||||
iyz="0.000001"
|
||||
izz="0.002820" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/left_arm_link_6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/left_arm_link_6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="left_arm_joint_6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 -0.125"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="left_arm_link_5" />
|
||||
<child
|
||||
link="left_arm_link_6" />
|
||||
<axis
|
||||
xyz="0 0 -1" />
|
||||
<limit
|
||||
lower="-3.14"
|
||||
upper="3.14"
|
||||
effort="30.0"
|
||||
velocity="3.0" />
|
||||
</joint>
|
||||
<link
|
||||
name="right_arm_link_1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.00121014259703951 0.00608867124860621 -1.29848221117196E-05"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.01438145710903" />
|
||||
<inertia
|
||||
ixx="0.001976"
|
||||
ixy="0.000032"
|
||||
ixz="-0.000001"
|
||||
iyy="0.001842"
|
||||
iyz="0.000001"
|
||||
izz="0.002157" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/right_arm_link_1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/right_arm_link_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_arm_joint_1"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.264499999954925 0"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="right_arm_link_1" />
|
||||
<axis
|
||||
xyz="0 -1 0" />
|
||||
<limit
|
||||
lower="-3.14"
|
||||
upper="3.14"
|
||||
effort="30.0"
|
||||
velocity="3.0" />
|
||||
</joint>
|
||||
<link
|
||||
name="right_arm_link_2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.00050247965429728 -0.00021546614906065 -0.12491520176963"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="3.44815930076977" />
|
||||
<inertia
|
||||
ixx="0.045011"
|
||||
ixy="0.000018"
|
||||
ixz="0.000032"
|
||||
iyy="0.029756"
|
||||
iyz="-0.001086"
|
||||
izz="0.01994" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/right_arm_link_2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/right_arm_link_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_arm_joint_2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="right_arm_link_1" />
|
||||
<child
|
||||
link="right_arm_link_2" />
|
||||
<axis
|
||||
xyz="-1 0 0" />
|
||||
<limit
|
||||
lower="-3.14"
|
||||
upper="3.14"
|
||||
effort="30.0"
|
||||
velocity="3.0" />
|
||||
</joint>
|
||||
<link
|
||||
name="right_arm_link_3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0244230615717252 1.69165931350745E-05 -0.0644017249622173"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.87259080684837" />
|
||||
<inertia
|
||||
ixx="0.002933"
|
||||
ixy="-0.000001"
|
||||
ixz="0.000001"
|
||||
iyy="0.00252"
|
||||
iyz="-0.00095"
|
||||
izz="0.001386" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/right_arm_link_3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/right_arm_link_3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_arm_joint_3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 -0.256"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="right_arm_link_2" />
|
||||
<child
|
||||
link="right_arm_link_3" />
|
||||
<axis
|
||||
xyz="-1 0 0" />
|
||||
<limit
|
||||
lower="-3.14"
|
||||
upper="3.14"
|
||||
effort="30.0"
|
||||
velocity="3.0" />
|
||||
</joint>
|
||||
<link
|
||||
name="right_arm_link_4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.0298883078018621 3.25861235550562E-07 -0.0634039016642952"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.554170739785691" />
|
||||
<inertia
|
||||
ixx="0.001529"
|
||||
ixy="-0.000001"
|
||||
ixz="0.000001"
|
||||
iyy="0.001395"
|
||||
iyz="0.000066"
|
||||
izz="0.000659" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/right_arm_link_4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/right_arm_link_4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_arm_joint_4"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 -0.14"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="right_arm_link_3" />
|
||||
<child
|
||||
link="right_arm_link_4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.14"
|
||||
upper="3.14"
|
||||
effort="30.0"
|
||||
velocity="3.0" />
|
||||
</joint>
|
||||
<link
|
||||
name="right_arm_link_5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000901888305542433 1.25876753304666E-05 -0.0452702924877855"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.878688258547313" />
|
||||
<inertia
|
||||
ixx="0.002476"
|
||||
ixy="-0.000001"
|
||||
ixz="0.000001"
|
||||
iyy="0.002441"
|
||||
iyz="0.000035"
|
||||
izz="0.000970" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/right_arm_link_5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.498039215686275 0.498039215686275 0.498039215686275 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/right_arm_link_5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_arm_joint_5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 -0.0799999999999815"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="right_arm_link_4" />
|
||||
<child
|
||||
link="right_arm_link_5" />
|
||||
<axis
|
||||
xyz="-1 0 0" />
|
||||
<limit
|
||||
lower="-3.14"
|
||||
upper="3.14"
|
||||
effort="30.0"
|
||||
velocity="3.0" />
|
||||
</joint>
|
||||
<link
|
||||
name="right_arm_link_6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000111720946994739 -0.015114065193529 -0.0652860148982759"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.908709" />
|
||||
<inertia
|
||||
ixx="0.006191"
|
||||
ixy="0.000018"
|
||||
ixz="-0.001104"
|
||||
iyy="0.006720"
|
||||
iyz="0.000001"
|
||||
izz="0.002820" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/right_arm_link_6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.498039215686275 0.498039215686275 0.498039215686275 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://dual_arm_description/meshes/right_arm_link_6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="right_arm_joint_6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 -0.125"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="right_arm_link_5" />
|
||||
<child
|
||||
link="right_arm_link_6" />
|
||||
<axis
|
||||
xyz="0 0 -1" />
|
||||
<limit
|
||||
lower="-3.14"
|
||||
upper="3.14"
|
||||
effort="30.0"
|
||||
velocity="3.0" />
|
||||
</joint>
|
||||
|
||||
<!-- ros2_control: mock_components/GenericSystem for fake hardware simulation.
|
||||
Joints teleport instantly to commanded position (no physics).
|
||||
Provides /joint_states via joint_state_broadcaster and
|
||||
FollowJointTrajectory action servers via joint_trajectory_controller. -->
|
||||
<ros2_control name="dual_arm_mock" type="system">
|
||||
<hardware>
|
||||
<plugin>mock_components/GenericSystem</plugin>
|
||||
</hardware>
|
||||
|
||||
<joint name="left_arm_joint_1">
|
||||
<command_interface name="position">
|
||||
<param name="min">-3.14</param>
|
||||
<param name="max">3.14</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
|
||||
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
|
||||
</joint>
|
||||
<joint name="left_arm_joint_2">
|
||||
<command_interface name="position">
|
||||
<param name="min">-3.14</param>
|
||||
<param name="max">3.14</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
|
||||
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
|
||||
</joint>
|
||||
<joint name="left_arm_joint_3">
|
||||
<command_interface name="position">
|
||||
<param name="min">-3.14</param>
|
||||
<param name="max">3.14</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
|
||||
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
|
||||
</joint>
|
||||
<joint name="left_arm_joint_4">
|
||||
<command_interface name="position">
|
||||
<param name="min">-3.14</param>
|
||||
<param name="max">3.14</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
|
||||
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
|
||||
</joint>
|
||||
<joint name="left_arm_joint_5">
|
||||
<command_interface name="position">
|
||||
<param name="min">-3.14</param>
|
||||
<param name="max">3.14</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
|
||||
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
|
||||
</joint>
|
||||
<joint name="left_arm_joint_6">
|
||||
<command_interface name="position">
|
||||
<param name="min">-3.14</param>
|
||||
<param name="max">3.14</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
|
||||
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
|
||||
</joint>
|
||||
|
||||
<joint name="right_arm_joint_1">
|
||||
<command_interface name="position">
|
||||
<param name="min">-3.14</param>
|
||||
<param name="max">3.14</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
|
||||
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
|
||||
</joint>
|
||||
<joint name="right_arm_joint_2">
|
||||
<command_interface name="position">
|
||||
<param name="min">-3.14</param>
|
||||
<param name="max">3.14</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
|
||||
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
|
||||
</joint>
|
||||
<joint name="right_arm_joint_3">
|
||||
<command_interface name="position">
|
||||
<param name="min">-3.14</param>
|
||||
<param name="max">3.14</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
|
||||
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
|
||||
</joint>
|
||||
<joint name="right_arm_joint_4">
|
||||
<command_interface name="position">
|
||||
<param name="min">-3.14</param>
|
||||
<param name="max">3.14</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
|
||||
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
|
||||
</joint>
|
||||
<joint name="right_arm_joint_5">
|
||||
<command_interface name="position">
|
||||
<param name="min">-3.14</param>
|
||||
<param name="max">3.14</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
|
||||
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
|
||||
</joint>
|
||||
<joint name="right_arm_joint_6">
|
||||
<command_interface name="position">
|
||||
<param name="min">-3.14</param>
|
||||
<param name="max">3.14</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
|
||||
<state_interface name="velocity"><param name="initial_value">0.0</param></state_interface>
|
||||
</joint>
|
||||
</ros2_control>
|
||||
|
||||
</robot>
|
||||
25
src/dual_arm_moveit_config/.setup_assistant
Normal file
25
src/dual_arm_moveit_config/.setup_assistant
Normal file
@@ -0,0 +1,25 @@
|
||||
moveit_setup_assistant_config:
|
||||
urdf:
|
||||
package: dual_arm_description
|
||||
relative_path: urdf/dual_arm.urdf
|
||||
srdf:
|
||||
relative_path: config/dual_arm.srdf
|
||||
package_settings:
|
||||
author_name: ray
|
||||
author_email: ray@hivecore.cn
|
||||
generated_timestamp: 1764123395
|
||||
control_xacro:
|
||||
command:
|
||||
- position
|
||||
state:
|
||||
- position
|
||||
- velocity
|
||||
modified_urdf:
|
||||
xacros:
|
||||
- control_xacro
|
||||
control_xacro:
|
||||
command:
|
||||
- position
|
||||
state:
|
||||
- position
|
||||
- velocity
|
||||
16
src/dual_arm_moveit_config/CMakeLists.txt
Normal file
16
src/dual_arm_moveit_config/CMakeLists.txt
Normal file
@@ -0,0 +1,16 @@
|
||||
cmake_minimum_required(VERSION 3.22)
|
||||
project(dual_arm_moveit_config)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
ament_package()
|
||||
|
||||
if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch")
|
||||
install(
|
||||
DIRECTORY launch
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
PATTERN "setup_assistant.launch" EXCLUDE)
|
||||
endif()
|
||||
|
||||
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
|
||||
install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME})
|
||||
@@ -0,0 +1,98 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:macro name="dual_arm_ros2_control" params="name initial_positions_file">
|
||||
<xacro:property name="initial_positions" value="${xacro.load_yaml(initial_positions_file)['initial_positions']}"/>
|
||||
|
||||
<ros2_control name="${name}" type="system">
|
||||
<hardware>
|
||||
<!-- By default, set up controllers for simulation. This won't work on real hardware -->
|
||||
<plugin>mock_components/GenericSystem</plugin>
|
||||
</hardware>
|
||||
<joint name="left_arm_joint_1">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_arm_joint_1']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="left_arm_joint_2">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_arm_joint_2']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="left_arm_joint_3">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_arm_joint_3']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="left_arm_joint_4">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_arm_joint_4']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="left_arm_joint_5">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_arm_joint_5']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="left_arm_joint_6">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_arm_joint_6']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="right_arm_joint_1">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_arm_joint_1']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="right_arm_joint_2">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_arm_joint_2']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="right_arm_joint_3">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_arm_joint_3']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="right_arm_joint_4">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_arm_joint_4']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="right_arm_joint_5">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_arm_joint_5']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="right_arm_joint_6">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_arm_joint_6']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
|
||||
</ros2_control>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
134
src/dual_arm_moveit_config/config/dual_arm.srdf
Normal file
134
src/dual_arm_moveit_config/config/dual_arm.srdf
Normal file
@@ -0,0 +1,134 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!--This does not replace URDF, and is not an extension of URDF.
|
||||
This is a format for representing semantic information about the robot structure.
|
||||
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
|
||||
-->
|
||||
<robot name="dual_arm">
|
||||
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
|
||||
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
|
||||
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
|
||||
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
|
||||
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
|
||||
<group name="arm_left">
|
||||
<chain base_link="base_link" tip_link="left_arm_link_6"/>
|
||||
</group>
|
||||
<group name="arm_right">
|
||||
<chain base_link="base_link" tip_link="right_arm_link_6"/>
|
||||
</group>
|
||||
<group name="dual_arm">
|
||||
<group name="arm_left"/>
|
||||
<group name="arm_right"/>
|
||||
</group>
|
||||
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
|
||||
<group_state name="arm_left_home_pose" group="arm_left">
|
||||
<joint name="left_arm_joint_1" value="0"/>
|
||||
<joint name="left_arm_joint_2" value="0"/>
|
||||
<joint name="left_arm_joint_3" value="0"/>
|
||||
<joint name="left_arm_joint_4" value="0"/>
|
||||
<joint name="left_arm_joint_5" value="0"/>
|
||||
<joint name="left_arm_joint_6" value="0"/>
|
||||
</group_state>
|
||||
<group_state name="arm_right_home_pose" group="arm_right">
|
||||
<joint name="right_arm_joint_1" value="0"/>
|
||||
<joint name="right_arm_joint_2" value="0"/>
|
||||
<joint name="right_arm_joint_3" value="0"/>
|
||||
<joint name="right_arm_joint_4" value="0"/>
|
||||
<joint name="right_arm_joint_5" value="0"/>
|
||||
<joint name="right_arm_joint_6" value="0"/>
|
||||
</group_state>
|
||||
<group_state name="dual_arm_home_pose" group="dual_arm">
|
||||
<joint name="left_arm_joint_1" value="0"/>
|
||||
<joint name="left_arm_joint_2" value="0"/>
|
||||
<joint name="left_arm_joint_3" value="0"/>
|
||||
<joint name="left_arm_joint_4" value="0"/>
|
||||
<joint name="left_arm_joint_5" value="0"/>
|
||||
<joint name="left_arm_joint_6" value="0"/>
|
||||
<joint name="right_arm_joint_1" value="0"/>
|
||||
<joint name="right_arm_joint_2" value="0"/>
|
||||
<joint name="right_arm_joint_3" value="0"/>
|
||||
<joint name="right_arm_joint_4" value="0"/>
|
||||
<joint name="right_arm_joint_5" value="0"/>
|
||||
<joint name="right_arm_joint_6" value="0"/>
|
||||
</group_state>
|
||||
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
|
||||
<virtual_joint name="base_joint" type="fixed" parent_frame="world" child_link="base_link"/>
|
||||
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
|
||||
<disable_collisions link1="base_link" link2="left_arm_link_1" reason="User"/>
|
||||
<disable_collisions link1="base_link" link2="left_arm_link_2" reason="User"/>
|
||||
<disable_collisions link1="base_link" link2="left_arm_link_3" reason="User"/>
|
||||
<disable_collisions link1="base_link" link2="left_arm_link_4" reason="User"/>
|
||||
<disable_collisions link1="base_link" link2="left_arm_link_5" reason="User"/>
|
||||
<disable_collisions link1="base_link" link2="left_arm_link_6" reason="User"/>
|
||||
<disable_collisions link1="base_link" link2="right_arm_link_1" reason="User"/>
|
||||
<disable_collisions link1="base_link" link2="right_arm_link_2" reason="User"/>
|
||||
<disable_collisions link1="base_link" link2="right_arm_link_3" reason="User"/>
|
||||
<disable_collisions link1="base_link" link2="right_arm_link_4" reason="User"/>
|
||||
<disable_collisions link1="base_link" link2="right_arm_link_5" reason="User"/>
|
||||
<disable_collisions link1="base_link" link2="right_arm_link_6" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_1" link2="left_arm_link_2" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_1" link2="left_arm_link_3" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_1" link2="left_arm_link_4" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_1" link2="left_arm_link_5" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_1" link2="left_arm_link_6" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_1" link2="right_arm_link_1" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_1" link2="right_arm_link_2" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_1" link2="right_arm_link_3" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_1" link2="right_arm_link_4" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_1" link2="right_arm_link_5" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_1" link2="right_arm_link_6" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_2" link2="left_arm_link_3" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_2" link2="left_arm_link_4" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_2" link2="left_arm_link_5" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_2" link2="left_arm_link_6" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_2" link2="right_arm_link_1" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_2" link2="right_arm_link_2" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_2" link2="right_arm_link_3" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_2" link2="right_arm_link_4" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_2" link2="right_arm_link_5" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_2" link2="right_arm_link_6" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_3" link2="left_arm_link_4" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_3" link2="left_arm_link_5" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_3" link2="left_arm_link_6" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_3" link2="right_arm_link_1" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_3" link2="right_arm_link_2" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_3" link2="right_arm_link_3" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_3" link2="right_arm_link_4" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_3" link2="right_arm_link_5" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_3" link2="right_arm_link_6" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_4" link2="left_arm_link_5" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_4" link2="left_arm_link_6" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_4" link2="right_arm_link_1" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_4" link2="right_arm_link_2" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_4" link2="right_arm_link_3" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_4" link2="right_arm_link_4" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_4" link2="right_arm_link_5" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_4" link2="right_arm_link_6" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_5" link2="left_arm_link_6" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_5" link2="right_arm_link_1" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_5" link2="right_arm_link_2" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_5" link2="right_arm_link_3" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_5" link2="right_arm_link_4" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_5" link2="right_arm_link_5" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_5" link2="right_arm_link_6" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_6" link2="right_arm_link_1" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_6" link2="right_arm_link_2" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_6" link2="right_arm_link_3" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_6" link2="right_arm_link_4" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_6" link2="right_arm_link_5" reason="User"/>
|
||||
<disable_collisions link1="left_arm_link_6" link2="right_arm_link_6" reason="User"/>
|
||||
<disable_collisions link1="right_arm_link_1" link2="right_arm_link_2" reason="User"/>
|
||||
<disable_collisions link1="right_arm_link_1" link2="right_arm_link_3" reason="User"/>
|
||||
<disable_collisions link1="right_arm_link_1" link2="right_arm_link_4" reason="User"/>
|
||||
<disable_collisions link1="right_arm_link_1" link2="right_arm_link_5" reason="User"/>
|
||||
<disable_collisions link1="right_arm_link_1" link2="right_arm_link_6" reason="User"/>
|
||||
<disable_collisions link1="right_arm_link_2" link2="right_arm_link_3" reason="User"/>
|
||||
<disable_collisions link1="right_arm_link_2" link2="right_arm_link_4" reason="User"/>
|
||||
<disable_collisions link1="right_arm_link_2" link2="right_arm_link_5" reason="User"/>
|
||||
<disable_collisions link1="right_arm_link_2" link2="right_arm_link_6" reason="User"/>
|
||||
<disable_collisions link1="right_arm_link_3" link2="right_arm_link_4" reason="User"/>
|
||||
<disable_collisions link1="right_arm_link_3" link2="right_arm_link_5" reason="User"/>
|
||||
<disable_collisions link1="right_arm_link_3" link2="right_arm_link_6" reason="User"/>
|
||||
<disable_collisions link1="right_arm_link_4" link2="right_arm_link_5" reason="User"/>
|
||||
<disable_collisions link1="right_arm_link_4" link2="right_arm_link_6" reason="User"/>
|
||||
<disable_collisions link1="right_arm_link_5" link2="right_arm_link_6" reason="User"/>
|
||||
</robot>
|
||||
14
src/dual_arm_moveit_config/config/dual_arm.urdf.xacro
Normal file
14
src/dual_arm_moveit_config/config/dual_arm.urdf.xacro
Normal file
@@ -0,0 +1,14 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="dual_arm">
|
||||
<xacro:arg name="initial_positions_file" default="initial_positions.yaml" />
|
||||
|
||||
<!-- Import dual_arm urdf file -->
|
||||
<xacro:include filename="$(find dual_arm_description)/urdf/dual_arm.urdf" />
|
||||
|
||||
<!-- Import control_xacro -->
|
||||
<xacro:include filename="dual_arm.ros2_control.xacro" />
|
||||
|
||||
|
||||
<xacro:dual_arm_ros2_control name="FakeSystem" initial_positions_file="$(arg initial_positions_file)"/>
|
||||
|
||||
</robot>
|
||||
15
src/dual_arm_moveit_config/config/initial_positions.yaml
Normal file
15
src/dual_arm_moveit_config/config/initial_positions.yaml
Normal file
@@ -0,0 +1,15 @@
|
||||
# Default initial positions for dual_arm's ros2_control fake system
|
||||
|
||||
initial_positions:
|
||||
left_arm_joint_1: 0
|
||||
left_arm_joint_2: 0
|
||||
left_arm_joint_3: 0
|
||||
left_arm_joint_4: 0
|
||||
left_arm_joint_5: 0
|
||||
left_arm_joint_6: 0
|
||||
right_arm_joint_1: 0
|
||||
right_arm_joint_2: 0
|
||||
right_arm_joint_3: 0
|
||||
right_arm_joint_4: 0
|
||||
right_arm_joint_5: 0
|
||||
right_arm_joint_6: 0
|
||||
70
src/dual_arm_moveit_config/config/joint_limits.yaml
Normal file
70
src/dual_arm_moveit_config/config/joint_limits.yaml
Normal file
@@ -0,0 +1,70 @@
|
||||
# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
|
||||
|
||||
# For beginners, we downscale velocity and acceleration limits.
|
||||
# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed.
|
||||
default_velocity_scaling_factor: 0.5
|
||||
default_acceleration_scaling_factor: 0.5
|
||||
|
||||
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
|
||||
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
|
||||
joint_limits:
|
||||
left_arm_joint_1:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 3.14
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 10.0
|
||||
left_arm_joint_2:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 3.14
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 10.0
|
||||
left_arm_joint_3:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 3.14
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 10.0
|
||||
left_arm_joint_4:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 3.14
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 10.0
|
||||
left_arm_joint_5:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 3.14
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 10.0
|
||||
left_arm_joint_6:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 3.14
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 10.0
|
||||
right_arm_joint_1:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 3.14
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 10.0
|
||||
right_arm_joint_2:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 3.14
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 10.0
|
||||
right_arm_joint_3:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 3.14
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 10.0
|
||||
right_arm_joint_4:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 3.14
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 10.0
|
||||
right_arm_joint_5:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 3.14
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 10.0
|
||||
right_arm_joint_6:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 3.14
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 10.0
|
||||
8
src/dual_arm_moveit_config/config/kinematics.yaml
Normal file
8
src/dual_arm_moveit_config/config/kinematics.yaml
Normal file
@@ -0,0 +1,8 @@
|
||||
arm_left:
|
||||
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
|
||||
kinematics_solver_search_resolution: 0.0050000000000000001
|
||||
kinematics_solver_timeout: 0.0050000000000000001
|
||||
arm_right:
|
||||
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
|
||||
kinematics_solver_search_resolution: 0.0050000000000000001
|
||||
kinematics_solver_timeout: 0.0050000000000000001
|
||||
51
src/dual_arm_moveit_config/config/moveit.rviz
Normal file
51
src/dual_arm_moveit_config/config/moveit.rviz
Normal file
@@ -0,0 +1,51 @@
|
||||
Panels:
|
||||
- Class: rviz_common/Displays
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /MotionPlanning1
|
||||
- Class: rviz_common/Help
|
||||
Name: Help
|
||||
- Class: rviz_common/Views
|
||||
Name: Views
|
||||
Visualization Manager:
|
||||
Displays:
|
||||
- Class: rviz_default_plugins/Grid
|
||||
Name: Grid
|
||||
Value: true
|
||||
- Class: moveit_rviz_plugin/MotionPlanning
|
||||
Name: MotionPlanning
|
||||
Planned Path:
|
||||
Loop Animation: true
|
||||
State Display Time: 0.05 s
|
||||
Trajectory Topic: display_planned_path
|
||||
Planning Scene Topic: monitored_planning_scene
|
||||
Robot Description: robot_description
|
||||
Scene Geometry:
|
||||
Scene Alpha: 1
|
||||
Scene Robot:
|
||||
Robot Alpha: 0.5
|
||||
Value: true
|
||||
Global Options:
|
||||
Fixed Frame: base_link
|
||||
Tools:
|
||||
- Class: rviz_default_plugins/Interact
|
||||
- Class: rviz_default_plugins/MoveCamera
|
||||
- Class: rviz_default_plugins/Select
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 2.0
|
||||
Focal Point:
|
||||
X: -0.1
|
||||
Y: 0.25
|
||||
Z: 0.30
|
||||
Name: Current View
|
||||
Pitch: 0.5
|
||||
Target Frame: base_link
|
||||
Yaw: -0.623
|
||||
Window Geometry:
|
||||
Height: 975
|
||||
QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Width: 1200
|
||||
47
src/dual_arm_moveit_config/config/moveit_controllers.yaml
Normal file
47
src/dual_arm_moveit_config/config/moveit_controllers.yaml
Normal file
@@ -0,0 +1,47 @@
|
||||
# MoveIt uses this configuration for controller management
|
||||
|
||||
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
|
||||
|
||||
moveit_simple_controller_manager:
|
||||
controller_names:
|
||||
- left_arm_controller
|
||||
- right_arm_controller
|
||||
# - dual_arm_controller
|
||||
|
||||
left_arm_controller:
|
||||
type: FollowJointTrajectory
|
||||
joints:
|
||||
- left_arm_joint_1
|
||||
- left_arm_joint_2
|
||||
- left_arm_joint_3
|
||||
- left_arm_joint_4
|
||||
- left_arm_joint_5
|
||||
- left_arm_joint_6
|
||||
action_ns: follow_joint_trajectory
|
||||
default: true
|
||||
right_arm_controller:
|
||||
type: FollowJointTrajectory
|
||||
joints:
|
||||
- right_arm_joint_1
|
||||
- right_arm_joint_2
|
||||
- right_arm_joint_3
|
||||
- right_arm_joint_4
|
||||
- right_arm_joint_5
|
||||
- right_arm_joint_6
|
||||
action_ns: follow_joint_trajectory
|
||||
default: false
|
||||
# dual_arm_controller:
|
||||
# type: FollowJointTrajectory
|
||||
# joints:
|
||||
# - left_arm_joint_1
|
||||
# - left_arm_joint_2
|
||||
# - left_arm_joint_3
|
||||
# - left_arm_joint_4
|
||||
# - left_arm_joint_5
|
||||
# - left_arm_joint_6
|
||||
# - right_arm_joint_1
|
||||
# - right_arm_joint_2
|
||||
# - right_arm_joint_3
|
||||
# - right_arm_joint_4
|
||||
# - right_arm_joint_5
|
||||
# - right_arm_joint_6
|
||||
@@ -0,0 +1,6 @@
|
||||
# Limits for the Pilz planner
|
||||
cartesian_limits:
|
||||
max_trans_vel: 1.0
|
||||
max_trans_acc: 2.25
|
||||
max_trans_dec: -5.0
|
||||
max_rot_vel: 1.57
|
||||
82
src/dual_arm_moveit_config/config/ros2_controllers.yaml
Normal file
82
src/dual_arm_moveit_config/config/ros2_controllers.yaml
Normal file
@@ -0,0 +1,82 @@
|
||||
# This config file is used by ros2_control
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 100 # Hz
|
||||
|
||||
left_arm_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
|
||||
right_arm_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
|
||||
# dual_arm_controller:
|
||||
# type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
left_arm_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- left_arm_joint_1
|
||||
- left_arm_joint_2
|
||||
- left_arm_joint_3
|
||||
- left_arm_joint_4
|
||||
- left_arm_joint_5
|
||||
- left_arm_joint_6
|
||||
command_interfaces:
|
||||
- position
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
state_publish_rate: 50.0
|
||||
action_monitor_rate: 20.0
|
||||
allow_partial_joints_goal: false
|
||||
stop_trajectory_duration: 0.25
|
||||
constraints:
|
||||
goal_time: 0.5
|
||||
stopped_velocity_tolerance: 0.01
|
||||
right_arm_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- right_arm_joint_1
|
||||
- right_arm_joint_2
|
||||
- right_arm_joint_3
|
||||
- right_arm_joint_4
|
||||
- right_arm_joint_5
|
||||
- right_arm_joint_6
|
||||
command_interfaces:
|
||||
- position
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
state_publish_rate: 50.0
|
||||
action_monitor_rate: 20.0
|
||||
allow_partial_joints_goal: false
|
||||
stop_trajectory_duration: 0.25
|
||||
constraints:
|
||||
goal_time: 0.5
|
||||
stopped_velocity_tolerance: 0.01
|
||||
# dual_arm_controller:
|
||||
# ros__parameters:
|
||||
# joints:
|
||||
# - left_arm_joint_1
|
||||
# - left_arm_joint_2
|
||||
# - left_arm_joint_3
|
||||
# - left_arm_joint_4
|
||||
# - left_arm_joint_5
|
||||
# - left_arm_joint_6
|
||||
# - right_arm_joint_1
|
||||
# - right_arm_joint_2
|
||||
# - right_arm_joint_3
|
||||
# - right_arm_joint_4
|
||||
# - right_arm_joint_5
|
||||
# - right_arm_joint_6
|
||||
# command_interfaces:
|
||||
# - position
|
||||
# state_interfaces:
|
||||
# - position
|
||||
# - velocity
|
||||
26
src/dual_arm_moveit_config/config/sensors_3d.yaml
Normal file
26
src/dual_arm_moveit_config/config/sensors_3d.yaml
Normal file
@@ -0,0 +1,26 @@
|
||||
sensors:
|
||||
[]
|
||||
# - default_sensor
|
||||
# - kinect_depthimage
|
||||
# default_sensor:
|
||||
# far_clipping_plane_distance: 5.0
|
||||
# filtered_cloud_topic: filtered_cloud
|
||||
# image_topic: /head_mount_kinect/depth_registered/image_raw
|
||||
# max_update_rate: 1.0
|
||||
# near_clipping_plane_distance: 0.3
|
||||
# padding_offset: 0.03
|
||||
# padding_scale: 4.0
|
||||
# queue_size: 5
|
||||
# sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
|
||||
# shadow_threshold: 0.2
|
||||
# kinect_depthimage:
|
||||
# far_clipping_plane_distance: 5.0
|
||||
# filtered_cloud_topic: filtered_cloud
|
||||
# image_topic: /head_mount_kinect/depth_registered/image_raw
|
||||
# max_update_rate: 1.0
|
||||
# near_clipping_plane_distance: 0.3
|
||||
# padding_offset: 0.03
|
||||
# padding_scale: 4.0
|
||||
# queue_size: 5
|
||||
# sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
|
||||
# shadow_threshold: 0.2
|
||||
7
src/dual_arm_moveit_config/launch/demo.launch.py
Normal file
7
src/dual_arm_moveit_config/launch/demo.launch.py
Normal file
@@ -0,0 +1,7 @@
|
||||
from moveit_configs_utils import MoveItConfigsBuilder
|
||||
from moveit_configs_utils.launches import generate_demo_launch
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
moveit_config = MoveItConfigsBuilder("dual_arm", package_name="dual_arm_moveit_config").to_moveit_configs()
|
||||
return generate_demo_launch(moveit_config)
|
||||
7
src/dual_arm_moveit_config/launch/move_group.launch.py
Normal file
7
src/dual_arm_moveit_config/launch/move_group.launch.py
Normal file
@@ -0,0 +1,7 @@
|
||||
from moveit_configs_utils import MoveItConfigsBuilder
|
||||
from moveit_configs_utils.launches import generate_move_group_launch
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
moveit_config = MoveItConfigsBuilder("dual_arm", package_name="dual_arm_moveit_config").to_moveit_configs()
|
||||
return generate_move_group_launch(moveit_config)
|
||||
@@ -0,0 +1,31 @@
|
||||
from moveit_configs_utils import MoveItConfigsBuilder
|
||||
from moveit_configs_utils.launches import generate_move_group_launch, generate_moveit_rviz_launch
|
||||
from launch import LaunchDescription
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""
|
||||
Launch file for MoveIt move_group node with RViz visualization.
|
||||
|
||||
This launch file starts both the move_group node and RViz for visualization
|
||||
and debugging. Use this when you need to visualize robot motion planning
|
||||
and execution in RViz.
|
||||
|
||||
Usage:
|
||||
ros2 launch dual_arm_moveit_config move_group_with_rviz.launch.py
|
||||
"""
|
||||
# Build MoveIt configuration
|
||||
moveit_config = MoveItConfigsBuilder("dual_arm", package_name="dual_arm_moveit_config").to_moveit_configs()
|
||||
|
||||
# Generate move_group launch description
|
||||
move_group_launch_description = generate_move_group_launch(moveit_config)
|
||||
|
||||
# Generate RViz launch description
|
||||
rviz_launch_description = generate_moveit_rviz_launch(moveit_config)
|
||||
|
||||
# Combine both launch descriptions
|
||||
return LaunchDescription([
|
||||
move_group_launch_description,
|
||||
rviz_launch_description
|
||||
])
|
||||
|
||||
7
src/dual_arm_moveit_config/launch/moveit_rviz.launch.py
Normal file
7
src/dual_arm_moveit_config/launch/moveit_rviz.launch.py
Normal file
@@ -0,0 +1,7 @@
|
||||
from moveit_configs_utils import MoveItConfigsBuilder
|
||||
from moveit_configs_utils.launches import generate_moveit_rviz_launch
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
moveit_config = MoveItConfigsBuilder("dual_arm", package_name="dual_arm_moveit_config").to_moveit_configs()
|
||||
return generate_moveit_rviz_launch(moveit_config)
|
||||
7
src/dual_arm_moveit_config/launch/rsp.launch.py
Normal file
7
src/dual_arm_moveit_config/launch/rsp.launch.py
Normal file
@@ -0,0 +1,7 @@
|
||||
from moveit_configs_utils import MoveItConfigsBuilder
|
||||
from moveit_configs_utils.launches import generate_rsp_launch
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
moveit_config = MoveItConfigsBuilder("dual_arm", package_name="dual_arm_moveit_config").to_moveit_configs()
|
||||
return generate_rsp_launch(moveit_config)
|
||||
@@ -0,0 +1,7 @@
|
||||
from moveit_configs_utils import MoveItConfigsBuilder
|
||||
from moveit_configs_utils.launches import generate_setup_assistant_launch
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
moveit_config = MoveItConfigsBuilder("dual_arm", package_name="dual_arm_moveit_config").to_moveit_configs()
|
||||
return generate_setup_assistant_launch(moveit_config)
|
||||
@@ -0,0 +1,7 @@
|
||||
from moveit_configs_utils import MoveItConfigsBuilder
|
||||
from moveit_configs_utils.launches import generate_spawn_controllers_launch
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
moveit_config = MoveItConfigsBuilder("dual_arm", package_name="dual_arm_moveit_config").to_moveit_configs()
|
||||
return generate_spawn_controllers_launch(moveit_config)
|
||||
@@ -0,0 +1,7 @@
|
||||
from moveit_configs_utils import MoveItConfigsBuilder
|
||||
from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
moveit_config = MoveItConfigsBuilder("dual_arm", package_name="dual_arm_moveit_config").to_moveit_configs()
|
||||
return generate_static_virtual_joint_tfs_launch(moveit_config)
|
||||
7
src/dual_arm_moveit_config/launch/warehouse_db.launch.py
Normal file
7
src/dual_arm_moveit_config/launch/warehouse_db.launch.py
Normal file
@@ -0,0 +1,7 @@
|
||||
from moveit_configs_utils import MoveItConfigsBuilder
|
||||
from moveit_configs_utils.launches import generate_warehouse_db_launch
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
moveit_config = MoveItConfigsBuilder("dual_arm", package_name="dual_arm_moveit_config").to_moveit_configs()
|
||||
return generate_warehouse_db_launch(moveit_config)
|
||||
52
src/dual_arm_moveit_config/package.xml
Normal file
52
src/dual_arm_moveit_config/package.xml
Normal file
@@ -0,0 +1,52 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>dual_arm_moveit_config</name>
|
||||
<version>0.3.0</version>
|
||||
<description>
|
||||
An automatically generated package with all the configuration and launch files for using the dual_arm with the MoveIt Motion Planning Framework
|
||||
</description>
|
||||
<maintainer email="ray@hivecore.cn">ray</maintainer>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<url type="website">http://moveit.ros.org/</url>
|
||||
<url type="bugtracker">https://github.com/ros-planning/moveit2/issues</url>
|
||||
<url type="repository">https://github.com/ros-planning/moveit2</url>
|
||||
|
||||
<author email="ray@hivecore.cn">ray</author>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<exec_depend>moveit_ros_move_group</exec_depend>
|
||||
<exec_depend>moveit_kinematics</exec_depend>
|
||||
<exec_depend>moveit_planners</exec_depend>
|
||||
<exec_depend>moveit_simple_controller_manager</exec_depend>
|
||||
<exec_depend>joint_state_publisher</exec_depend>
|
||||
<exec_depend>joint_state_publisher_gui</exec_depend>
|
||||
<exec_depend>tf2_ros</exec_depend>
|
||||
<exec_depend>xacro</exec_depend>
|
||||
<!-- The next 2 packages are required for the gazebo simulation.
|
||||
We don't include them by default to prevent installing gazebo and all its dependencies. -->
|
||||
<!-- <exec_depend>joint_trajectory_controller</exec_depend> -->
|
||||
<!-- <exec_depend>gazebo_ros_control</exec_depend> -->
|
||||
<exec_depend>controller_manager</exec_depend>
|
||||
<exec_depend>dual_arm_description</exec_depend>
|
||||
<exec_depend>moveit_configs_utils</exec_depend>
|
||||
<exec_depend>moveit_ros_move_group</exec_depend>
|
||||
<exec_depend>moveit_ros_visualization</exec_depend>
|
||||
<exec_depend>moveit_ros_warehouse</exec_depend>
|
||||
<exec_depend>moveit_setup_assistant</exec_depend>
|
||||
<exec_depend>robot_state_publisher</exec_depend>
|
||||
<exec_depend>rviz2</exec_depend>
|
||||
<exec_depend>rviz_common</exec_depend>
|
||||
<exec_depend>rviz_default_plugins</exec_depend>
|
||||
<exec_depend>tf2_ros</exec_depend>
|
||||
<exec_depend>warehouse_ros_mongo</exec_depend>
|
||||
<exec_depend>xacro</exec_depend>
|
||||
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -1,5 +1,16 @@
|
||||
cmake_minimum_required(VERSION 3.16) # 提升最低版本以匹配ROS 2推荐版本
|
||||
project(robot_control)
|
||||
set(ROS2_HUMBLE_PATH /opt/ros/humble)
|
||||
# 手动指定 eigen_stl_containers 的cmake配置文件路径
|
||||
set(eigen_stl_containers_DIR ${ROS2_HUMBLE_PATH}/share/eigen_stl_containers/cmake)
|
||||
# 手动指定 eigen3 相关依赖路径 (moveit_core同时依赖这个)
|
||||
set(eigen3_cmake_module_DIR ${ROS2_HUMBLE_PATH}/share/eigen3_cmake_module/cmake)
|
||||
set(tf2_eigen_DIR ${ROS2_HUMBLE_PATH}/share/tf2_eigen/cmake)
|
||||
# 手动指定 moveit_core 路径（双重保险）
|
||||
set(moveit_core_DIR ${ROS2_HUMBLE_PATH}/share/moveit_core/cmake)
|
||||
# 设置 C++17 标准（std::filesystem 需要）
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
@@ -13,10 +24,22 @@ find_package(geometry_msgs REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(trajectory_msgs REQUIRED)
|
||||
find_package(control_msgs REQUIRED)
|
||||
find_package(controller_manager_msgs REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(action_msgs REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
find_package(interfaces REQUIRED)
|
||||
find_package(moveit_core REQUIRED)
|
||||
find_package(moveit_ros_planning_interface REQUIRED)
|
||||
find_package(moveit_msgs REQUIRED)
|
||||
|
||||
find_package(eigen_stl_containers REQUIRED)
|
||||
find_package(eigen3_cmake_module REQUIRED)
|
||||
find_package(tf2_eigen REQUIRED)
|
||||
find_package(moveit_ros_planning REQUIRED)
|
||||
find_package(moveit_ros_planning_interface REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(moveit_msgs REQUIRED)
|
||||
|
||||
# 头文件目录
|
||||
include_directories(
|
||||
@@ -27,15 +50,30 @@ include_directories(
|
||||
|
||||
# 源文件列表
|
||||
set(SOURCES
|
||||
src/trapezoidal_trajectory.cpp
|
||||
src/robot_control_manager.cpp
|
||||
src/robot_control_node.cpp
|
||||
src/arm_control.cpp
|
||||
src/leg_control.cpp
|
||||
src/robot_model.cpp
|
||||
src/wheel_control.cpp
|
||||
src/waist_control.cpp
|
||||
src/control_base.cpp
|
||||
src/core/robot_control_node.cpp
|
||||
src/core/robot_control_manager.cpp
|
||||
src/core/controller_factory.cpp
|
||||
src/core/remote_controller.cpp
|
||||
src/services/motor_service.cpp
|
||||
src/services/safety_service.cpp
|
||||
src/services/arm_hardware_service.cpp
|
||||
src/services/kinematics_service.cpp
|
||||
src/services/arm_status_service.cpp
|
||||
src/core/motion_parameters.cpp
|
||||
src/actions/action_manager.cpp
|
||||
src/actions/move_home_action.cpp
|
||||
src/actions/move_leg_action.cpp
|
||||
src/actions/move_waist_action.cpp
|
||||
src/actions/move_wheel_action.cpp
|
||||
src/actions/dual_arm_action.cpp
|
||||
src/controllers/control_base.cpp
|
||||
src/controllers/leg_control.cpp
|
||||
src/controllers/arm_control.cpp
|
||||
src/controllers/wheel_control.cpp
|
||||
src/controllers/waist_control.cpp
|
||||
src/utils/trapezoidal_trajectory.cpp
|
||||
src/utils/s_curve_trajectory.cpp
|
||||
src/utils/trajectory_blend.cpp
|
||||
src/main.cpp
|
||||
)
|
||||
|
||||
@@ -52,18 +90,42 @@ ament_target_dependencies(robot_control_node
|
||||
sensor_msgs
|
||||
trajectory_msgs
|
||||
control_msgs
|
||||
controller_manager_msgs
|
||||
rclcpp_action
|
||||
interfaces
|
||||
moveit_core
|
||||
moveit_ros_planning_interface
|
||||
moveit_msgs
|
||||
)
|
||||
|
||||
# 构建 right_arm_joint_test 可执行文件
|
||||
add_executable(right_arm_joint_test
|
||||
src/utils/right_arm_joint_test.cpp
|
||||
)
|
||||
|
||||
# 链接ROS依赖
|
||||
ament_target_dependencies(right_arm_joint_test
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
moveit_core
|
||||
moveit_ros_planning_interface
|
||||
control_msgs
|
||||
)
|
||||
|
||||
# 安装可执行文件和launch目录
|
||||
install(TARGETS
|
||||
robot_control_node
|
||||
right_arm_joint_test
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
install(DIRECTORY launch
|
||||
DESTINATION share/${PROJECT_NAME}/
|
||||
)
|
||||
|
||||
install(PROGRAMS
|
||||
scripts/dual_arm_action_client.py
|
||||
scripts/run_dual_arm_mock_tests.py
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
|
||||
@@ -1,8 +0,0 @@
|
||||
# 目标 回零点
|
||||
---
|
||||
# 结果：运动成功，或执行完毕
|
||||
bool success
|
||||
string message
|
||||
---
|
||||
# 反馈：各关节位置，运动进度
|
||||
int64[] joint_values
|
||||
@@ -1,9 +0,0 @@
|
||||
# 目标 腿伸长或缩短运动
|
||||
float32 move_up_distance
|
||||
---
|
||||
# 结果：运动成功，或执行完毕
|
||||
bool success
|
||||
string message
|
||||
---
|
||||
# 反馈：各关节角度
|
||||
int64[] joint_values
|
||||
@@ -1,10 +0,0 @@
|
||||
# 目标 腰部运动
|
||||
float32 move_pitch_degree
|
||||
float32 move_yaw_degree
|
||||
---
|
||||
# 结果：运动成功，或执行完毕
|
||||
bool success
|
||||
string message
|
||||
---
|
||||
# 反馈：各关节角度
|
||||
int64[] joint_values
|
||||
@@ -1,11 +0,0 @@
|
||||
# 目标 底盘运动
|
||||
float32 move_distance
|
||||
float32 move_angle
|
||||
---
|
||||
# 结果：运动成功，或执行完毕
|
||||
bool success
|
||||
string message
|
||||
---
|
||||
# 反馈：当前位置，当前角度，运动进度
|
||||
float32 current_pos
|
||||
float32 current_angle
|
||||
25
src/robot_control/include/actions/action_context.hpp
Normal file
25
src/robot_control/include/actions/action_context.hpp
Normal file
@@ -0,0 +1,25 @@
|
||||
/**
|
||||
* @file action_context.hpp
|
||||
* @brief 动作上下文结构体，聚合控制器与服务引用
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
class RobotControlManager;
|
||||
class SafetyService;
|
||||
|
||||
struct ActionContext
|
||||
{
|
||||
rclcpp::Node* node{nullptr};
|
||||
RobotControlManager* robot_control_manager{nullptr};
|
||||
SafetyService* safety_service{nullptr};
|
||||
};
|
||||
} // namespace robot_control
|
||||
|
||||
|
||||
62
src/robot_control/include/actions/action_manager.hpp
Normal file
62
src/robot_control/include/actions/action_manager.hpp
Normal file
@@ -0,0 +1,62 @@
|
||||
/**
|
||||
* @file action_manager.hpp
|
||||
* @brief Thin orchestrator that wires per-action handlers.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
class RobotControlManager;
|
||||
|
||||
class MoveHomeAction;
|
||||
class MoveLegAction;
|
||||
class MoveWaistAction;
|
||||
class MoveWheelAction;
|
||||
class DualArmAction;
|
||||
class MotorService;
|
||||
class SafetyService;
|
||||
|
||||
class ActionManager
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数，初始化动作管理器并持有所需依赖
|
||||
* @param node ROS 2 节点指针
|
||||
* @param robot_control_manager 机器人控制管理器引用
|
||||
* @param safety_service 安全服务共享指针
|
||||
*/
|
||||
ActionManager(
|
||||
rclcpp::Node* node,
|
||||
RobotControlManager& robot_control_manager,
|
||||
const std::shared_ptr<SafetyService>& safety_service);
|
||||
|
||||
/**
|
||||
* @brief 析构函数
|
||||
*/
|
||||
~ActionManager();
|
||||
|
||||
/**
|
||||
* @brief 初始化所有动作服务端
|
||||
*/
|
||||
void Initialize();
|
||||
|
||||
private:
|
||||
rclcpp::Node* node_{nullptr};
|
||||
RobotControlManager& robot_control_manager_;
|
||||
std::shared_ptr<MotorService> motor_service_;
|
||||
std::shared_ptr<SafetyService> safety_service_;
|
||||
|
||||
std::unique_ptr<MoveHomeAction> move_home_;
|
||||
std::unique_ptr<MoveLegAction> move_leg_;
|
||||
std::unique_ptr<MoveWaistAction> move_waist_;
|
||||
std::unique_ptr<MoveWheelAction> move_wheel_;
|
||||
std::unique_ptr<DualArmAction> dual_arm_;
|
||||
};
|
||||
} // namespace robot_control
|
||||
|
||||
|
||||
196
src/robot_control/include/actions/dual_arm_action.hpp
Normal file
196
src/robot_control/include/actions/dual_arm_action.hpp
Normal file
@@ -0,0 +1,196 @@
|
||||
/**
|
||||
* @file dual_arm_action.hpp
|
||||
* @brief 双臂协调动作服务端接口
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "control_msgs/action/follow_joint_trajectory.hpp"
|
||||
#include "interfaces/action/dual_arm.hpp"
|
||||
#include "trajectory_msgs/msg/joint_trajectory.hpp"
|
||||
#include "geometry_msgs/msg/pose.hpp"
|
||||
|
||||
#include "actions/action_context.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
struct MotionParameters;
|
||||
|
||||
class DualArmAction
|
||||
{
|
||||
public:
|
||||
using DualArm = interfaces::action::DualArm;
|
||||
using GoalHandleDualArm = rclcpp_action::ServerGoalHandle<DualArm>;
|
||||
using FollowJTA = control_msgs::action::FollowJointTrajectory;
|
||||
using GoalHandleFollowJTA = rclcpp_action::ClientGoalHandle<FollowJTA>;
|
||||
|
||||
/**
|
||||
* @brief 构造函数，初始化双臂协调动作服务端
|
||||
* @param ctx 动作上下文（包含节点和控制器引用）
|
||||
*/
|
||||
explicit DualArmAction(const ActionContext& ctx);
|
||||
|
||||
/**
|
||||
* @brief 创建并注册 ROS 2 Action 服务端及左右臂 FJT 客户端
|
||||
*/
|
||||
void Initialize();
|
||||
|
||||
/**
|
||||
* @brief 查询当前是否正在执行双臂协调动作
|
||||
* @return true 正在执行，false 未执行
|
||||
*/
|
||||
bool IsExecuting() const { return executing_.load(); }
|
||||
|
||||
/**
|
||||
* @brief 设置整体执行状态标志
|
||||
* @param v true 表示正在执行，false 表示未执行
|
||||
*/
|
||||
void SetExecuting(bool v) { executing_.store(v); }
|
||||
|
||||
private:
|
||||
rclcpp_action::GoalResponse handle_goal_(
|
||||
const rclcpp_action::GoalUUID& uuid,
|
||||
std::shared_ptr<const DualArm::Goal> goal);
|
||||
|
||||
rclcpp_action::CancelResponse handle_cancel_(
|
||||
const std::shared_ptr<GoalHandleDualArm> goal_handle);
|
||||
|
||||
void handle_accepted_(const std::shared_ptr<GoalHandleDualArm> goal_handle);
|
||||
void execute_(const std::shared_ptr<GoalHandleDualArm> goal_handle);
|
||||
|
||||
bool collect_dual_arm_joint_targets_(
|
||||
const DualArm::Goal& goal,
|
||||
size_t arm_dof,
|
||||
bool* need_left,
|
||||
std::vector<double>* left_target,
|
||||
bool* need_right,
|
||||
std::vector<double>* right_target,
|
||||
std::string* error_msg) const;
|
||||
|
||||
bool validate_movel_pose_(
|
||||
const geometry_msgs::msg::Pose& pose,
|
||||
std::string* error_msg) const;
|
||||
|
||||
bool export_and_normalize_trajectory_(
|
||||
const MotionParameters& motion_params,
|
||||
trajectory_msgs::msg::JointTrajectory* out,
|
||||
double* total_time_sec,
|
||||
std::string* error_msg) const;
|
||||
|
||||
void publish_planning_feedback_(
|
||||
const std::shared_ptr<GoalHandleDualArm>& goal_handle,
|
||||
const std::shared_ptr<DualArm::Feedback>& feedback) const;
|
||||
|
||||
double trajectory_total_time_sec_(
|
||||
const trajectory_msgs::msg::JointTrajectory& traj) const;
|
||||
|
||||
void split_dual_arm_trajectory_(
|
||||
const MotionParameters& motion_params,
|
||||
size_t arm_dof,
|
||||
const trajectory_msgs::msg::JointTrajectory& traj,
|
||||
trajectory_msgs::msg::JointTrajectory* left,
|
||||
trajectory_msgs::msg::JointTrajectory* right) const;
|
||||
|
||||
void execute_single_arm_(
|
||||
const std::shared_ptr<GoalHandleDualArm>& goal_handle,
|
||||
const std::shared_ptr<const DualArm::Goal>& goal,
|
||||
const std::shared_ptr<DualArm::Feedback>& feedback,
|
||||
const std::shared_ptr<DualArm::Result>& result,
|
||||
double acceleration_scaling);
|
||||
|
||||
void execute_dual_arm_(
|
||||
const std::shared_ptr<GoalHandleDualArm>& goal_handle,
|
||||
const std::shared_ptr<const DualArm::Goal>& goal,
|
||||
const std::shared_ptr<DualArm::Feedback>& feedback,
|
||||
const std::shared_ptr<DualArm::Result>& result,
|
||||
const MotionParameters& motion_params,
|
||||
double acceleration_scaling);
|
||||
|
||||
bool wait_for_execution_(
|
||||
const std::shared_ptr<GoalHandleDualArm>& goal_handle,
|
||||
const std::shared_ptr<DualArm::Feedback>& feedback,
|
||||
const std::shared_ptr<DualArm::Result>& result,
|
||||
double total_time_sec,
|
||||
const std::function<bool()>& is_done,
|
||||
const std::function<void()>& cancel_actions,
|
||||
const std::string& cancel_message,
|
||||
bool* canceled) const;
|
||||
|
||||
void cancel_single_arm_fjt_(
|
||||
const rclcpp_action::Client<FollowJTA>::SharedPtr& client,
|
||||
const GoalHandleFollowJTA::SharedPtr& goal_handle_arm,
|
||||
uint8_t arm_id,
|
||||
const std::vector<std::string>& joint_names) const;
|
||||
|
||||
void cancel_dual_arm_fjt_(
|
||||
const GoalHandleFollowJTA::SharedPtr& left_handle,
|
||||
const GoalHandleFollowJTA::SharedPtr& right_handle,
|
||||
const std::vector<std::string>& left_joint_names,
|
||||
const std::vector<std::string>& right_joint_names) const;
|
||||
|
||||
bool run_single_arm_wait_(
|
||||
const std::shared_ptr<GoalHandleDualArm>& goal_handle,
|
||||
const std::shared_ptr<DualArm::Feedback>& feedback,
|
||||
const std::shared_ptr<DualArm::Result>& result,
|
||||
double total_time_sec,
|
||||
const std::shared_future<typename GoalHandleFollowJTA::WrappedResult>& res_future,
|
||||
const rclcpp_action::Client<FollowJTA>::SharedPtr& fjt_client,
|
||||
const GoalHandleFollowJTA::SharedPtr& goal_handle_arm,
|
||||
uint8_t arm_id,
|
||||
const std::vector<std::string>& joint_names,
|
||||
bool* canceled) const;
|
||||
|
||||
bool run_dual_arm_wait_(
|
||||
const std::shared_ptr<GoalHandleDualArm>& goal_handle,
|
||||
const std::shared_ptr<DualArm::Feedback>& feedback,
|
||||
const std::shared_ptr<DualArm::Result>& result,
|
||||
double total_time_sec,
|
||||
const std::shared_future<typename GoalHandleFollowJTA::WrappedResult>& left_res_future,
|
||||
const std::shared_future<typename GoalHandleFollowJTA::WrappedResult>& right_res_future,
|
||||
const GoalHandleFollowJTA::SharedPtr& left_handle,
|
||||
const GoalHandleFollowJTA::SharedPtr& right_handle,
|
||||
const std::vector<std::string>& left_joint_names,
|
||||
const std::vector<std::string>& right_joint_names,
|
||||
bool* canceled) const;
|
||||
|
||||
bool is_estop_latched_() const;
|
||||
|
||||
void save_trajectory_to_file_(
|
||||
const trajectory_msgs::msg::JointTrajectory& traj,
|
||||
const std::string& tag) const;
|
||||
|
||||
ActionContext ctx_;
|
||||
// handle_goal 校验通过后写入，execute 时直接使用，避免重复校验 velocity_scaling
|
||||
double accepted_velocity_scale_{0.0};
|
||||
// 整体 DualArm 执行标志（用于双臂协同运动场景）
|
||||
mutable std::atomic<bool> executing_{false};
|
||||
// 单臂执行标志：允许一只手在运动时，下发另一只手的指令
|
||||
mutable std::atomic<bool> executing_left_{false};
|
||||
mutable std::atomic<bool> executing_right_{false};
|
||||
// MoveIt 规划器占用标志：同一时刻只允许一组规划（单臂或双臂）
|
||||
std::atomic<bool> planner_busy_{false};
|
||||
|
||||
rclcpp_action::Server<DualArm>::SharedPtr server_;
|
||||
|
||||
// 通过左右臂各自的 FollowJointTrajectory action 下发轨迹
|
||||
rclcpp_action::Client<FollowJTA>::SharedPtr left_fjt_client_;
|
||||
rclcpp_action::Client<FollowJTA>::SharedPtr right_fjt_client_;
|
||||
|
||||
bool save_trajectory_enabled_{true};
|
||||
bool trajectory_execution_enabled_{true};
|
||||
bool movel_sync_use_s_curve_progress_{true};
|
||||
double movel_sync_min_dt_sec_{0.01};
|
||||
int movel_sync_max_samples_{500};
|
||||
|
||||
};
|
||||
} // namespace robot_control
|
||||
|
||||
|
||||
64
src/robot_control/include/actions/move_home_action.hpp
Normal file
64
src/robot_control/include/actions/move_home_action.hpp
Normal file
@@ -0,0 +1,64 @@
|
||||
/**
|
||||
* @file move_home_action.hpp
|
||||
* @brief 归零动作服务端接口
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "interfaces/action/move_home.hpp"
|
||||
|
||||
#include "actions/action_context.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
class MoveHomeAction
|
||||
{
|
||||
public:
|
||||
using MoveHome = interfaces::action::MoveHome;
|
||||
using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle<MoveHome>;
|
||||
|
||||
/**
|
||||
* @brief 构造函数，初始化归零动作服务端
|
||||
* @param ctx 动作上下文（包含节点和控制器引用）
|
||||
*/
|
||||
explicit MoveHomeAction(const ActionContext& ctx);
|
||||
|
||||
/**
|
||||
* @brief 创建并注册 ROS 2 Action 服务端
|
||||
*/
|
||||
void Initialize();
|
||||
|
||||
/**
|
||||
* @brief 查询当前是否正在执行归零动作
|
||||
* @return true 正在执行，false 未执行
|
||||
*/
|
||||
bool IsExecuting() const { return executing_.load(); }
|
||||
|
||||
/**
|
||||
* @brief 设置执行状态标志
|
||||
* @param v true 表示正在执行，false 表示未执行
|
||||
*/
|
||||
void SetExecuting(bool v) { executing_.store(v); }
|
||||
|
||||
private:
|
||||
rclcpp_action::GoalResponse handle_goal_(
|
||||
const rclcpp_action::GoalUUID& uuid,
|
||||
std::shared_ptr<const MoveHome::Goal> goal);
|
||||
|
||||
rclcpp_action::CancelResponse handle_cancel_(
|
||||
const std::shared_ptr<GoalHandleMoveHome> goal_handle);
|
||||
|
||||
void handle_accepted_(const std::shared_ptr<GoalHandleMoveHome> goal_handle);
|
||||
void execute_(const std::shared_ptr<GoalHandleMoveHome> goal_handle);
|
||||
|
||||
ActionContext ctx_;
|
||||
std::atomic<bool> executing_{false};
|
||||
rclcpp_action::Server<MoveHome>::SharedPtr server_;
|
||||
};
|
||||
} // namespace robot_control
|
||||
|
||||
|
||||
64
src/robot_control/include/actions/move_leg_action.hpp
Normal file
64
src/robot_control/include/actions/move_leg_action.hpp
Normal file
@@ -0,0 +1,64 @@
|
||||
/**
|
||||
* @file move_leg_action.hpp
|
||||
* @brief 腿部运动动作服务端接口
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "interfaces/action/move_leg.hpp"
|
||||
|
||||
#include "actions/action_context.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
class MoveLegAction
|
||||
{
|
||||
public:
|
||||
using MoveLeg = interfaces::action::MoveLeg;
|
||||
using GoalHandleMoveLeg = rclcpp_action::ServerGoalHandle<MoveLeg>;
|
||||
|
||||
/**
|
||||
* @brief 构造函数，初始化腿部运动动作服务端
|
||||
* @param ctx 动作上下文（包含节点和控制器引用）
|
||||
*/
|
||||
explicit MoveLegAction(const ActionContext& ctx);
|
||||
|
||||
/**
|
||||
* @brief 创建并注册 ROS 2 Action 服务端
|
||||
*/
|
||||
void Initialize();
|
||||
|
||||
/**
|
||||
* @brief 查询当前是否正在执行腿部运动动作
|
||||
* @return true 正在执行，false 未执行
|
||||
*/
|
||||
bool IsExecuting() const { return executing_.load(); }
|
||||
|
||||
/**
|
||||
* @brief 设置执行状态标志
|
||||
* @param v true 表示正在执行，false 表示未执行
|
||||
*/
|
||||
void SetExecuting(bool v) { executing_.store(v); }
|
||||
|
||||
private:
|
||||
rclcpp_action::GoalResponse handle_goal_(
|
||||
const rclcpp_action::GoalUUID& uuid,
|
||||
std::shared_ptr<const MoveLeg::Goal> goal);
|
||||
|
||||
rclcpp_action::CancelResponse handle_cancel_(
|
||||
const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
|
||||
|
||||
void handle_accepted_(const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
|
||||
void execute_(const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
|
||||
|
||||
ActionContext ctx_;
|
||||
std::atomic<bool> executing_{false};
|
||||
rclcpp_action::Server<MoveLeg>::SharedPtr server_;
|
||||
};
|
||||
} // namespace robot_control
|
||||
|
||||
|
||||
64
src/robot_control/include/actions/move_waist_action.hpp
Normal file
64
src/robot_control/include/actions/move_waist_action.hpp
Normal file
@@ -0,0 +1,64 @@
|
||||
/**
|
||||
* @file move_waist_action.hpp
|
||||
* @brief 腰部运动动作服务端接口
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "interfaces/action/move_waist.hpp"
|
||||
|
||||
#include "actions/action_context.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
class MoveWaistAction
|
||||
{
|
||||
public:
|
||||
using MoveWaist = interfaces::action::MoveWaist;
|
||||
using GoalHandleMoveWaist = rclcpp_action::ServerGoalHandle<MoveWaist>;
|
||||
|
||||
/**
|
||||
* @brief 构造函数，初始化腰部运动动作服务端
|
||||
* @param ctx 动作上下文（包含节点和控制器引用）
|
||||
*/
|
||||
explicit MoveWaistAction(const ActionContext& ctx);
|
||||
|
||||
/**
|
||||
* @brief 创建并注册 ROS 2 Action 服务端
|
||||
*/
|
||||
void Initialize();
|
||||
|
||||
/**
|
||||
* @brief 查询当前是否正在执行腰部运动动作
|
||||
* @return true 正在执行，false 未执行
|
||||
*/
|
||||
bool IsExecuting() const { return executing_.load(); }
|
||||
|
||||
/**
|
||||
* @brief 设置执行状态标志
|
||||
* @param v true 表示正在执行，false 表示未执行
|
||||
*/
|
||||
void SetExecuting(bool v) { executing_.store(v); }
|
||||
|
||||
private:
|
||||
rclcpp_action::GoalResponse handle_goal_(
|
||||
const rclcpp_action::GoalUUID& uuid,
|
||||
std::shared_ptr<const MoveWaist::Goal> goal);
|
||||
|
||||
rclcpp_action::CancelResponse handle_cancel_(
|
||||
const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
|
||||
|
||||
void handle_accepted_(const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
|
||||
void execute_(const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
|
||||
|
||||
ActionContext ctx_;
|
||||
std::atomic<bool> executing_{false};
|
||||
rclcpp_action::Server<MoveWaist>::SharedPtr server_;
|
||||
};
|
||||
} // namespace robot_control
|
||||
|
||||
|
||||
71
src/robot_control/include/actions/move_wheel_action.hpp
Normal file
71
src/robot_control/include/actions/move_wheel_action.hpp
Normal file
@@ -0,0 +1,71 @@
|
||||
/**
|
||||
* @file move_wheel_action.hpp
|
||||
* @brief 轮式运动动作服务端接口
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "interfaces/action/move_wheel.hpp"
|
||||
|
||||
#include "actions/action_context.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
|
||||
class MotorService;
|
||||
|
||||
class MoveWheelAction
|
||||
{
|
||||
public:
|
||||
using MoveWheel = interfaces::action::MoveWheel;
|
||||
using GoalHandleMoveWheel = rclcpp_action::ServerGoalHandle<MoveWheel>;
|
||||
|
||||
/**
|
||||
* @brief 构造函数，初始化轮式运动动作服务端
|
||||
* @param ctx 动作上下文（包含节点和控制器引用）
|
||||
* @param motor_service 电机服务共享指针
|
||||
*/
|
||||
MoveWheelAction(
|
||||
const ActionContext& ctx,
|
||||
std::shared_ptr<MotorService> motor_service);
|
||||
|
||||
/**
|
||||
* @brief 创建并注册 ROS 2 Action 服务端
|
||||
*/
|
||||
void Initialize();
|
||||
|
||||
/**
|
||||
* @brief 查询当前是否正在执行轮式运动动作
|
||||
* @return true 正在执行，false 未执行
|
||||
*/
|
||||
bool IsExecuting() const { return executing_.load(); }
|
||||
|
||||
/**
|
||||
* @brief 设置执行状态标志
|
||||
* @param v true 表示正在执行，false 表示未执行
|
||||
*/
|
||||
void SetExecuting(bool v) { executing_.store(v); }
|
||||
|
||||
private:
|
||||
rclcpp_action::GoalResponse handle_goal_(
|
||||
const rclcpp_action::GoalUUID& uuid,
|
||||
std::shared_ptr<const MoveWheel::Goal> goal);
|
||||
|
||||
rclcpp_action::CancelResponse handle_cancel_(
|
||||
const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
|
||||
|
||||
void handle_accepted_(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
|
||||
void execute_(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
|
||||
|
||||
ActionContext ctx_;
|
||||
std::shared_ptr<MotorService> motor_service_;
|
||||
std::atomic<bool> executing_{false};
|
||||
rclcpp_action::Server<MoveWheel>::SharedPtr server_;
|
||||
};
|
||||
} // namespace robot_control
|
||||
|
||||
|
||||
@@ -1,32 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <Eigen/Dense>
|
||||
#include "control_base.hpp" // 包含父类头文件
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
class ArmControl : public ControlBase
|
||||
{
|
||||
public:
|
||||
ArmControl(
|
||||
size_t total_joints,
|
||||
const std::vector<double>& lengthParameters,
|
||||
const std::vector<double>& maxSpeed,
|
||||
const std::vector<double>& maxAcc,
|
||||
const std::vector<double>& minLimits,
|
||||
const std::vector<double>& maxLimits,
|
||||
const std::vector<double>& home_positions,
|
||||
const std::vector<double>& zero_positions,
|
||||
const std::vector<int>& joint_directions
|
||||
);
|
||||
|
||||
~ArmControl() override;
|
||||
|
||||
bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
|
||||
|
||||
bool MoveUp(std::vector<double>& joint_positions, double dt);
|
||||
|
||||
bool MoveDown(std::vector<double>& joint_positions, double dt);
|
||||
};
|
||||
|
||||
}
|
||||
@@ -1,14 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
/**
|
||||
* @brief 运动模块枚举
|
||||
*/
|
||||
enum class MovementPart {
|
||||
LEG,
|
||||
WAIST,
|
||||
WHEEL,
|
||||
ALL
|
||||
};
|
||||
}
|
||||
@@ -1,89 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <Eigen/Dense>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "trapezoidal_trajectory.hpp"
|
||||
#include "motion_parameters.hpp"
|
||||
|
||||
#define POSITION_TOLERANCE 1.0
|
||||
#define TIME_OUT_COUNT 10000
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
class ControlBase
|
||||
{
|
||||
// 重要：将成员变量改为 protected，子类可访问但外部不可见
|
||||
protected:
|
||||
size_t total_joints_; // 总关节数
|
||||
std::vector<double> lengthParameters_; // 机械参数（子类可复用）
|
||||
std::vector<double> maxSpeed_; // 各关节最大速度
|
||||
std::vector<double> maxAcc_; // 各关节最大加速度
|
||||
|
||||
double stopDurationTime_;
|
||||
double movingDurationTime_;
|
||||
|
||||
TrapezoidalTrajectory* trapezoidalTrajectory_; // 轨迹规划器（子类直接用）
|
||||
|
||||
std::vector<double> joint_home_positions_; // 回home点位置
|
||||
std::vector<double> joint_zero_positions_; // 零点位置
|
||||
std::vector<int> joint_directions_; // 关节运动方向
|
||||
std::vector<double> joint_position_; // 当前关节位置
|
||||
std::vector<double> joint_commands_; // 当前关节指令
|
||||
std::vector<double> joint_velocity_; // 当前关节速度
|
||||
std::vector<double> joint_velocity_commands_; // 当前关节速度指令
|
||||
std::vector<double> joint_acceleration_; // 当前关节加速度
|
||||
std::vector<double> joint_torque_; // 当前关节力矩
|
||||
std::vector<double> minLimits_; // 关节位置下限
|
||||
std::vector<double> maxLimits_; // 关节位置上限
|
||||
std::vector<double> joint_position_desired_; // 期望关节位置
|
||||
|
||||
bool is_moving_; // 是否运动中
|
||||
bool is_stopping_; // 是否停止中
|
||||
bool is_target_set_; // 是否已设置目标点
|
||||
bool is_cmd_send_finished_;
|
||||
bool is_joint_position_initialized_; // 是否已初始化关节位置
|
||||
|
||||
int time_out_count_; // 超时时间
|
||||
public:
|
||||
// 构造函数（子类需调用此构造函数初始化父类）
|
||||
ControlBase(
|
||||
size_t total_joints,
|
||||
const std::vector<double>& lengthParameters,
|
||||
const std::vector<double>& maxSpeed,
|
||||
const std::vector<double>& maxAcc,
|
||||
const std::vector<double>& minLimits,
|
||||
const std::vector<double>& maxLimits,
|
||||
const std::vector<double>& home_positions,
|
||||
const std::vector<double>& zero_positions,
|
||||
const std::vector<int>& joint_directions
|
||||
);
|
||||
|
||||
virtual ~ControlBase();
|
||||
|
||||
// 需子类重写的函数：声明为 virtual（纯虚函数/普通虚函数）
|
||||
// 1. 笛卡尔空间目标点运动（机械臂需解算运动学，子类重写）
|
||||
virtual bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) = 0;
|
||||
|
||||
// 2. 关节空间目标运动（通用逻辑，父类可实现，子类可重写）
|
||||
virtual bool MoveToTargetJoint(std::vector<double>& joint_positions, const std::vector<double>& target_joint, double duration);
|
||||
|
||||
// 3. 回零点（通用逻辑，父类实现）
|
||||
virtual bool GoHome(std::vector<double>& joint_positions, double dt);
|
||||
|
||||
// 4. 停止运动（通用逻辑，父类实现）
|
||||
virtual bool Stop(std::vector<double>& joint_positions, double dt);
|
||||
|
||||
// 5. 更新关节状态（通用逻辑，父类实现）
|
||||
virtual void UpdateJointStates(const std::vector<double>& joint_positions, const std::vector<double>& joint_velocities, const std::vector<double>& joint_torques);
|
||||
|
||||
// 6. 判断是否运动中（通用逻辑，父类实现）
|
||||
virtual bool IsMoving();
|
||||
|
||||
virtual bool IsReached(const std::vector<double>& target_joint); // 判断是否到达目标点
|
||||
|
||||
virtual void SetHomePositions(const std::vector<double>& home_positions);
|
||||
|
||||
bool checkJointLimits(const std::vector<double>& target_joint);
|
||||
};
|
||||
|
||||
}
|
||||
343
src/robot_control/include/controllers/arm_control.hpp
Normal file
343
src/robot_control/include/controllers/arm_control.hpp
Normal file
@@ -0,0 +1,343 @@
|
||||
/**
|
||||
* @file arm_control.hpp
|
||||
* @brief 手臂控制器 - 控制机器人手臂运动
|
||||
* @author Ray
|
||||
* @date 2025
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "controllers/control_base.hpp"
|
||||
#include "geometry_msgs/msg/pose.hpp"
|
||||
#include "moveit/move_group_interface/move_group_interface.h"
|
||||
#include "trajectory_msgs/msg/joint_trajectory.hpp"
|
||||
#include "utils/s_curve_trajectory.hpp"
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
// 前向声明
|
||||
namespace rclcpp {
|
||||
class Node;
|
||||
}
|
||||
|
||||
namespace moveit {
|
||||
namespace planning_interface {
|
||||
class MoveGroupInterface;
|
||||
}
|
||||
}
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
/**
|
||||
* @class ArmControl
|
||||
* @brief 手臂控制器类
|
||||
*
|
||||
* 继承自ControlBase，使用MoveIt进行轨迹规划。
|
||||
* 结合MotionParameters中的配置信息和MoveIt的URDF/SRDF配置实现手臂控制。
|
||||
*/
|
||||
class ArmControl : public ControlBase
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数
|
||||
* @param joint_names 关节名称列表（左臂+右臂）
|
||||
* @param motion_params MotionParameters 引用，用于获取 JointInfo
|
||||
* @param lengthParameters 长度参数（从MotionParameters中的dual_arm_length_获取，包含左右臂）
|
||||
*/
|
||||
ArmControl(
|
||||
const std::vector<std::string>& joint_names,
|
||||
const std::map<std::string, JointInfo>& joints_info_map,
|
||||
const std::vector<double>& lengthParameters);
|
||||
|
||||
/**
|
||||
* @brief 析构函数
|
||||
*/
|
||||
~ArmControl();
|
||||
|
||||
/**
|
||||
* @brief 初始化MoveIt（需要在ROS节点创建后调用）
|
||||
* 初始化三个MoveGroup: arm_left, arm_right, dual_arm
|
||||
* @param node ROS 2节点指针
|
||||
* @return true 所有初始化成功，false 初始化失败
|
||||
*/
|
||||
bool InitializeMoveIt(rclcpp::Node* node);
|
||||
|
||||
/**
|
||||
* @brief 规划关节空间运动（MOVEJ）
|
||||
*
|
||||
* 使用 MoveIt 规划并将完整轨迹存入内部的 TrajectoryState，
|
||||
* 上层通过 GetInterpolatedPoint/HasActiveTrajectory 获取插补点，无需关心具体轨迹点数组。
|
||||
*
|
||||
* @param arm_id 手臂ID（0=左臂，1=右臂）
|
||||
* @param target_joints 目标关节角度（弧度）
|
||||
* @param velocity_scaling 速度缩放因子 [0,1]
|
||||
* @param acceleration_scaling 加速度缩放因子 [0,1]
|
||||
* @return true 规划成功，false 规划失败
|
||||
*/
|
||||
bool PlanJointMotion(
|
||||
uint8_t arm_id,
|
||||
const std::vector<double>& target_joints,
|
||||
double velocity_scaling,
|
||||
double acceleration_scaling);
|
||||
|
||||
/**
|
||||
* @brief 导出最近一次规划得到的 JointTrajectory（包含 time/pos/vel/acc）
|
||||
* @param arm_id 0=左臂，1=右臂，2=双臂
|
||||
* @param out 输出：规划轨迹
|
||||
*/
|
||||
bool ExportPlannedJointTrajectory(uint8_t arm_id, trajectory_msgs::msg::JointTrajectory& out) const;
|
||||
|
||||
/**
|
||||
* @brief 规划笛卡尔空间运动（MOVEL）
|
||||
*
|
||||
* 使用 MoveIt 规划并将完整轨迹存入内部的 TrajectoryState，
|
||||
* 上层通过 GetInterpolatedPoint/HasActiveTrajectory 获取插补点。
|
||||
*
|
||||
* @param arm_id 手臂ID（0=左臂，1=右臂）
|
||||
* @param target_pose 目标位姿
|
||||
* @param velocity_scaling 速度缩放因子 [0,1]
|
||||
* @param acceleration_scaling 加速度缩放因子 [0,1]
|
||||
* @return true 规划成功，false 规划失败
|
||||
*/
|
||||
bool PlanCartesianMotion(
|
||||
uint8_t arm_id,
|
||||
const geometry_msgs::msg::Pose& target_pose,
|
||||
double velocity_scaling,
|
||||
double acceleration_scaling);
|
||||
|
||||
/**
|
||||
* @brief 仅进行逆解/可达性求解，不写入执行轨迹
|
||||
* @param arm_id 手臂ID（0=左臂，1=右臂）
|
||||
* @param target_pose 目标位姿
|
||||
* @param out_joint_angles 输出关节角（弧度）
|
||||
* @param error_msg 输出错误信息（可选）
|
||||
* @return true 求解成功，false 失败
|
||||
*/
|
||||
bool SolveInverseKinematics(
|
||||
uint8_t arm_id,
|
||||
const geometry_msgs::msg::Pose& target_pose,
|
||||
std::vector<double>* out_joint_angles,
|
||||
std::string* error_msg = nullptr) const;
|
||||
|
||||
/**
|
||||
* @brief 获取当前末端位姿（MoveGroup 当前状态）
|
||||
* @param arm_id 手臂ID（0=左臂，1=右臂）
|
||||
* @param out_pose 输出位姿
|
||||
* @param error_msg 输出错误信息（可选）
|
||||
* @return true 获取成功，false 失败
|
||||
*/
|
||||
bool GetCurrentEndEffectorPose(
|
||||
uint8_t arm_id,
|
||||
geometry_msgs::msg::Pose* out_pose,
|
||||
std::string* error_msg = nullptr) const;
|
||||
|
||||
/**
|
||||
* @brief 根据给定关节角计算当前臂末端位姿（FK）
|
||||
* @param arm_id 手臂ID（0=左臂，1=右臂）
|
||||
* @param joint_values 该臂关节角（弧度）
|
||||
* @param out_pose 输出位姿
|
||||
* @param error_msg 输出错误信息（可选）
|
||||
* @return true 成功，false 失败
|
||||
*/
|
||||
bool ComputeEndEffectorPoseFromJointValues(
|
||||
uint8_t arm_id,
|
||||
const std::vector<double>& joint_values,
|
||||
geometry_msgs::msg::Pose* out_pose,
|
||||
std::string* error_msg = nullptr) const;
|
||||
|
||||
/**
|
||||
* @brief 检查MoveIt是否已初始化
|
||||
* @return true 所有move group已初始化，false 未初始化
|
||||
*/
|
||||
bool IsMoveItInitialized() const;
|
||||
|
||||
// ==================== 重写ControlBase的方法（采用多态方式处理） ====================
|
||||
|
||||
/**
|
||||
* @brief 移动到目标关节位置（重写ControlBase的方法）
|
||||
* 使用MoveIt规划轨迹，然后通过插补执行。如果MoveIt未初始化，回退到基类实现。
|
||||
* @param joint_positions 输出：当前关节位置（会被更新）
|
||||
* @param target_joint 目标关节位置（弧度）
|
||||
* @param dt 时间步长（秒）
|
||||
* @return true 到达目标，false 运动进行中
|
||||
*/
|
||||
bool MoveToTargetJoint(
|
||||
std::vector<double>& joint_positions,
|
||||
const std::vector<double>& target_joint,
|
||||
double dt);
|
||||
|
||||
/**
|
||||
* @brief 回零运动（重写ControlBase的方法）
|
||||
* 使用MoveIt规划轨迹，然后通过插补执行。如果MoveIt未初始化，回退到基类实现。
|
||||
* @param joint_positions 输出：当前关节位置（会被更新）
|
||||
* @param dt 时间步长（秒）
|
||||
* @return true 回零完成，false 回零进行中
|
||||
*/
|
||||
bool GoHome(std::vector<double>& joint_positions, double dt);
|
||||
|
||||
/**
|
||||
* @brief 停止运动（重写ControlBase的方法）
|
||||
* 清除MoveIt轨迹并调用基类的停止方法。
|
||||
* @param joint_positions 输出：当前关节位置（会被更新）
|
||||
* @param dt 时间步长（秒）
|
||||
* @return true 完全停止，false 正在停止
|
||||
*/
|
||||
bool Stop(std::vector<double>& joint_positions, double dt);
|
||||
|
||||
// ==================== MoveIt特定功能 ====================
|
||||
|
||||
/**
|
||||
* @brief 获取当前插补点位（根据时间周期获取下一个点位）
|
||||
* @param arm_id 手臂ID（0=左臂，1=右臂）
|
||||
* @param dt 时间步长（秒）
|
||||
* @param joint_positions 输出：当前插补点位（关节角度，弧度）
|
||||
* @return true 还有更多点位，false 轨迹执行完成
|
||||
*/
|
||||
bool GetInterpolatedPoint(uint8_t arm_id, double dt, std::vector<double>& joint_positions);
|
||||
|
||||
/**
|
||||
* @brief 检查是否有正在执行的轨迹
|
||||
* @param arm_id 手臂ID（0=左臂，1=右臂）
|
||||
* @return true 有轨迹在执行，false 没有轨迹
|
||||
*/
|
||||
bool HasActiveTrajectory(uint8_t arm_id) const;
|
||||
|
||||
/**
|
||||
* @brief 获取该臂当前缓存轨迹的末端关节位置（用于判断是否到达目标）
|
||||
* @param arm_id 手臂ID（0=左臂，1=右臂）
|
||||
* @param end_positions 输出：末端点关节位置（弧度）
|
||||
* @return true 获取成功，false 无轨迹或参数错误
|
||||
*/
|
||||
bool GetTrajectoryEndPositions(uint8_t arm_id, std::vector<double>& end_positions) const;
|
||||
|
||||
/**
|
||||
* @brief 获取该臂当前轨迹的时间进度 [0,1]
|
||||
* @param arm_id 手臂ID（0=左臂，1=右臂）
|
||||
* @return 进度（无轨迹返回 1.0；有轨迹按 elapsed_time/total_time 计算并 clamp 到 [0,1]）
|
||||
*/
|
||||
double GetTrajectoryProgress(uint8_t arm_id) const;
|
||||
|
||||
/**
|
||||
* @brief 清除轨迹
|
||||
* @param arm_id 手臂ID（0=左臂，1=右臂）
|
||||
*/
|
||||
void ClearTrajectory(uint8_t arm_id);
|
||||
|
||||
private:
|
||||
// MoveGroup接口（三个move group）
|
||||
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_left_; ///< 左臂MoveGroup接口
|
||||
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_right_; ///< 右臂MoveGroup接口
|
||||
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_dual_; ///< 双臂MoveGroup接口
|
||||
|
||||
rclcpp::Node* node_; ///< ROS节点指针（用于MoveIt初始化）
|
||||
|
||||
uint8_t current_arm_id_; ///< 当前控制的臂ID（0=左臂，1=右臂，2=双臂）
|
||||
|
||||
// 是否启用 S 型曲线时间参数化（默认关闭，便于对比测试）
|
||||
bool enable_s_curve_time_parameterization_ {true};
|
||||
|
||||
/**
|
||||
* @brief 根据arm_id获取对应的MoveGroup接口
|
||||
* @param arm_id 手臂ID（0=左臂，1=右臂，2=双臂）
|
||||
* @return MoveGroup接口指针，如果arm_id无效返回nullptr
|
||||
*/
|
||||
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> GetMoveGroup(uint8_t arm_id) const;
|
||||
|
||||
/**
|
||||
* @brief 将度数转换为弧度
|
||||
* @param degrees 度数
|
||||
* @return 弧度
|
||||
*/
|
||||
static double DegToRad(double degrees) { return degrees * M_PI / 180.0; }
|
||||
|
||||
/**
|
||||
* @brief 将弧度转换为度数
|
||||
* @param radians 弧度
|
||||
* @return 度数
|
||||
*/
|
||||
static double RadToDeg(double radians) { return radians * 180.0 / M_PI; }
|
||||
|
||||
// 轨迹执行相关
|
||||
struct TrajectoryPoint {
|
||||
std::vector<double> positions; ///< 关节位置（弧度）
|
||||
std::vector<double> velocities; ///< 关节速度（rad/s）
|
||||
std::vector<double> accelerations; ///< 关节加速度（rad/s²）
|
||||
double time_from_start; ///< 从轨迹开始的时间（秒）
|
||||
};
|
||||
|
||||
struct TrajectoryState {
|
||||
std::vector<TrajectoryPoint> trajectory_points; ///< 完整轨迹点信息
|
||||
size_t current_index; ///< 当前轨迹点索引
|
||||
double elapsed_time; ///< 已执行时间
|
||||
bool is_active; ///< 是否激活
|
||||
};
|
||||
TrajectoryState left_arm_trajectory_; ///< 左臂轨迹状态
|
||||
TrajectoryState right_arm_trajectory_; ///< 右臂轨迹状态
|
||||
TrajectoryState dual_arm_trajectory_; ///< 双臂轨迹状态（12关节）
|
||||
|
||||
// 同步保存为 JointTrajectory msg，便于 FollowJointTrajectory 一次性下发
|
||||
trajectory_msgs::msg::JointTrajectory left_arm_traj_msg_;
|
||||
trajectory_msgs::msg::JointTrajectory right_arm_traj_msg_;
|
||||
trajectory_msgs::msg::JointTrajectory dual_arm_traj_msg_;
|
||||
|
||||
/**
|
||||
* @brief 对 MoveIt 规划结果进行时间参数化，并写入对应臂的 TrajectoryState
|
||||
* @param arm_id 手臂ID（0=左臂，1=右臂）
|
||||
* @param move_group 对应 MoveGroupInterface
|
||||
* @param plan MoveIt 规划结果（会被写回带 time_from_start 的轨迹）
|
||||
* @param velocity_scaling 速度缩放因子
|
||||
* @param acceleration_scaling 加速度缩放因子
|
||||
* @return true 成功
|
||||
*/
|
||||
bool timeParameterizeAndStore_(
|
||||
uint8_t arm_id,
|
||||
moveit::planning_interface::MoveGroupInterface::Plan& plan,
|
||||
double velocity_scaling,
|
||||
double acceleration_scaling);
|
||||
|
||||
/**
|
||||
* @brief 基于S型曲线对 MoveIt 轨迹进行时间参数化并重采样
|
||||
* @param arm_id 手臂ID
|
||||
* @param in_traj 输入轨迹（仅位置）
|
||||
* @param out_traj 输出轨迹（包含 time/pos/vel/acc）
|
||||
* @param velocity_scaling 速度缩放因子
|
||||
* @param acceleration_scaling 加速度缩放因子
|
||||
* @return true 成功生成 S 型参数化轨迹
|
||||
*/
|
||||
bool sCurveTimeParameterize_(
|
||||
uint8_t arm_id,
|
||||
const trajectory_msgs::msg::JointTrajectory& in_traj,
|
||||
trajectory_msgs::msg::JointTrajectory& out_traj,
|
||||
double velocity_scaling,
|
||||
double acceleration_scaling) const;
|
||||
|
||||
/**
|
||||
* @brief 辅助函数：更新 joints_info_map_ 从单个轨迹点
|
||||
* @param arm_id 手臂ID（0=左臂，1=右臂）
|
||||
* @param point 轨迹点（仅包含该臂的自由度）
|
||||
*/
|
||||
void UpdateJointsInfo(uint8_t arm_id, const TrajectoryPoint& point);
|
||||
|
||||
/**
|
||||
* @brief 辅助函数：更新 joints_info_map_ 从插补后的值
|
||||
* @param arm_id 手臂ID（0=左臂，1=右臂）
|
||||
* @param p1 起始轨迹点
|
||||
* @param p2 结束轨迹点
|
||||
* @param alpha 插补系数 [0, 1]
|
||||
* @param num_joints 关节数量（该臂的自由度）
|
||||
* @param joint_positions 插补后的关节位置
|
||||
*/
|
||||
void UpdateJointsInfoInterpolated(
|
||||
uint8_t arm_id,
|
||||
const TrajectoryPoint& p1,
|
||||
const TrajectoryPoint& p2,
|
||||
double alpha,
|
||||
size_t num_joints,
|
||||
const std::vector<double>& joint_positions);
|
||||
|
||||
public:
|
||||
};
|
||||
}
|
||||
166
src/robot_control/include/controllers/control_base.hpp
Normal file
166
src/robot_control/include/controllers/control_base.hpp
Normal file
@@ -0,0 +1,166 @@
|
||||
/**
|
||||
* @file control_base.hpp
|
||||
* @brief 控制器基类 - 所有控制器的基类
|
||||
* @author Ray
|
||||
* @date 2025
|
||||
*
|
||||
* 该类定义了所有控制器的通用接口和功能，包括：
|
||||
* - 关节位置、速度、加速度管理
|
||||
* - 梯形轨迹规划
|
||||
* - 回零运动
|
||||
* - 停止运动
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "utils/trapezoidal_trajectory.hpp"
|
||||
#include "utils/s_curve_trajectory.hpp"
|
||||
#include "core/motion_parameters.hpp"
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
/**
|
||||
* @brief 轨迹规划器类型枚举
|
||||
*/
|
||||
enum class TrajectoryPlannerType {
|
||||
TRAPEZOIDAL = 0, ///< 梯形轨迹规划器（默认）
|
||||
S_CURVE = 1 ///< S型曲线轨迹规划器
|
||||
};
|
||||
/**
|
||||
* @class ControlBase
|
||||
* @brief 控制器基类
|
||||
*
|
||||
* 所有控制器（LegControl, WheelControl, WaistControl, ArmControl）的基类。
|
||||
* 提供通用的控制功能和接口。
|
||||
*/
|
||||
class ControlBase
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数（新版本，使用 JointInfo）
|
||||
* @param joint_names 关节名称列表
|
||||
* @param joints_info_map 关节信息字典（只包含与当前控制器相关的关节）
|
||||
* @param lengthParameters 长度参数
|
||||
*/
|
||||
ControlBase(
|
||||
const std::vector<std::string>& joint_names,
|
||||
const std::map<std::string, JointInfo>& joints_info_map,
|
||||
const std::vector<double>& lengthParameters);
|
||||
|
||||
/**
|
||||
* @brief 虚析构函数
|
||||
*/
|
||||
virtual ~ControlBase();
|
||||
|
||||
/**
|
||||
* @brief 移动到目标关节位置
|
||||
* @param joint_positions 输出：当前关节位置（会被更新）
|
||||
* @param target_joint 目标关节位置
|
||||
* @param dt 时间步长（秒）
|
||||
* @return true 到达目标，false 运动进行中
|
||||
*/
|
||||
bool MoveToTargetJoint(
|
||||
std::vector<double>& joint_positions,
|
||||
const std::vector<double>& target_joint,
|
||||
double dt);
|
||||
|
||||
/**
|
||||
* @brief 回零运动
|
||||
* @param joint_positions 输出：当前关节位置（会被更新）
|
||||
* @param dt 时间步长（秒）
|
||||
* @return true 回零完成，false 回零进行中
|
||||
*/
|
||||
bool GoHome(std::vector<double>& joint_positions, double dt);
|
||||
|
||||
/**
|
||||
* @brief 停止运动
|
||||
* @param joint_positions 输出：当前关节位置（会被更新）
|
||||
* @param dt 时间步长（秒）
|
||||
* @return true 完全停止，false 正在停止
|
||||
*/
|
||||
bool Stop(std::vector<double>& joint_positions, double dt);
|
||||
|
||||
/**
|
||||
* @brief 更新关节状态
|
||||
* @param joint_positions 关节位置
|
||||
* @param joint_velocities 关节速度
|
||||
* @param joint_torques 关节力矩
|
||||
*/
|
||||
void UpdateJointStates(
|
||||
const std::vector<double>& joint_positions,
|
||||
const std::vector<double>& joint_velocities,
|
||||
const std::vector<double>& joint_torques);
|
||||
|
||||
/**
|
||||
* @brief 检查是否在运动
|
||||
* @return true 正在运动，false 未运动
|
||||
*/
|
||||
bool IsMoving();
|
||||
|
||||
/**
|
||||
* @brief 检查是否到达目标位置
|
||||
* @param target_joint 目标关节位置
|
||||
* @return true 已到达，false 未到达
|
||||
*/
|
||||
bool IsReached(const std::vector<double>& target_joint);
|
||||
|
||||
/**
|
||||
* @brief 检查关节限位
|
||||
* @param joint_positions 关节位置（会被修改以满足限位）
|
||||
*/
|
||||
bool CheckJointLimits(std::vector<double>& joint_positions);
|
||||
|
||||
/**
|
||||
* @brief 设置回零位置
|
||||
* @param home_positions 回零位置
|
||||
*/
|
||||
void SetHomePositions(const std::vector<double>& home_positions);
|
||||
|
||||
/**
|
||||
* @brief 设置轨迹规划器类型
|
||||
* @param planner_type 规划器类型（TRAPEZOIDAL 或 S_CURVE）
|
||||
*/
|
||||
void SetTrajectoryPlannerType(TrajectoryPlannerType planner_type);
|
||||
|
||||
/**
|
||||
* @brief 获取当前轨迹规划器类型
|
||||
* @return 当前规划器类型
|
||||
*/
|
||||
TrajectoryPlannerType GetTrajectoryPlannerType() const { return planner_type_; }
|
||||
|
||||
protected:
|
||||
size_t total_joints_; ///< 关节总数
|
||||
|
||||
std::vector<std::string> joint_names_; ///< 关节名称列表（保持顺序）
|
||||
std::map<std::string, JointInfo> joints_info_map_; ///< 关节信息字典（包含限位、速度、加速度、home位置、当前状态、指令等）
|
||||
|
||||
// 通用参数（所有控制器共享）
|
||||
std::vector<double> lengthParameters_; ///< 长度参数
|
||||
bool is_target_set_; ///< 目标是否已设置
|
||||
|
||||
TrapezoidalTrajectory* trapezoidalTrajectory_; ///< 梯形轨迹规划器
|
||||
S_CurveTrajectory* s_curveTrajectory_; ///< S型曲线轨迹规划器
|
||||
TrajectoryPlannerType planner_type_; ///< 当前使用的规划器类型
|
||||
|
||||
bool is_moving_; ///< 是否在运动
|
||||
bool is_stopping_; ///< 是否在停止
|
||||
double moving_duration_time_; ///< 运动持续时间
|
||||
double stop_duration_time_; ///< 停止持续时间
|
||||
|
||||
bool is_joint_position_initialized_; ///< 关节位置是否已初始化
|
||||
|
||||
static constexpr double POSITION_TOLERANCE = 0.01; ///< 位置容差（弧度）
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief 从 joints_info_map_ 提取当前位置向量（供轨迹规划器使用）
|
||||
*/
|
||||
std::vector<double> getCurrentPositions() const;
|
||||
|
||||
};
|
||||
}
|
||||
57
src/robot_control/include/controllers/leg_control.hpp
Normal file
57
src/robot_control/include/controllers/leg_control.hpp
Normal file
@@ -0,0 +1,57 @@
|
||||
/**
|
||||
* @file leg_control.hpp
|
||||
* @brief 腿部控制器 - 控制机器人腿部运动
|
||||
* @author Ray
|
||||
* @date 2025
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "controllers/control_base.hpp"
|
||||
#include <string>
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
// 前向声明
|
||||
class MotionParameters;
|
||||
/**
|
||||
* @class LegControl
|
||||
* @brief 腿部控制器类
|
||||
*
|
||||
* 继承自ControlBase，提供腿部特定的控制功能。
|
||||
*/
|
||||
class LegControl : public ControlBase
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数
|
||||
* @param joint_names 关节名称列表
|
||||
* @param motion_params MotionParameters 引用，用于获取 JointInfo
|
||||
* @param lengthParameters 长度参数
|
||||
*/
|
||||
LegControl(
|
||||
const std::vector<std::string>& joint_names,
|
||||
const std::map<std::string, JointInfo>& joints_info_map,
|
||||
const std::vector<double>& lengthParameters);
|
||||
|
||||
/**
|
||||
* @brief 析构函数
|
||||
*/
|
||||
~LegControl();
|
||||
|
||||
/**
|
||||
* @brief 向上移动（相对运动）
|
||||
* @param joint_positions 输出：当前关节位置（会被更新）
|
||||
* @param dt 时间步长（秒）
|
||||
* @return true 移动完成，false 移动进行中
|
||||
*/
|
||||
bool MoveUp(std::vector<double>& joint_positions, double dt);
|
||||
|
||||
// Internal methods - made public for RobotControlManager access
|
||||
bool SetMoveLegParametersInternal(double moveLegDistance);
|
||||
|
||||
private:
|
||||
double CalculateAngleFromLinks_(double side1, double side2, double side_opposite, bool is_degree);
|
||||
std::vector<double> target_joint_positions_; ///< 目标关节位置
|
||||
};
|
||||
}
|
||||
61
src/robot_control/include/controllers/waist_control.hpp
Normal file
61
src/robot_control/include/controllers/waist_control.hpp
Normal file
@@ -0,0 +1,61 @@
|
||||
/**
|
||||
* @file waist_control.hpp
|
||||
* @brief 腰部控制器 - 控制机器人腰部运动
|
||||
* @author Ray
|
||||
* @date 2025
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "controllers/control_base.hpp"
|
||||
#include <Eigen/Dense>
|
||||
#include <string>
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
// 前向声明
|
||||
class MotionParameters;
|
||||
/**
|
||||
* @class WaistControl
|
||||
* @brief 腰部控制器类
|
||||
*
|
||||
* 继承自ControlBase，提供腰部特定的控制功能。
|
||||
*/
|
||||
class WaistControl : public ControlBase
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数
|
||||
* @param joint_names 关节名称列表
|
||||
* @param motion_params MotionParameters 引用，用于获取 JointInfo
|
||||
* @param lengthParameters 长度参数
|
||||
*/
|
||||
WaistControl(
|
||||
const std::vector<std::string>& joint_names,
|
||||
const std::map<std::string, JointInfo>& joints_info_map,
|
||||
const std::vector<double>& lengthParameters);
|
||||
|
||||
/**
|
||||
* @brief 析构函数
|
||||
*/
|
||||
~WaistControl();
|
||||
|
||||
/**
|
||||
* @brief 移动腰部
|
||||
* @param joint_positions 输出：当前关节位置（会被更新）
|
||||
* @param dt 时间步长（秒）
|
||||
* @return true 移动完成，false 移动进行中
|
||||
*/
|
||||
bool MoveWaist(std::vector<double>& joint_positions, double dt);
|
||||
|
||||
// Internal methods - made public for RobotControlManager access
|
||||
bool SetMoveWaistParametersInternal(double movePitchAngle, double moveYawAngle);
|
||||
|
||||
private:
|
||||
// 成员变量（特定于腰部控制器）
|
||||
bool is_cmd_send_finished_; ///< 命令是否已发送完成
|
||||
int time_out_count_; ///< 超时计数
|
||||
static constexpr int TIME_OUT_COUNT = 500; ///< 超时计数阈值
|
||||
std::vector<double> target_joint_positions_; ///< 目标关节位置
|
||||
};
|
||||
}
|
||||
66
src/robot_control/include/controllers/wheel_control.hpp
Normal file
66
src/robot_control/include/controllers/wheel_control.hpp
Normal file
@@ -0,0 +1,66 @@
|
||||
/**
|
||||
* @file wheel_control.hpp
|
||||
* @brief 轮子控制器 - 控制机器人轮子运动
|
||||
* @author Ray
|
||||
* @date 2025
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "controllers/control_base.hpp"
|
||||
#include <Eigen/Dense>
|
||||
#include <string>
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
// 前向声明
|
||||
class MotionParameters;
|
||||
/**
|
||||
* @class WheelControl
|
||||
* @brief 轮子控制器类
|
||||
*
|
||||
* 继承自ControlBase，提供轮子特定的控制功能。
|
||||
*/
|
||||
class WheelControl : public ControlBase
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数
|
||||
* @param joint_names 关节名称列表
|
||||
* @param motion_params MotionParameters 引用，用于获取 JointInfo
|
||||
* @param lengthParameters 长度参数
|
||||
*/
|
||||
WheelControl(
|
||||
const std::vector<std::string>& joint_names,
|
||||
const std::map<std::string, JointInfo>& joints_info_map,
|
||||
const std::vector<double>& lengthParameters);
|
||||
|
||||
/**
|
||||
* @brief 析构函数
|
||||
*/
|
||||
~WheelControl();
|
||||
|
||||
/**
|
||||
* @brief 移动轮子
|
||||
* @param joint_positions 输出：当前关节位置（会被更新）
|
||||
* @return true 移动完成，false 移动进行中
|
||||
*/
|
||||
bool MoveWheel(std::vector<double>& joint_positions);
|
||||
|
||||
/**
|
||||
* @brief 获取轮子速度比例
|
||||
* @return 速度比例
|
||||
*/
|
||||
double GetWheelRatioInternal();
|
||||
|
||||
// Internal methods - made public for RobotControlManager access
|
||||
bool SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle);
|
||||
bool SetMoveWheelParametersInternalJog(double moveWheelDistance, double moveWheelAngle);
|
||||
|
||||
private:
|
||||
// 成员变量（特定于轮子控制器）
|
||||
double last_move_distance_; ///< 上次移动距离
|
||||
double move_ratio_; ///< 移动比例
|
||||
std::vector<double> target_joint_positions_; ///< 目标关节位置
|
||||
};
|
||||
}
|
||||
53
src/robot_control/include/core/common_enum.hpp
Normal file
53
src/robot_control/include/core/common_enum.hpp
Normal file
@@ -0,0 +1,53 @@
|
||||
/**
|
||||
* @file common_enum.hpp
|
||||
* @brief 通用枚举定义
|
||||
* @author Ray
|
||||
* @date 2025
|
||||
*
|
||||
* 该文件包含所有机器人控制相关的通用枚举定义。
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
/**
|
||||
* @enum MovementPart
|
||||
* @brief 运动部位枚举
|
||||
*
|
||||
* 用于标识机器人不同的运动部位，用于查询和控制特定部位的运动状态。
|
||||
*/
|
||||
enum class MovementPart {
|
||||
LEG, ///< 腿部
|
||||
WHEEL, ///< 轮子
|
||||
WAIST, ///< 腰部
|
||||
ALL ///< 全部部位（用于查询所有部位的运动状态）
|
||||
};
|
||||
|
||||
/**
|
||||
* @enum ControllerType
|
||||
* @brief 控制器类型枚举
|
||||
*
|
||||
* 用于标识不同类型的控制器，用于创建和管理控制器实例。
|
||||
* 注意：枚举值使用 _CONTROLLER 后缀以区别于 MovementPart 枚举。
|
||||
*/
|
||||
enum class ControllerType {
|
||||
LEG_CONTROLLER, ///< 腿部控制器
|
||||
WHEEL_CONTROLLER, ///< 轮子控制器
|
||||
WAIST_CONTROLLER, ///< 腰部控制器
|
||||
ARM_CONTROLLER, ///< 手臂控制器
|
||||
UNKNOWN_CONTROLLER ///< 未知类型
|
||||
};
|
||||
|
||||
/**
|
||||
* @enum LimitType
|
||||
* @brief 限制类型枚举
|
||||
*
|
||||
* 用于标识不同类型的关节限制（位置、速度、力矩）。
|
||||
*/
|
||||
enum class LimitType {
|
||||
POSITION, ///< 位置限制（单位：度/弧度）
|
||||
VELOCITY, ///< 速度限制（单位：度/秒/弧度/秒）
|
||||
EFFORT ///< 力矩限制（单位：N·m）
|
||||
};
|
||||
}
|
||||
72
src/robot_control/include/core/controller_factory.hpp
Normal file
72
src/robot_control/include/core/controller_factory.hpp
Normal file
@@ -0,0 +1,72 @@
|
||||
/**
|
||||
* @file controller_factory.hpp
|
||||
* @brief 控制器工厂 - 用于创建和管理控制器实例
|
||||
* @author Ray
|
||||
* @date 2025
|
||||
*
|
||||
* 该类提供工厂模式来创建不同类型的控制器实例，
|
||||
* 支持动态控制器加载和管理。
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <map>
|
||||
|
||||
#include "core/motion_parameters.hpp"
|
||||
#include "core/common_enum.hpp" // 包含 ControllerType 枚举定义
|
||||
#include "controllers/control_base.hpp"
|
||||
#include "controllers/leg_control.hpp"
|
||||
#include "controllers/wheel_control.hpp"
|
||||
#include "controllers/waist_control.hpp"
|
||||
#include "controllers/arm_control.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
/**
|
||||
* @class ControllerFactory
|
||||
* @brief 控制器工厂类
|
||||
*
|
||||
* 提供静态方法来创建和管理控制器实例。
|
||||
* 使用工厂模式实现控制器的统一创建和管理。
|
||||
*/
|
||||
class ControllerFactory
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 创建指定类型的控制器
|
||||
* @param type 控制器类型
|
||||
* @param motion_params 运动参数
|
||||
* @return 控制器智能指针，如果类型无效返回nullptr
|
||||
*/
|
||||
static std::unique_ptr<ControlBase> CreateController(
|
||||
ControllerType type,
|
||||
const MotionParameters& motion_params);
|
||||
|
||||
/**
|
||||
* @brief 检查指定控制器是否在启用列表中
|
||||
* @param type 控制器类型
|
||||
* @param enabled_controllers 启用的控制器名称列表
|
||||
* @return true 已启用，false 未启用
|
||||
*/
|
||||
static bool IsControllerEnabled(
|
||||
ControllerType type,
|
||||
const std::vector<std::string>& enabled_controllers);
|
||||
|
||||
/**
|
||||
* @brief 将控制器类型转换为字符串
|
||||
* @param type 控制器类型
|
||||
* @return 控制器类型字符串（小写）
|
||||
*/
|
||||
static std::string ControllerTypeToString(ControllerType type);
|
||||
|
||||
/**
|
||||
* @brief 将字符串转换为控制器类型
|
||||
* @param type_str 控制器类型字符串
|
||||
* @return 控制器类型，如果无效返回UNKNOWN
|
||||
*/
|
||||
static ControllerType StringToControllerType(const std::string& type_str);
|
||||
};
|
||||
}
|
||||
175
src/robot_control/include/core/motion_parameters.hpp
Normal file
175
src/robot_control/include/core/motion_parameters.hpp
Normal file
@@ -0,0 +1,175 @@
|
||||
/**
|
||||
* @file motion_parameters.hpp
|
||||
* @brief 运动参数类 - 定义机器人运动控制的所有参数
|
||||
* @author Ray
|
||||
* @date 2025
|
||||
*
|
||||
* 该类包含机器人运动控制所需的所有参数，包括：
|
||||
* - 关节索引和方向
|
||||
* - 关节限位、速度和加速度
|
||||
* - 初始位置和零点位置
|
||||
* - 轮子控制参数
|
||||
* - 关节传动比和分辨率
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "core/common_enum.hpp" // 包含 LimitType 枚举定义
|
||||
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
// ==================== 宏定义 ====================
|
||||
|
||||
#define CYCLE_TIME 4 ///< 插补周期（毫秒）
|
||||
|
||||
#ifndef DEG2RAD
|
||||
#define DEG2RAD(x) ((x)*0.017453293) ///< 度转弧度
|
||||
#endif
|
||||
|
||||
#ifndef RAD2DEG
|
||||
#define RAD2DEG(x) ((x)*57.29578) ///< 弧度转度
|
||||
#endif
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846 ///< 圆周率
|
||||
#endif
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
// ==================== 结构体定义 ====================
|
||||
|
||||
/**
|
||||
* @struct JointInfo
|
||||
* @brief 关节信息结构体
|
||||
*
|
||||
* 包含关节的所有信息：限制、速度、加速度、初始位置、当前状态、指令等。
|
||||
*/
|
||||
struct JointInfo {
|
||||
// 配置参数
|
||||
double max_limit; ///< 关节运动范围上限（如位置最大角度）
|
||||
double min_limit; ///< 关节运动范围下限（如位置最小角度）
|
||||
LimitType limit_type; ///< 限制类型（区分位置/速度/力矩）
|
||||
double max_velocity; ///< 关节最大速度（度/秒或rad/s）
|
||||
double max_acceleration; ///< 关节最大加速度（度/秒²或rad/s²）
|
||||
double home_position; ///< 关节初始位置（Home位置，度或弧度）
|
||||
|
||||
// 当前状态
|
||||
double current_position; ///< 当前关节位置
|
||||
double current_velocity; ///< 当前关节速度
|
||||
double current_acceleration; ///< 当前关节加速度
|
||||
double current_torque; ///< 当前关节力矩
|
||||
|
||||
// 指令参数
|
||||
double command_position; ///< 指令关节位置
|
||||
double command_velocity; ///< 指令关节速度
|
||||
double command_acceleration; ///< 指令关节加速度
|
||||
double command_torque; ///< 指令关节力矩
|
||||
|
||||
/**
|
||||
* @brief 默认构造函数
|
||||
* 初始化所有成员为默认值（状态和指令参数初始化为0）
|
||||
*/
|
||||
JointInfo()
|
||||
: max_limit(0.0), min_limit(0.0), limit_type(LimitType::POSITION),
|
||||
max_velocity(0.0), max_acceleration(0.0), home_position(0.0),
|
||||
current_position(0.0), current_velocity(0.0), current_acceleration(0.0), current_torque(0.0),
|
||||
command_position(0.0), command_velocity(0.0), command_acceleration(0.0), command_torque(0.0) {}
|
||||
|
||||
/**
|
||||
* @brief 带参构造函数
|
||||
* @param max_lim 最大值
|
||||
* @param min_lim 最小值
|
||||
* @param type 限制类型
|
||||
* @param max_vel 最大速度
|
||||
* @param max_acc 最大加速度
|
||||
* @param home_pos Home位置
|
||||
*/
|
||||
JointInfo(double max_lim, double min_lim, LimitType type, double max_vel, double max_acc, double home_pos = 0.0)
|
||||
: max_limit(max_lim), min_limit(min_lim), limit_type(type),
|
||||
max_velocity(max_vel), max_acceleration(max_acc), home_position(home_pos),
|
||||
current_position(0.0), current_velocity(0.0), current_acceleration(0.0), current_torque(0.0),
|
||||
command_position(0.0), command_velocity(0.0), command_acceleration(0.0), command_torque(0.0)
|
||||
{
|
||||
if (max_limit < min_limit)
|
||||
{
|
||||
// max_limit < min_limit: swap silently to ensure valid range
|
||||
std::swap(max_limit, min_limit);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// ==================== 类定义 ====================
|
||||
|
||||
/**
|
||||
* @class MotionParameters
|
||||
* @brief 运动参数类
|
||||
*
|
||||
* 包含机器人运动控制所需的所有参数配置。
|
||||
* 支持基于关节名称的动态配置，适应不同的机器人配置（不同的关节组合）。
|
||||
*/
|
||||
class MotionParameters
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数
|
||||
*
|
||||
* 初始化所有运动参数，包括：
|
||||
* - 关节索引和方向
|
||||
* - 关节限位、速度和加速度
|
||||
* - 初始位置和零点位置
|
||||
* - 轮子控制参数
|
||||
* - 关节传动比和分辨率
|
||||
*/
|
||||
MotionParameters();
|
||||
|
||||
/**
|
||||
* @brief 从 JointState 消息初始化关节名称映射
|
||||
* @param joint_names 关节名称列表（从 JointState.name 获取）
|
||||
*
|
||||
* 该方法根据传入的关节名称列表，建立名称到索引的映射，
|
||||
* 并根据配置的关节组名称列表（wheel_joint_names_等）生成对应的索引。
|
||||
* 同时从现有的基于索引的参数生成基于名称的映射。
|
||||
*/
|
||||
void InitializeJointNameMapping(const std::vector<std::string>& joint_names);
|
||||
|
||||
// ==================== 公共成员变量 ====================
|
||||
|
||||
// ---------- 基于名称的关节配置（主要使用） ----------
|
||||
std::vector<std::string> all_joint_names_; ///< 所有关节名称列表（按顺序，从JointState消息中获取）
|
||||
std::map<std::string, size_t> joint_name_to_index_; ///< 关节名称到索引的映射
|
||||
std::map<std::string, JointInfo> joints_info_map_; ///< 关节信息字典（基于名称，包含限制、速度、加速度等信息）
|
||||
|
||||
// 关节组名称列表（用于配置哪些关节属于哪个控制器）
|
||||
std::vector<std::string> wheel_joint_names_; ///< 轮子关节名称列表
|
||||
std::vector<std::string> waist_joint_names_; ///< 腰部关节名称列表
|
||||
std::vector<std::string> leg_joint_names_; ///< 腿部关节名称列表
|
||||
std::vector<std::string> dual_arm_joint_names_; ///< 双臂关节名称列表（合并左右臂）
|
||||
|
||||
// ---------- 关节索引和方向（保留以保持兼容性，但将从名称映射生成） ----------
|
||||
size_t total_joints_; ///< 总关节数
|
||||
|
||||
// ---------- 轮子控制参数 ----------
|
||||
double wheel_radius_; ///< 轮子半径（米）
|
||||
double wheel_separation_; ///< 轮距（米）
|
||||
double max_linear_speed_x_; ///< 最大线速度 X（m/s）
|
||||
double max_linear_speed_z_; ///< 最大线速度 Z（m/s）
|
||||
double max_angular_speed_z_; ///< 最大角速度 Z（rad/s）
|
||||
|
||||
// ---------- 尺寸参数 ----------
|
||||
std::vector<double> leg_length_; ///< 腿部长度参数（米）+
|
||||
std::vector<double> waist_length_; ///< 腰部长度参数（米）
|
||||
std::vector<double> wheel_length_; ///< 轮子长度参数（米）
|
||||
std::vector<double> dual_arm_length_; ///< 双臂长度参数（米，合并左右臂）
|
||||
|
||||
// ---------- 手臂参数 ----------
|
||||
size_t arm_dof_; ///< 每条手臂的自由度数（关节数）
|
||||
|
||||
// ---------- 控制器启用标志 ----------
|
||||
bool leg_controller_enabled_; ///< 腿部控制器是否启用
|
||||
bool wheel_controller_enabled_; ///< 轮子控制器是否启用
|
||||
bool waist_controller_enabled_; ///< 腰部控制器是否启用
|
||||
bool arm_controller_enabled_; ///< 手臂控制器是否启用
|
||||
};
|
||||
}
|
||||
163
src/robot_control/include/core/remote_controller.hpp
Normal file
163
src/robot_control/include/core/remote_controller.hpp
Normal file
@@ -0,0 +1,163 @@
|
||||
/**
|
||||
* @file remote_controller.hpp
|
||||
* @brief 遥控器控制器 - 处理遥控器指令
|
||||
* @author Ray
|
||||
* @date 2025
|
||||
*
|
||||
* 该类负责：
|
||||
* - 处理遥控器命令（gamepad消息）
|
||||
* - 单关节点动控制（jog）
|
||||
* - 各部位运动控制（leg, waist, wheel等）
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <functional>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
#include "core/common_enum.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
// 前向声明
|
||||
class RobotControlManager;
|
||||
|
||||
/**
|
||||
* @class RemoteController
|
||||
* @brief 遥控器控制器类
|
||||
*
|
||||
* 负责处理遥控器命令，分为两大类功能：
|
||||
* 1. 单关节点动（jog）- 精确控制单个关节的运动
|
||||
* 2. 运动控制 - 控制各个部位的运动（leg, waist, wheel等）
|
||||
*/
|
||||
class RemoteController
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数
|
||||
* @param node ROS 2节点指针（用于日志）
|
||||
* @param robot_control_manager 机器人控制管理器引用
|
||||
*/
|
||||
RemoteController(
|
||||
rclcpp::Node* node,
|
||||
RobotControlManager& robot_control_manager);
|
||||
|
||||
/**
|
||||
* @brief 析构函数
|
||||
*/
|
||||
~RemoteController();
|
||||
|
||||
/**
|
||||
* @brief 处理遥控器命令
|
||||
* @param msg 遥控器命令消息
|
||||
*/
|
||||
void ProcessCommand(const std_msgs::msg::String::SharedPtr msg);
|
||||
|
||||
// ==================== Jog 控制状态查询 ====================
|
||||
|
||||
/**
|
||||
* @brief 检查是否处于点动模式
|
||||
* @return true 点动模式开启，false 点动模式关闭
|
||||
*/
|
||||
bool IsJogMode() const { return is_jog_mode_; }
|
||||
|
||||
/**
|
||||
* @brief 获取当前点动的关节索引
|
||||
* @return 关节索引
|
||||
*/
|
||||
size_t GetJogIndex() const { return jog_index_; }
|
||||
|
||||
/**
|
||||
* @brief 获取当前点动方向
|
||||
* @return 方向（-1, 0, 1）
|
||||
*/
|
||||
int GetJogDirection() const { return jog_direction_; }
|
||||
|
||||
/**
|
||||
* @brief 设置点动模式状态（用于外部控制）
|
||||
* @param enabled true 启用点动模式，false 禁用点动模式
|
||||
*/
|
||||
void SetJogMode(bool enabled) { is_jog_mode_ = enabled; }
|
||||
|
||||
/**
|
||||
* @brief 检查是否有停止请求
|
||||
* @return true 有停止请求，false 无停止请求
|
||||
*/
|
||||
bool HasStopRequest() const { return is_stopping_; }
|
||||
|
||||
/**
|
||||
* @brief 清除停止请求标志
|
||||
*/
|
||||
void ClearStopRequest() { is_stopping_ = false; }
|
||||
|
||||
private:
|
||||
// ==================== 核心组件引用 ====================
|
||||
|
||||
rclcpp::Node* node_; ///< ROS 2节点指针（用于日志）
|
||||
RobotControlManager& robot_control_manager_; ///< 机器人控制管理器引用
|
||||
|
||||
// ==================== Jog 控制状态 ====================
|
||||
|
||||
bool is_jog_mode_; ///< 是否处于点动模式
|
||||
int jog_direction_; ///< 点动方向（-1, 0, 1）
|
||||
size_t jog_index_; ///< 当前点动的关节索引
|
||||
|
||||
// ==================== 运动控制状态 ====================
|
||||
|
||||
bool is_stopping_; ///< 是否有停止请求
|
||||
|
||||
// ==================== 命令去重 ====================
|
||||
|
||||
std::string last_command_; ///< 上次处理的命令（用于去重）
|
||||
|
||||
// ==================== Jog 控制方法 ====================
|
||||
|
||||
/**
|
||||
* @brief 处理点动模式开关命令
|
||||
* @param command 命令字符串
|
||||
*/
|
||||
void HandleJogModeToggle(const std::string& command);
|
||||
|
||||
/**
|
||||
* @brief 处理关节切换命令
|
||||
* @param command 命令字符串
|
||||
*/
|
||||
void HandleJogAxisSwitch(const std::string& command);
|
||||
|
||||
/**
|
||||
* @brief 处理点动方向命令
|
||||
* @param command 命令字符串
|
||||
*/
|
||||
void HandleJogDirection(const std::string& command);
|
||||
|
||||
// ==================== 运动控制方法 ====================
|
||||
|
||||
/**
|
||||
* @brief 处理紧急停止命令
|
||||
*/
|
||||
void HandleEmergencyStop();
|
||||
|
||||
/**
|
||||
* @brief 处理运动部位控制命令（预留接口，用于未来扩展）
|
||||
* @param command 命令字符串
|
||||
*/
|
||||
void HandleMovementControl(const std::string& command);
|
||||
|
||||
// ==================== 辅助方法 ====================
|
||||
|
||||
/**
|
||||
* @brief 检查命令是否为重复命令
|
||||
* @param command 命令字符串
|
||||
* @return true 重复命令，false 新命令
|
||||
*/
|
||||
bool IsDuplicateCommand_(const std::string& command);
|
||||
|
||||
/**
|
||||
* @brief 更新最后处理的命令
|
||||
* @param command 命令字符串
|
||||
*/
|
||||
void UpdateLastCommand_(const std::string& command);
|
||||
};
|
||||
}
|
||||
|
||||
374
src/robot_control/include/core/robot_control_manager.hpp
Normal file
374
src/robot_control/include/core/robot_control_manager.hpp
Normal file
@@ -0,0 +1,374 @@
|
||||
/**
|
||||
* @file robot_control_manager.hpp
|
||||
* @brief 机器人控制管理器 - 统一管理所有控制器（腿、轮、腰、臂）
|
||||
* @author Ray
|
||||
* @date 2025
|
||||
*
|
||||
* 该类负责：
|
||||
* - 初始化和管理各个控制器（Leg, Wheel, Waist, Arm）
|
||||
* - 协调不同控制器之间的运动
|
||||
* - 提供统一的接口进行机器人控制
|
||||
* - 处理关节状态更新和命令发布
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sensor_msgs/msg/joint_state.hpp"
|
||||
#include "sensor_msgs/msg/imu.hpp"
|
||||
#include "trajectory_msgs/msg/joint_trajectory.hpp"
|
||||
#include "geometry_msgs/msg/pose.hpp"
|
||||
#include "core/motion_parameters.hpp"
|
||||
#include "controllers/arm_control.hpp"
|
||||
#include "controllers/leg_control.hpp"
|
||||
#include "controllers/wheel_control.hpp"
|
||||
#include "controllers/waist_control.hpp"
|
||||
#include "core/common_enum.hpp"
|
||||
#include "core/controller_factory.hpp"
|
||||
|
||||
#include "interfaces/msg/motor_pos.hpp"
|
||||
#include "interfaces/msg/imu_msg.hpp"
|
||||
#include "interfaces/msg/arm_motion_params.hpp"
|
||||
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
using MotorPos = interfaces::msg::MotorPos;
|
||||
using ImuMsg = sensor_msgs::msg::Imu;
|
||||
using ArmMotionParams = interfaces::msg::ArmMotionParams;
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
/**
|
||||
* @class RobotControlManager
|
||||
* @brief 机器人控制管理器类
|
||||
*
|
||||
* 统一管理机器人的所有控制器，协调各控制器的运动，
|
||||
* 处理关节状态更新，提供统一的控制接口。
|
||||
*/
|
||||
class RobotControlManager
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数
|
||||
*/
|
||||
explicit RobotControlManager();
|
||||
|
||||
/**
|
||||
* @brief 析构函数
|
||||
*/
|
||||
~RobotControlManager();
|
||||
|
||||
// ==================== 运动控制接口 ====================
|
||||
|
||||
/**
|
||||
* @brief 控制轮子运动
|
||||
* @return true 运动完成，false 运动进行中
|
||||
*/
|
||||
bool MoveWheel();
|
||||
|
||||
/**
|
||||
* @brief 设置轮子点动模式
|
||||
* @param value true 启用点动，false 禁用点动
|
||||
*/
|
||||
void SetJogWheel(bool value);
|
||||
|
||||
/**
|
||||
* @brief 获取轮子点动模式状态
|
||||
* @return true 点动模式启用，false 点动模式禁用
|
||||
*/
|
||||
bool GetJogWheel();
|
||||
|
||||
// ==================== 参数设置接口 ====================
|
||||
|
||||
/**
|
||||
* @brief 设置轮子运动参数
|
||||
* @param moveWheelDistance 移动距离
|
||||
* @param moveWheelAngle 移动角度
|
||||
* @return true 参数设置成功，false 参数设置失败
|
||||
*/
|
||||
bool SetMoveWheelParameters(double moveWheelDistance, double moveWheelAngle);
|
||||
|
||||
/**
|
||||
* @brief 设置腿部运动参数
|
||||
* @param moveLegDistance 移动距离
|
||||
* @return true 参数设置成功，false 参数设置失败
|
||||
*/
|
||||
bool SetMoveLegParameters(double moveLegDistance);
|
||||
|
||||
/**
|
||||
* @brief 设置腰部运动参数
|
||||
* @param movePitchAngle 俯仰角
|
||||
* @param moveYawAngle 偏航角
|
||||
* @return true 参数设置成功，false 参数设置失败
|
||||
*/
|
||||
bool SetMoveWaistParameters(double movePitchAngle, double moveYawAngle);
|
||||
|
||||
/**
|
||||
* @brief 规划手臂运动（调用ArmControl的规划方法）
|
||||
*
|
||||
* 规划结果会被存入 ArmControl 内部的轨迹缓存，
|
||||
* 上层通过 GetArmInterpolatedPoint / HasActiveArmTrajectory 获取执行点位。
|
||||
*
|
||||
* @param arm_params 手臂运动参数（包含MOVEJ或MOVEL）
|
||||
* @param velocity_scaling 速度缩放因子 [0,1]
|
||||
* @param acceleration_scaling 加速度缩放因子 [0,1]
|
||||
* @return true 规划成功，false 规划失败
|
||||
*/
|
||||
bool PlanArmMotion(
|
||||
const ArmMotionParams& arm_params,
|
||||
double velocity_scaling,
|
||||
double acceleration_scaling);
|
||||
|
||||
/**
|
||||
* @brief 检查手臂关节目标是否超过限位（MOVEJ 用）
|
||||
* @param arm_id 手臂ID（0=左臂，1=右臂）
|
||||
* @param target_joints 目标关节角（弧度），大小应为 arm_dof_
|
||||
* @param error_msg 输出：失败原因（可为空指针）
|
||||
* @return true 目标在限位内；false 超限或参数无效
|
||||
*/
|
||||
bool ValidateArmJointTarget(
|
||||
uint8_t arm_id,
|
||||
const std::vector<double>& target_joints,
|
||||
std::string* error_msg = nullptr) const;
|
||||
|
||||
/**
|
||||
* @brief 初始化ArmControl的MoveIt（需要在ROS节点创建后调用）
|
||||
* @param node ROS 2节点指针
|
||||
* @return true 初始化成功，false 初始化失败
|
||||
*/
|
||||
bool InitializeArmMoveIt(rclcpp::Node* node);
|
||||
|
||||
|
||||
/**
|
||||
* @brief 规划双臂（或单臂保持另一臂）关节运动，生成一个 12关节 的 MoveIt 轨迹
|
||||
* @param has_left 是否需要设置左臂目标
|
||||
* @param left_target 左臂目标（size=arm_dof_，弧度）
|
||||
* @param has_right 是否需要设置右臂目标
|
||||
* @param right_target 右臂目标（size=arm_dof_，弧度）
|
||||
*/
|
||||
bool PlanDualArmJointMotion(
|
||||
bool has_left,
|
||||
const std::vector<double>& left_target,
|
||||
bool has_right,
|
||||
const std::vector<double>& right_target,
|
||||
double velocity_scaling,
|
||||
double acceleration_scaling);
|
||||
|
||||
/**
|
||||
* @brief 单臂逆解（通过 MoveIt）
|
||||
* @param arm_id 手臂ID（0=左臂，1=右臂）
|
||||
* @param target_pose 目标笛卡尔位姿
|
||||
* @param out_joint_angles 输出关节角（弧度）
|
||||
* @param error_msg 输出错误信息（可选）
|
||||
* @return true 成功，false 失败
|
||||
*/
|
||||
bool SolveArmInverseKinematics(
|
||||
uint8_t arm_id,
|
||||
const geometry_msgs::msg::Pose& target_pose,
|
||||
std::vector<double>* out_joint_angles,
|
||||
std::string* error_msg = nullptr) const;
|
||||
|
||||
/**
|
||||
* @brief 获取单臂当前末端笛卡尔位姿（相对 MoveGroup 基座）
|
||||
* @param arm_id 手臂ID（0=左臂，1=右臂）
|
||||
* @param out_pose 输出位姿
|
||||
* @param error_msg 输出错误信息（可选）
|
||||
* @return true 成功，false 失败
|
||||
*/
|
||||
bool GetArmCurrentPose(
|
||||
uint8_t arm_id,
|
||||
geometry_msgs::msg::Pose* out_pose,
|
||||
std::string* error_msg = nullptr) const;
|
||||
|
||||
/**
|
||||
* @brief 导出最近一次双臂规划轨迹（12关节，包含 time/pos/vel/acc）
|
||||
*/
|
||||
bool ExportDualArmPlannedTrajectory(trajectory_msgs::msg::JointTrajectory& out) const;
|
||||
|
||||
/**
|
||||
* @brief 导出最近一次单臂规划轨迹（6关节，包含 time/pos/vel/acc）
|
||||
* @param arm_id 手臂ID（0=左臂，1=右臂）
|
||||
*/
|
||||
bool ExportArmPlannedTrajectory(uint8_t arm_id, trajectory_msgs::msg::JointTrajectory& out) const;
|
||||
|
||||
// ==================== 状态查询接口 ====================
|
||||
|
||||
/**
|
||||
* @brief 获取当前关节位置（内部顺序）
|
||||
*/
|
||||
const std::vector<double>& GetJointPositions() const;
|
||||
|
||||
/**
|
||||
* @brief 按关节名列表获取当前关节位置（用于软停等按名称子集取值的场景）
|
||||
* @param joint_names 关节名列表
|
||||
* @param out_positions 输出位置（弧度），与 joint_names 一一对应
|
||||
* @param error_msg 失败时错误信息（可传 nullptr）
|
||||
* @return true 成功，false 名称未映射或索引越界
|
||||
*/
|
||||
bool GetJointPositionsByNames(
|
||||
const std::vector<std::string>& joint_names,
|
||||
std::vector<double>* out_positions,
|
||||
std::string* error_msg) const;
|
||||
|
||||
/**
|
||||
* @brief 按关节名列表获取当前关节速度（来自 JointState，用于在轨减速停止等）
|
||||
* @param joint_names 关节名列表
|
||||
* @param out_velocities 输出速度（rad/s），与 joint_names 一一对应
|
||||
* @param error_msg 失败时错误信息（可传 nullptr）
|
||||
* @return true 成功，false 名称未映射或索引越界
|
||||
*/
|
||||
bool GetJointVelocitiesByNames(
|
||||
const std::vector<std::string>& joint_names,
|
||||
std::vector<double>* out_velocities,
|
||||
std::string* error_msg) const;
|
||||
|
||||
/**
|
||||
* @brief 生成在轨减速停止轨迹：用当前位姿+速度（JointState）按关节限速减速到零；无速度时退化为保持轨迹
|
||||
* @param joint_names 关节名列表（如某臂的 FJT joint_names）
|
||||
* @return 减速或保持轨迹；参数无效时返回空轨迹（points 为空）
|
||||
*/
|
||||
trajectory_msgs::msg::JointTrajectory MakeStopTrajectory(
|
||||
const std::vector<std::string>& joint_names) const;
|
||||
|
||||
/**
|
||||
* @brief 获取当前轮子位置（wheel joint order）
|
||||
*/
|
||||
const std::vector<double>& GetWheelPositions() const;
|
||||
|
||||
/**
|
||||
* @brief 获取最近一次下发的轮子命令（wheel joint order）
|
||||
*/
|
||||
const std::vector<double>& GetWheelCommandPositions() const;
|
||||
|
||||
/**
|
||||
* @brief 获取轮子速度比例
|
||||
* @return 速度比例
|
||||
*/
|
||||
double GetWheelRatio();
|
||||
|
||||
/**
|
||||
* @brief 更新关节状态（从JointState消息）
|
||||
* @param msg 关节状态消息
|
||||
*/
|
||||
void UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg);
|
||||
|
||||
/**
|
||||
* @brief 更新轮子状态（从MotorPos消息）
|
||||
* @param msg 电机位置消息
|
||||
*/
|
||||
void UpdateWheelStates(const MotorPos::SharedPtr msg);
|
||||
|
||||
/**
|
||||
* @brief 更新IMU消息
|
||||
* @param msg IMU消息
|
||||
*/
|
||||
void UpdateImuMsg(const ImuMsg::SharedPtr msg);
|
||||
|
||||
/**
|
||||
* @brief 获取运动参数（只读引用，避免拷贝）
|
||||
*/
|
||||
const MotionParameters& GetMotionParameters() const;
|
||||
|
||||
/**
|
||||
* @brief 获取IMU差值
|
||||
* @return IMU yaw 差值（与内部 yaw 单位一致）
|
||||
*/
|
||||
double GetImuYawDifference() const;
|
||||
|
||||
/**
|
||||
* @brief 检查指定部位是否在运动
|
||||
* @param part 运动部位（LEG, WHEEL, WAIST, ALL）
|
||||
* @return true 正在运动，false 未运动
|
||||
*/
|
||||
bool IsMoving(MovementPart part);
|
||||
|
||||
/**
|
||||
* @brief 检查机器人初始化是否完成
|
||||
* @return true 初始化完成，false 初始化未完成
|
||||
*/
|
||||
bool RobotInitFinished();
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief 初始化函数
|
||||
*/
|
||||
void Init_();
|
||||
|
||||
/**
|
||||
* @brief 从关节名称列表获取索引列表（辅助函数）
|
||||
* @param joint_names 关节名称列表
|
||||
* @return 索引列表
|
||||
*/
|
||||
std::vector<size_t> GetJointIndicesFromNames_(const std::vector<std::string>& joint_names) const;
|
||||
|
||||
/**
|
||||
* @brief 按关节名构建子控制器所需的 positions/velocities/efforts
|
||||
* @param joint_names 目标关节名列表
|
||||
* @param out_positions 输出位置
|
||||
* @param out_velocities 输出速度
|
||||
* @param out_efforts 输出力矩
|
||||
* @return true 构建成功且有至少一个关节数据，false 失败或无有效数据
|
||||
*/
|
||||
bool BuildControllerJointStatesByNames_(
|
||||
const std::vector<std::string>& joint_names,
|
||||
std::vector<double>* out_positions,
|
||||
std::vector<double>* out_velocities,
|
||||
std::vector<double>* out_efforts) const;
|
||||
|
||||
// Quaternion conversion utilities (private helpers)
|
||||
void QuaternionToRpyRad_(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw);
|
||||
void QuaternionToRpyDeg_(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw);
|
||||
|
||||
// ==================== 成员变量 ====================
|
||||
|
||||
MotionParameters motion_params_; ///< 运动参数配置
|
||||
|
||||
bool is_wheel_homed_; ///< 轮子是否已回零
|
||||
|
||||
bool is_wheel_jog_; ///< 轮子点动模式标志
|
||||
|
||||
// 控制器（使用智能指针，支持可选加载）
|
||||
std::unique_ptr<LegControl> leg_controller_; ///< 腿部控制器
|
||||
std::unique_ptr<WheelControl> wheel_controller_; ///< 轮子控制器
|
||||
std::unique_ptr<WaistControl> waist_controller_; ///< 腰部控制器
|
||||
std::unique_ptr<ArmControl> arm_controller_; ///< 手臂控制器
|
||||
|
||||
// 控制器启用标志
|
||||
bool leg_controller_enabled_; ///< 腿部控制器是否启用
|
||||
bool wheel_controller_enabled_; ///< 轮子控制器是否启用
|
||||
bool waist_controller_enabled_; ///< 腰部控制器是否启用
|
||||
bool arm_controller_enabled_; ///< 手臂控制器是否启用
|
||||
|
||||
// 运动状态
|
||||
std::vector<double> wheel_positions_; ///< 轮子位置
|
||||
std::vector<double> joint_positions_; ///< 关节位置（弧度）
|
||||
std::vector<double> joint_velocities_; ///< 关节速度（弧度/秒）
|
||||
std::vector<double> wheel_velocities_; ///< 轮子速度（弧度/秒）
|
||||
std::vector<double> joint_efforts_; ///< 关节力矩
|
||||
|
||||
// 状态
|
||||
std::vector<double> wheel_commands_; ///< 轮子命令（wheel joint order）
|
||||
sensor_msgs::msg::JointState joint_states_; ///< 关节状态
|
||||
|
||||
// 临时变量（用于轮子控制器）
|
||||
std::vector<double> temp_wheel_cmd_; ///< 轮子临时命令
|
||||
|
||||
// IMU相关
|
||||
std::vector<double> gyro_values_; ///< 陀螺仪值
|
||||
std::vector<double> gyro_init_values_; ///< 陀螺仪初始值
|
||||
std::vector<double> gyro_velocities_; ///< 陀螺仪速度
|
||||
std::vector<double> gyro_accelerations_; ///< 陀螺仪加速度
|
||||
sensor_msgs::msg::Imu imu_msg_; ///< IMU消息
|
||||
bool is_gyro_inited_; ///< 陀螺仪是否已初始化
|
||||
|
||||
// 轮子状态相关
|
||||
interfaces::msg::MotorPos motor_pos_; ///< 电机位置消息
|
||||
|
||||
// 初始化相关
|
||||
bool is_joint_init_value_set_; ///< 关节初始化值是否已设置
|
||||
bool joint_name_mapping_initialized_; ///< 关节名称映射是否已初始化
|
||||
};
|
||||
}
|
||||
181
src/robot_control/include/core/robot_control_node.hpp
Normal file
181
src/robot_control/include/core/robot_control_node.hpp
Normal file
@@ -0,0 +1,181 @@
|
||||
/**
|
||||
* @file robot_control_node.hpp
|
||||
* @brief ROS 2 机器人控制节点 - 主控制节点
|
||||
* @author Ray
|
||||
* @date 2025
|
||||
*
|
||||
* 该类是ROS 2节点，负责：
|
||||
* - 创建和管理ROS 2话题、服务和动作服务器
|
||||
* - 协调ActionManager和RobotControlManager
|
||||
* - 处理传感器数据回调
|
||||
* - 执行控制循环
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "std_msgs/msg/float64_multi_array.hpp"
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
#include "core/robot_control_manager.hpp"
|
||||
#include "core/remote_controller.hpp"
|
||||
#include "services/arm_hardware_service.hpp"
|
||||
#include "services/kinematics_service.hpp"
|
||||
#include "services/arm_status_service.hpp"
|
||||
#include "services/safety_service.hpp"
|
||||
|
||||
#include "control_msgs/msg/dynamic_interface_group_values.hpp"
|
||||
#include "control_msgs/msg/dynamic_joint_state.hpp"
|
||||
#include <controller_manager_msgs/srv/switch_controller.hpp>
|
||||
#include "interfaces/srv/inverse_kinematics.hpp"
|
||||
#include "interfaces/srv/set_arm_enable.hpp"
|
||||
#include "interfaces/srv/clear_arm_error.hpp"
|
||||
#include "interfaces/msg/robot_arm_status.hpp"
|
||||
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
// 前向声明
|
||||
class ActionManager;
|
||||
|
||||
/**
|
||||
* @class RobotControlNode
|
||||
* @brief ROS 2机器人控制节点类
|
||||
*
|
||||
* 这是主要的ROS 2节点，负责：
|
||||
* - 管理ROS 2通信（发布器、订阅器、客户端、动作服务器）
|
||||
* - 执行控制循环
|
||||
* - 处理传感器数据
|
||||
* - 协调ActionManager和RobotControlManager
|
||||
*/
|
||||
class RobotControlNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数
|
||||
*/
|
||||
RobotControlNode();
|
||||
|
||||
/**
|
||||
* @brief 析构函数
|
||||
*/
|
||||
~RobotControlNode();
|
||||
|
||||
private:
|
||||
// ==================== ROS 2 通信接口 ====================
|
||||
|
||||
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr left_gpio_pub_; ///< 左臂GPIO发布器
|
||||
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr right_gpio_pub_; ///< 右臂GPIO发布器
|
||||
rclcpp::Publisher<interfaces::msg::RobotArmStatus>::SharedPtr robot_arm_status_pub_; ///< 机械臂状态发布器
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr joy_command_sub_; ///< 手柄命令订阅器
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_states_sub_; ///< 关节状态订阅器
|
||||
rclcpp::Subscription<MotorPos>::SharedPtr wheel_states_sub_; ///< 轮子状态订阅器
|
||||
rclcpp::Subscription<ImuMsg>::SharedPtr imu_msg_sub_; ///< IMU消息订阅器
|
||||
rclcpp::Subscription<control_msgs::msg::DynamicJointState>::SharedPtr dynamic_joint_states_sub_; ///< 动态关节状态订阅器
|
||||
rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr switch_client_; ///< 控制器切换客户端
|
||||
rclcpp::Service<interfaces::srv::InverseKinematics>::SharedPtr inverse_kinematics_srv_; ///< 单臂逆解服务
|
||||
rclcpp::Service<interfaces::srv::SetArmEnable>::SharedPtr set_arm_enable_srv_; ///< 上下使能服务
|
||||
rclcpp::Service<interfaces::srv::ClearArmError>::SharedPtr clear_arm_error_srv_; ///< 清错服务
|
||||
rclcpp::TimerBase::SharedPtr robot_arm_status_timer_; ///< 状态发布定时器
|
||||
|
||||
// ==================== 核心组件 ====================
|
||||
|
||||
RobotControlManager robot_control_manager_; ///< 机器人控制管理器
|
||||
std::unique_ptr<ActionManager> action_manager_; ///< 动作管理器
|
||||
std::unique_ptr<RemoteController> remote_controller_; ///< 遥控器控制器
|
||||
std::unique_ptr<ArmHardwareService> arm_hardware_service_; ///< 机械臂GPIO服务实现
|
||||
std::unique_ptr<KinematicsService> kinematics_service_; ///< 运动学服务实现
|
||||
std::unique_ptr<ArmStatusService> arm_status_service_; ///< 机械臂状态服务实现
|
||||
std::shared_ptr<SafetyService> safety_service_; ///< 急停状态服务实现
|
||||
|
||||
// ==================== 控制状态 ====================
|
||||
bool is_robot_enabled_; ///< 机器人是否启用
|
||||
std::vector<bool> joint_enabled_cache_; ///< 关节使能缓存（双臂顺序）
|
||||
std::vector<int32_t> joint_error_code_cache_; ///< 关节错误码缓存（双臂顺序）
|
||||
|
||||
// ==================== 回调函数 ====================
|
||||
|
||||
/**
|
||||
* @brief 关节状态回调函数
|
||||
* @param msg 关节状态消息
|
||||
*/
|
||||
void JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
|
||||
|
||||
/**
|
||||
* @brief 手柄命令回调函数
|
||||
* @param msg 手柄命令消息
|
||||
*/
|
||||
void JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg);
|
||||
|
||||
/**
|
||||
* @brief 轮子状态回调函数
|
||||
* @param cmd_msg 电机位置消息
|
||||
*/
|
||||
void WheelStatesCallback(const MotorPos::SharedPtr cmd_msg);
|
||||
|
||||
/**
|
||||
* @brief IMU消息回调函数
|
||||
* @param msg IMU消息
|
||||
*/
|
||||
void ImuMsgCallback(const ImuMsg::SharedPtr msg);
|
||||
/**
|
||||
* @brief 激活指定名称的控制器
|
||||
* @param controller_name 控制器名称
|
||||
* @return 激活是否成功
|
||||
*/
|
||||
bool ActivateController_(const std::string& controller_name);
|
||||
|
||||
/**
|
||||
* @brief 重置所有电机故障
|
||||
*/
|
||||
void MotorResetFaultAll_();
|
||||
|
||||
/**
|
||||
* @brief 启用所有电机
|
||||
* @param enable 使能标志（1为使能，0为禁用）
|
||||
*/
|
||||
void motor_enable_all(double enable);
|
||||
|
||||
/**
|
||||
* @brief 设置电机 fault（参考 test_motor.cpp）
|
||||
* @param id 关节序号（0=全部；否则按 dual_arm_joint_names_ 的 index+1 语义）
|
||||
* @param fault fault 值（通常 1 表示复位脉冲，0 表示释放）
|
||||
*/
|
||||
void MotorFault_(int id, double fault);
|
||||
|
||||
/**
|
||||
* @brief 设置电机 enable（参考 test_motor.cpp）
|
||||
* @param id 关节序号（0=全部；否则按 dual_arm_joint_names_ 的 index+1 语义）
|
||||
* @param enable 使能值（1=使能，0=禁用）
|
||||
*/
|
||||
void MotorEnable_(int id, double enable);
|
||||
/**
|
||||
* @brief 根据 dual_arm_joint_names_ 拆分左右臂关节名称
|
||||
*/
|
||||
bool SplitArmJointNames_(
|
||||
const MotionParameters& motion_params,
|
||||
std::vector<std::string>& left,
|
||||
std::vector<std::string>& right) const;
|
||||
|
||||
/**
|
||||
* @brief 发布GPIO命令（fault/enable）
|
||||
* @param pub 目标GPIO控制器发布器
|
||||
* @param joints 关节名称列表（对应 interface_groups）
|
||||
* @param interface_name 接口名称（fault 或 enable）
|
||||
* @param id 关节序号（0=全部）
|
||||
* @param value 指令值
|
||||
* @param offset 该列表在全局双臂数组中的起始偏移
|
||||
*/
|
||||
void PublishGpioCommand_(
|
||||
const rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr& pub,
|
||||
const std::vector<std::string>& joints,
|
||||
const std::string& interface_name,
|
||||
int id,
|
||||
double value,
|
||||
int offset) const;
|
||||
|
||||
};
|
||||
}
|
||||
@@ -1,43 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <Eigen/Dense>
|
||||
#include <fstream> // 用于保存轨迹数据
|
||||
|
||||
namespace robot_control {
|
||||
class EKF
|
||||
{
|
||||
private:
|
||||
double dt;
|
||||
|
||||
// 卡尔曼滤波参数
|
||||
Eigen::Matrix4d Q; // 过程噪声协方差矩阵
|
||||
Eigen::MatrixXd R; // 测量噪声协方差矩阵
|
||||
|
||||
// 状态向量 [x, y, θ, ω]
|
||||
Eigen::Vector4d X;
|
||||
// 协方差矩阵
|
||||
Eigen::Matrix4d P;
|
||||
|
||||
// 角度归一化到[-π, π]
|
||||
double normalizeAngle(double angle) {
|
||||
while (angle > M_PI) angle -= 2.0 * M_PI;
|
||||
while (angle < -M_PI) angle += 2.0 * M_PI;
|
||||
return angle;
|
||||
}
|
||||
|
||||
public:
|
||||
EKF(double init_x = 0, double init_y = 0, double init_theta = 0, double init_omega = 0);
|
||||
|
||||
// 预测步骤
|
||||
void predict(double v, double omega_wheel);
|
||||
|
||||
// 更新步骤
|
||||
void update(double gyro_omega);
|
||||
|
||||
// 获取当前状态
|
||||
Eigen::Vector4d getState() { return X; }
|
||||
};
|
||||
}
|
||||
@@ -1,40 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "trapezoidal_trajectory.hpp"
|
||||
#include "control_base.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
/**
|
||||
* @brief LegControl 类用于控制腿部运动
|
||||
*
|
||||
* 该类提供了腿部关节的控制功能，包括关节重置、移动和计算目标位置等操作。
|
||||
*/
|
||||
class LegControl : public ControlBase
|
||||
{
|
||||
public:
|
||||
LegControl(
|
||||
size_t total_joints,
|
||||
const std::vector<double>& lengthParameters,
|
||||
const std::vector<double>& maxSpeed,
|
||||
const std::vector<double>& maxAcc,
|
||||
const std::vector<double>& minLimits,
|
||||
const std::vector<double>& maxLimits,
|
||||
const std::vector<double>& home_positions,
|
||||
const std::vector<double>& zero_positions,
|
||||
const std::vector<int>& joint_directions
|
||||
);
|
||||
|
||||
~LegControl() override;
|
||||
|
||||
bool SetMoveLegParametersInternal(double moveDistance);
|
||||
|
||||
double calculate_angle_from_links(double side1, double side2, double side_opposite, bool is_degree = false);
|
||||
|
||||
bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
|
||||
|
||||
bool MoveUp(std::vector<double>& joint_positions, double dt);
|
||||
};
|
||||
|
||||
} // namespace robot_control
|
||||
@@ -1,320 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <map>
|
||||
#include <string>
|
||||
|
||||
|
||||
#define CYCLE_TIME 8 // 插补周期（毫秒）
|
||||
|
||||
#ifndef DEG2RAD
|
||||
#define DEG2RAD(x) ((x)*0.017453293)
|
||||
#endif
|
||||
|
||||
#ifndef RAD2DEG
|
||||
#define RAD2DEG(x) ((x)*57.29578)
|
||||
#endif
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
enum class LimitType {
|
||||
POSITION, // 位置限制（单位：度/弧度）
|
||||
VELOCITY, // 速度限制（单位：度/秒/弧度/秒）
|
||||
EFFORT // 力矩限制（单位：N·m）
|
||||
};
|
||||
|
||||
// 单个关节的限制参数：包含 max（最大值）、min（最小值）、限制类型
|
||||
struct JointLimit {
|
||||
int index; // 关节索引
|
||||
double max; // 关节运动范围上限（如位置最大角度）
|
||||
double min; // 关节运动范围下限（如位置最小角度）
|
||||
LimitType limit_type;// 限制类型（区分位置/速度/力矩）
|
||||
|
||||
// 构造函数：默认初始化（避免未定义值）
|
||||
JointLimit() : index(0), max(0.0), min(0.0), limit_type(LimitType::POSITION) {}
|
||||
|
||||
// 带参构造函数：快速初始化
|
||||
JointLimit(int i, double max_val, double min_val, LimitType type)
|
||||
:index(i), max(max_val), min(min_val), limit_type(type) {
|
||||
if (max < min) {
|
||||
std::cerr << "[Warning] JointLimit: max (" << max << ") < min (" << min << "), swapping values!" << std::endl;
|
||||
std::swap(max, min);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
class MotionParameters
|
||||
{
|
||||
public:
|
||||
MotionParameters()
|
||||
{
|
||||
// 初始化结构参数 unit m
|
||||
//TODO: 修改为实际参数
|
||||
wheel_radius_ = 0.09;
|
||||
wheel_separation_ = 0.55;
|
||||
|
||||
wheel_length_ = {
|
||||
wheel_radius_,
|
||||
wheel_separation_
|
||||
};
|
||||
|
||||
// 腿长参数 unit m
|
||||
leg_length_ = {
|
||||
0.66,
|
||||
0.385,
|
||||
0.362,
|
||||
0.55,
|
||||
0.4
|
||||
};
|
||||
|
||||
waist_length_ = {
|
||||
0.1,
|
||||
};
|
||||
|
||||
// 轮子速度参数
|
||||
max_linear_speed_x_ = 100;
|
||||
max_linear_speed_z_ = 10;
|
||||
max_angular_speed_z_ = 50;
|
||||
|
||||
// 关节速度参数
|
||||
max_joint_velocity_ = {
|
||||
30,
|
||||
30,
|
||||
50,
|
||||
50,
|
||||
50,
|
||||
50,
|
||||
50,
|
||||
50
|
||||
};
|
||||
|
||||
max_joint_acceleration_ = {
|
||||
80,
|
||||
80,
|
||||
100,
|
||||
100,
|
||||
100,
|
||||
100,
|
||||
100,
|
||||
100
|
||||
};
|
||||
|
||||
// 初始化关节索引
|
||||
wheel_joint_indices_ = {0, 1};
|
||||
|
||||
waist_joint_indices_ = {0, 1};
|
||||
|
||||
leg_joint_indices_ = {2, 3, 4, 5, 6, 7};
|
||||
|
||||
total_joints_ = 8;
|
||||
|
||||
// 初始化关节方向
|
||||
wheel_joint_directions_ = {1, -1};
|
||||
|
||||
waist_joint_directions_ = {1, 1};
|
||||
|
||||
leg_joint_directions_ = {-1, 1, 1, -1, -1, 1};
|
||||
|
||||
//TODO: check the ratio
|
||||
joint_gear_ratio_ = {100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}; // 传动比 都一样
|
||||
|
||||
joint_resolution_ = {524288, 524288, 524288 , 524288, 524288, 524288, 524288, 524288}; // 分辨率 都一样
|
||||
|
||||
|
||||
//TODO: 限位需要修改
|
||||
joint_limits_ = {
|
||||
JointLimit(0, 40.0, -40.0, LimitType::POSITION),
|
||||
JointLimit(1, 190.0, -190.0, LimitType::POSITION),
|
||||
JointLimit(2, 0.0, -90.0, LimitType::POSITION),
|
||||
JointLimit(3, 90.0, 0.0, LimitType::POSITION),
|
||||
JointLimit(4, 60, 0.0, LimitType::POSITION),
|
||||
JointLimit(5, 0.0, -90.0, LimitType::POSITION),
|
||||
JointLimit(6, 0.0, -60.0, LimitType::POSITION),
|
||||
JointLimit(7, 90.0, 0.0, LimitType::POSITION),
|
||||
};
|
||||
|
||||
// 初始化限制参数
|
||||
wheel_max_velocity_.resize(wheel_joint_indices_.size());
|
||||
wheel_max_acceleration_.resize(wheel_joint_indices_.size());
|
||||
wheel_max_limit_.resize(wheel_joint_indices_.size());
|
||||
wheel_min_limit_.resize(wheel_joint_indices_.size());
|
||||
|
||||
wheel_max_velocity_ = {
|
||||
5,
|
||||
5
|
||||
};
|
||||
wheel_max_acceleration_ = {
|
||||
25,
|
||||
25
|
||||
};
|
||||
|
||||
//There is no limit for wheel
|
||||
wheel_max_limit_ = {
|
||||
0.0,
|
||||
0.0
|
||||
};
|
||||
wheel_min_limit_ = {
|
||||
0.0,
|
||||
0.0
|
||||
};
|
||||
|
||||
waist_min_limit_.resize(waist_joint_indices_.size());
|
||||
waist_max_limit_.resize(waist_joint_indices_.size());
|
||||
waist_max_velocity_.resize(waist_joint_indices_.size());
|
||||
waist_max_acceleration_.resize(waist_joint_indices_.size());
|
||||
|
||||
leg_max_limit_.resize(leg_joint_indices_.size());
|
||||
leg_min_limit_.resize(leg_joint_indices_.size());
|
||||
leg_max_velocity_.resize(leg_joint_indices_.size());
|
||||
leg_max_acceleration_.resize(leg_joint_indices_.size());
|
||||
|
||||
for (size_t i = 0; i < waist_joint_indices_.size(); i++)
|
||||
{
|
||||
waist_max_limit_[i] = joint_limits_[waist_joint_indices_[i]].max;
|
||||
waist_min_limit_[i] = joint_limits_[waist_joint_indices_[i]].min;
|
||||
waist_max_velocity_[i] = max_joint_velocity_[waist_joint_indices_[i]];
|
||||
waist_max_acceleration_[i] = max_joint_acceleration_[waist_joint_indices_[i]];
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < leg_joint_indices_.size(); i++)
|
||||
{
|
||||
leg_max_limit_[i] = joint_limits_[leg_joint_indices_[i]].max;
|
||||
leg_min_limit_[i] = joint_limits_[leg_joint_indices_[i]].min;
|
||||
leg_max_velocity_[i] = max_joint_velocity_[leg_joint_indices_[i]];
|
||||
leg_max_acceleration_[i] = max_joint_acceleration_[leg_joint_indices_[i]];
|
||||
}
|
||||
|
||||
waist_zero_positions_ = {
|
||||
350.0,
|
||||
261.82
|
||||
};
|
||||
|
||||
leg_zero_positions_ = {
|
||||
217.52, //316786.46
|
||||
120.84, //175986.005
|
||||
108.7, //158305.849
|
||||
221.95, //323238.116
|
||||
234.14, //340991.09
|
||||
125.39 //182612.423
|
||||
};
|
||||
|
||||
waist_home_positions_ = {
|
||||
350.0,
|
||||
261.82
|
||||
};
|
||||
|
||||
|
||||
// 后腿零点为站直时候的状态, 然后 + 25度, 后腿长 0.66m, 25度为 0.6m
|
||||
// 前腿零点为站直时候的状态, 然后 髋部 pitch + 49度, 大腿长 0.38m, 38度为 0.25m , 小腿长0.36 膝盖 pitch 再收回 35.5 度, 剩余 13.5度 长度为 0.35m
|
||||
// 初始化初始位置
|
||||
leg_home_positions_ = {
|
||||
217.52 - 65.0,
|
||||
120.84 + 41.0,
|
||||
108.7 + 40.63, //217479
|
||||
221.95 - 41.0,
|
||||
234.14 - 29.504, //298023
|
||||
125.39 + 65.0
|
||||
};
|
||||
|
||||
// leg_home_positions_ = {
|
||||
// 217.52 - 90.0, //185.714.46
|
||||
// 120.84 + 90.0, //307058
|
||||
// 108.7 + 0.0, //158305
|
||||
// 221.95 - 90.0, //192166
|
||||
// 234.14 - 0.0, //340991
|
||||
// 125.39 + 90.0 //313684
|
||||
// };
|
||||
|
||||
// 初始化零点位置
|
||||
wheel_home_positions_ = {
|
||||
0.0,
|
||||
0.0
|
||||
};
|
||||
|
||||
wheel_zero_positions_ = {
|
||||
0.0,
|
||||
0.0
|
||||
};
|
||||
|
||||
pulse_to_degree_.resize(joint_resolution_.size());
|
||||
degree_to_pulse_.resize(joint_resolution_.size());
|
||||
|
||||
for (size_t i = 0; i < joint_resolution_.size(); i++)
|
||||
{
|
||||
degree_to_pulse_[i] = joint_resolution_[i] / 360.0;
|
||||
pulse_to_degree_[i] = 360.0 / joint_resolution_[i];
|
||||
}
|
||||
|
||||
jog_step_size_ = 10.0/ (1000.0 / CYCLE_TIME) ; // 5度每秒
|
||||
|
||||
};
|
||||
|
||||
// 运动参数
|
||||
size_t total_joints_; // 总关节数
|
||||
|
||||
// 关节索引
|
||||
std::vector<int> leg_joint_indices_; // 腿部关节索引
|
||||
std::vector<int> wheel_joint_indices_; // 轮子关节索引
|
||||
std::vector<int> waist_joint_indices_; // 腰部关节索引
|
||||
std::vector<int> real_waist_joint_indices_; // 实际腰部关节索引
|
||||
std::vector<int> real_leg_joint_indices_; // 实际腿部关节索引
|
||||
|
||||
std::vector<int> wheel_joint_directions_; // 轮子关节方向
|
||||
std::vector<int> waist_joint_directions_; // 身体关节方向
|
||||
std::vector<int> leg_joint_directions_; // 腿部关节方向
|
||||
|
||||
std::vector<double> leg_home_positions_; // 左腿初始位置
|
||||
std::vector<double> waist_home_positions_; // 身体初始位置
|
||||
std::vector<double> wheel_home_positions_; // 轮子零点位置
|
||||
|
||||
std::vector<double> waist_zero_positions_; // 身体零点位置
|
||||
std::vector<double> leg_zero_positions_; // 左腿零点位置
|
||||
std::vector<double> wheel_zero_positions_; // 轮子零点位置
|
||||
|
||||
// 限制参数
|
||||
std::vector<JointLimit> joint_limits_; // 关节限制
|
||||
std::vector<double> waist_min_limit_;
|
||||
std::vector<double> waist_max_limit_;
|
||||
std::vector<double> leg_min_limit_;
|
||||
std::vector<double> leg_max_limit_;
|
||||
std::vector<double> wheel_min_limit_;
|
||||
std::vector<double> wheel_max_limit_;
|
||||
|
||||
std::vector<double> max_joint_velocity_;
|
||||
std::vector<double> waist_max_velocity_;
|
||||
std::vector<double> leg_max_velocity_;
|
||||
std::vector<double> wheel_max_velocity_;
|
||||
|
||||
std::vector<double> max_joint_acceleration_;
|
||||
std::vector<double> waist_max_acceleration_;
|
||||
std::vector<double> leg_max_acceleration_;
|
||||
std::vector<double> wheel_max_acceleration_;
|
||||
|
||||
// 轮子控制相关
|
||||
double wheel_radius_; // 轮子半径（米）
|
||||
double wheel_separation_; // 轮距（米）
|
||||
double max_linear_speed_x_; // 线速度（m/s）
|
||||
double max_linear_speed_z_; // 线速度（m/s）
|
||||
double max_angular_speed_z_; // 角速度（rad/s）
|
||||
|
||||
// 尺寸相关
|
||||
std::vector<double> leg_length_;
|
||||
std::vector<double> waist_length_;
|
||||
std::vector<double> wheel_length_;
|
||||
|
||||
//关节传动比和分辨率
|
||||
std::vector<double> joint_gear_ratio_;
|
||||
std::vector<double> joint_resolution_;
|
||||
|
||||
std::vector<double> degree_to_pulse_; // 角度转脉冲的转换因子
|
||||
std::vector<double> pulse_to_degree_; // 脉冲转角度的转换因子
|
||||
|
||||
double jog_step_size_; // Jog步长
|
||||
|
||||
};
|
||||
}
|
||||
@@ -1,127 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "std_msgs/msg/float64_multi_array.hpp"
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "sensor_msgs/msg/joint_state.hpp"
|
||||
#include "sensor_msgs/msg/imu.hpp"
|
||||
#include "motion_parameters.hpp"
|
||||
#include "arm_control.hpp"
|
||||
#include "leg_control.hpp"
|
||||
#include "wheel_control.hpp"
|
||||
#include "robot_model.hpp"
|
||||
#include "common_enum.hpp"
|
||||
#include "waist_control.hpp"
|
||||
|
||||
#include "interfaces/msg/motor_pos.hpp"
|
||||
#include "interfaces/msg/imu_msg.hpp"
|
||||
|
||||
using MotorPos = interfaces::msg::MotorPos;
|
||||
using ImuMsg = sensor_msgs::msg::Imu;
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
class RobotControlManager
|
||||
{
|
||||
public:
|
||||
RobotControlManager();
|
||||
~RobotControlManager();
|
||||
|
||||
// 控制机器人运动
|
||||
bool MoveLeg(double dt);
|
||||
bool MoveWaist(double dt);
|
||||
bool MoveWheel();
|
||||
bool Stop(double dt);
|
||||
bool GoHome(double dt);
|
||||
void JogAxis(size_t axis, int dir);
|
||||
void SetJogWheel(bool value);
|
||||
bool GetJogWheel();
|
||||
|
||||
void WheelReset(){isWheelHomed_ = false;};
|
||||
void ImuReset(){isGyroInited_ = false;}
|
||||
|
||||
// 检查参数是否合理
|
||||
bool SetMoveWheelParameters(double moveWheelDistance, double moveWheelAngle);
|
||||
bool SetMoveLegParameters(double moveLegDistance);
|
||||
bool SetMoveWaistParameters(double movePitchAngle, double moveYawAngle);
|
||||
|
||||
// 机器人关节指令
|
||||
std_msgs::msg::Float64MultiArray GetJointCommands();
|
||||
std_msgs::msg::Float64MultiArray GetJointFeedback();
|
||||
std_msgs::msg::Float64MultiArray GetWheelCommands();
|
||||
std_msgs::msg::Float64MultiArray GetWheelFeedback();
|
||||
double GetWheelRatio();
|
||||
|
||||
// 机器人状态
|
||||
void CheckJointLimits(std_msgs::msg::Float64MultiArray& jointCommands);
|
||||
void UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg);
|
||||
void UpdateWheelStates(const MotorPos::SharedPtr msg);
|
||||
void UpdateImuMsg(const ImuMsg::SharedPtr msg);
|
||||
|
||||
MotionParameters GetMotionParameters();
|
||||
|
||||
std::vector<double> GetImuDifference();
|
||||
|
||||
bool IsMoving(MovementPart part);
|
||||
|
||||
bool RobotInitFinished();
|
||||
|
||||
private:
|
||||
void init();
|
||||
|
||||
MotionParameters motionParams_;
|
||||
|
||||
bool isWaistHomed_;
|
||||
bool isLegHomed_;
|
||||
bool isWheelHomed_;
|
||||
|
||||
bool is_wheel_jog_;
|
||||
|
||||
// 控制器
|
||||
LegControl* legController_;
|
||||
WheelControl* wheelController_;
|
||||
WaistControl* waistController_;
|
||||
|
||||
// 运动状态
|
||||
std::vector<double> wheelPositions_;
|
||||
std::vector<double> jointPositions_; // 关节位置（弧度）
|
||||
|
||||
std::vector<double> jointVelocities_; // 关节速度（弧度/秒）
|
||||
std::vector<double> wheelVelocities_; // 关节速度（弧度/秒）
|
||||
|
||||
std::vector<double> jointAccelerations_; // 关节加速度（弧度/秒^2）
|
||||
std::vector<double> wheelAccelerations_; // 关节加速度（弧度/秒^2）
|
||||
|
||||
std::vector<double> jointEfforts_; // 关节力矩（牛顿米）
|
||||
std::vector<double> wheelEfforts_; // 关节力矩（牛顿米）
|
||||
|
||||
std::vector<bool> jointInited_; // 机器人是否已经初始化
|
||||
bool isJointInitValueSet_;
|
||||
|
||||
// 陀螺仪状态
|
||||
ImuMsg imuMsg_;
|
||||
bool isGyroInited_;
|
||||
std::vector<double> gyroValues_; // 陀螺仪数据（弧度/秒）
|
||||
std::vector<double> gyroInitValues_; // 陀螺仪初始位置
|
||||
std::vector<double> gyroVelocities_; // 陀螺仪速度（弧度/秒）
|
||||
std::vector<double> gyroAccelerations_; // 陀螺仪加速度（弧度/秒^2）
|
||||
|
||||
// 临时变量
|
||||
std::vector<double> tempWaistCmd_;
|
||||
std::vector<double> tempLegCmd_;
|
||||
std::vector<double> tempWheelCmd_;
|
||||
|
||||
MotorPos motorPos_;
|
||||
sensor_msgs::msg::JointState jointStates_;
|
||||
std_msgs::msg::Float64MultiArray jointCommands_;
|
||||
std_msgs::msg::Float64MultiArray wheelCommands_;
|
||||
|
||||
void AssignTempValues();
|
||||
void UpdateJointCommands();
|
||||
|
||||
void QuaternionToRPYRad(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw);
|
||||
void QuaternionToRPYDeg(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw);
|
||||
};
|
||||
|
||||
} // namespace robot_control_manager
|
||||
@@ -1,117 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <filesystem>
|
||||
#include <fstream> // 添加这行来支持文件流操作
|
||||
#include <time.h>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "std_msgs/msg/float64_multi_array.hpp"
|
||||
#include "trajectory_msgs/msg/joint_trajectory.hpp"
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
#include "robot_control_manager.hpp"
|
||||
|
||||
#include "interfaces/action/move_home.hpp"
|
||||
#include "interfaces/action/move_leg.hpp"
|
||||
#include "interfaces/action/move_waist.hpp"
|
||||
#include "interfaces/action/move_wheel.hpp"
|
||||
#include "interfaces/msg/motor_cmd.hpp"
|
||||
#include "interfaces/srv/motor_param.hpp"
|
||||
|
||||
#define RECORD_FLAG 0
|
||||
|
||||
using MoveHome = interfaces::action::MoveHome;
|
||||
using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle<MoveHome>;
|
||||
|
||||
using MoveLeg = interfaces::action::MoveLeg;
|
||||
using GoalHandleMoveLeg = rclcpp_action::ServerGoalHandle<MoveLeg>;
|
||||
|
||||
using MoveWaist = interfaces::action::MoveWaist;
|
||||
using GoalHandleMoveWaist = rclcpp_action::ServerGoalHandle<MoveWaist>;
|
||||
|
||||
using MoveWheel = interfaces::action::MoveWheel;
|
||||
using GoalHandleMoveWheel = rclcpp_action::ServerGoalHandle<MoveWheel>;
|
||||
|
||||
using MotorCmd = interfaces::msg::MotorCmd;
|
||||
|
||||
using MotorParam = interfaces::srv::MotorParam;
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
class RobotControlNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
RobotControlNode();
|
||||
~RobotControlNode();
|
||||
|
||||
// 状态机主循环
|
||||
void ControlLoop();
|
||||
|
||||
rclcpp_action::GoalResponse handle_move_home_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const MoveHome::Goal> goal);
|
||||
rclcpp_action::CancelResponse handle_move_home_cancel(const std::shared_ptr<GoalHandleMoveHome> goal_handle);
|
||||
void handle_move_home_accepted(const std::shared_ptr<GoalHandleMoveHome> goal_handle);
|
||||
void move_home_execute(const std::shared_ptr<GoalHandleMoveHome> goal_handle);
|
||||
|
||||
rclcpp_action::GoalResponse handle_move_leg_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const MoveLeg::Goal> goal);
|
||||
rclcpp_action::CancelResponse handle_move_leg_cancel(const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
|
||||
void handle_move_leg_accepted(const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
|
||||
void move_leg_execute(const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
|
||||
|
||||
rclcpp_action::GoalResponse handle_move_waist_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const MoveWaist::Goal> goal);
|
||||
rclcpp_action::CancelResponse handle_move_waist_cancel(const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
|
||||
void handle_move_waist_accepted(const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
|
||||
void move_waist_execute(const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
|
||||
|
||||
rclcpp_action::GoalResponse handle_move_wheel_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const MoveWheel::Goal> goal);
|
||||
rclcpp_action::CancelResponse handle_move_wheel_cancel(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
|
||||
void handle_move_wheel_accepted(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
|
||||
void move_wheel_execute(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
|
||||
|
||||
private:
|
||||
|
||||
rclcpp_action::Server<MoveHome>::SharedPtr move_home_action_server_;
|
||||
bool move_home_executing_;
|
||||
rclcpp_action::Server<MoveLeg>::SharedPtr move_leg_action_server_;
|
||||
bool move_leg_executing_;
|
||||
rclcpp_action::Server<MoveWaist>::SharedPtr move_waist_action_server_;
|
||||
bool move_waist_executing_;
|
||||
rclcpp_action::Server<MoveWheel>::SharedPtr move_wheel_action_server_;
|
||||
bool move_wheel_executing_;
|
||||
|
||||
bool isStopping_;
|
||||
|
||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr ethercatSetPub_;
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr joyCommandSub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_;
|
||||
rclcpp::Publisher<MotorCmd>::SharedPtr motorCmdPub_;
|
||||
rclcpp::Subscription<MotorPos>::SharedPtr wheelStatesSub_;
|
||||
rclcpp::Subscription<ImuMsg>::SharedPtr imuMsgSub_;
|
||||
rclcpp::Client<MotorParam>::SharedPtr motorParamClient_;
|
||||
|
||||
RobotControlManager robotControlManager_;
|
||||
|
||||
// 文件流相关
|
||||
std::ofstream data_file_; // 用于写入数据的文件流
|
||||
std::string data_file_path_; // 数据文件路径
|
||||
|
||||
rclcpp::TimerBase::SharedPtr controlTimer_;
|
||||
rclcpp::Time lastTime_; // 移至类成员
|
||||
|
||||
bool isJogMode_;
|
||||
|
||||
int jogDirection_;
|
||||
size_t jogIndex_;
|
||||
double jogValue_;
|
||||
|
||||
double lastSpeed_;
|
||||
|
||||
void Publish_joint_trajectory();
|
||||
|
||||
void JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
|
||||
void JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg);
|
||||
void WheelStatesCallback(const MotorPos::SharedPtr cmd_msg);
|
||||
void ImuMsgCallback(const ImuMsg::SharedPtr msg);
|
||||
};
|
||||
|
||||
} // namespace robot_control
|
||||
|
||||
@@ -1,90 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "robot_model.hpp"
|
||||
#include <Eigen/Dense>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
|
||||
/**
|
||||
* @class RobotKinematics
|
||||
* @brief 机器人运动学计算类，处理手臂和腿部的正逆运动学计算
|
||||
*/
|
||||
class RobotKinematics
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数
|
||||
* @param robot_model 机器人模型指针
|
||||
*/
|
||||
explicit RobotKinematics(std::shared_ptr<RobotModel> robot_model);
|
||||
|
||||
/**
|
||||
* @brief 析构函数
|
||||
*/
|
||||
~RobotKinematics() = default;
|
||||
|
||||
// 手臂运动学
|
||||
|
||||
/**
|
||||
* @brief 计算手臂正运动学
|
||||
* @param joint_angles 关节角度数组
|
||||
* @return 末端执行器位置 (x, y, z)
|
||||
*/
|
||||
Eigen::Vector3d arm_forward_kinematics(const std::vector<double>& joint_angles);
|
||||
|
||||
/**
|
||||
* @brief 计算手臂逆运动学
|
||||
* @param target_pos 目标位置 (x, y, z)
|
||||
* @param[out] joint_angles 计算得到的关节角度
|
||||
* @return 成功返回true，失败返回false
|
||||
*/
|
||||
bool arm_inverse_kinematics(const Eigen::Vector3d& target_pos, std::vector<double>& joint_angles);
|
||||
|
||||
// 腿部运动学
|
||||
|
||||
/**
|
||||
* @brief 计算腿部正运动学
|
||||
* @param leg_index 腿的索引 (0-3)
|
||||
* @param joint_angles 关节角度数组
|
||||
* @return 足部位置 (x, y, z)
|
||||
*/
|
||||
Eigen::Vector3d leg_forward_kinematics(size_t leg_index, const std::vector<double>& joint_angles);
|
||||
|
||||
/**
|
||||
* @brief 计算腿部逆运动学
|
||||
* @param leg_index 腿的索引 (0-3)
|
||||
* @param target_pos 目标位置 (x, y, z)
|
||||
* @param[out] joint_angles 计算得到的关节角度
|
||||
* @return 成功返回true，失败返回false
|
||||
*/
|
||||
bool leg_inverse_kinematics(size_t leg_index, const Eigen::Vector3d& target_pos, std::vector<double>& joint_angles);
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief 创建旋转矩阵 (绕Z轴)
|
||||
* @param angle 旋转角度(弧度)
|
||||
* @return 旋转矩阵
|
||||
*/
|
||||
Eigen::Matrix3d rotate_z(double angle);
|
||||
|
||||
/**
|
||||
* @brief 创建旋转矩阵 (绕Y轴)
|
||||
* @param angle 旋转角度(弧度)
|
||||
* @return 旋转矩阵
|
||||
*/
|
||||
Eigen::Matrix3d rotate_y(double angle);
|
||||
|
||||
/**
|
||||
* @brief 创建旋转矩阵 (绕X轴)
|
||||
* @param angle 旋转角度(弧度)
|
||||
* @return 旋转矩阵
|
||||
*/
|
||||
Eigen::Matrix3d rotate_x(double angle);
|
||||
|
||||
std::shared_ptr<RobotModel> robot_model_; // 机器人模型指针
|
||||
};
|
||||
|
||||
} // namespace robot_model
|
||||
@@ -1,111 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
|
||||
class UrdfParser;
|
||||
|
||||
// 关节结构体
|
||||
struct Joint
|
||||
{
|
||||
std::string name; // 关节名称
|
||||
double angle; // 关节角度（弧度）
|
||||
double min_angle; // 最小角度限制
|
||||
double max_angle; // 最大角度限制
|
||||
double effort_limit; // 力/力矩限制
|
||||
|
||||
Joint(const std::string & joint_name, double min, double max, double effort)
|
||||
: name(joint_name), angle(0.0), min_angle(min), max_angle(max), effort_limit(effort) {}
|
||||
};
|
||||
|
||||
// 连杆结构体
|
||||
struct Link
|
||||
{
|
||||
std::string name; // 连杆名称
|
||||
double length; // 连杆长度
|
||||
double mass; // 连杆质量
|
||||
|
||||
Link(const std::string & link_name, double len, double m)
|
||||
: name(link_name), length(len), mass(m) {}
|
||||
};
|
||||
|
||||
// 机械臂结构体
|
||||
struct Arm
|
||||
{
|
||||
std::vector<std::shared_ptr<Joint>> joints; // 关节
|
||||
std::vector<std::shared_ptr<Link>> links; // 连杆
|
||||
std::string side; // 机械臂位置（如"left", "right"）
|
||||
};
|
||||
|
||||
// 腿部结构体
|
||||
struct Leg
|
||||
{
|
||||
std::vector<std::shared_ptr<Joint>> joints; // 关节
|
||||
std::vector<std::shared_ptr<Link>> links; // 连杆
|
||||
std::string side; // 腿部位置（如"front_left", "front_right", "rear_left", "rear_right"）
|
||||
};
|
||||
|
||||
class RobotModel
|
||||
{
|
||||
// 声明UrdfParser为友元类，使其可以访问私有成员
|
||||
friend class UrdfParser;
|
||||
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数
|
||||
*/
|
||||
RobotModel();
|
||||
|
||||
/**
|
||||
* @brief 析构函数
|
||||
*/
|
||||
~RobotModel() = default;
|
||||
|
||||
/**
|
||||
* @brief 获取机械臂
|
||||
* @return 机械臂对象
|
||||
*/
|
||||
const Arm & get_arm(size_t index) const;
|
||||
|
||||
/**
|
||||
* @brief 获取腿部
|
||||
* @param index 腿的索引（0-3）
|
||||
* @return 腿部对象
|
||||
*/
|
||||
const Leg & get_leg(size_t index) const;
|
||||
|
||||
/**
|
||||
* @brief 设置关节角度
|
||||
* @param joint_name 关节名称
|
||||
* @param angle 目标角度（弧度）
|
||||
* @return 成功返回true，失败返回false
|
||||
*/
|
||||
bool set_joint_angle(const std::string & joint_name, double angle);
|
||||
|
||||
/**
|
||||
* @brief 获取关节角度
|
||||
* @param joint_name 关节名称
|
||||
* @return 关节角度（弧度），未找到返回0.0
|
||||
*/
|
||||
double get_joint_angle(const std::string & joint_name) const;
|
||||
|
||||
/**
|
||||
* @brief 打印机器人模型信息
|
||||
*/
|
||||
void print_model_info() const;
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief 初始化机器人模型
|
||||
*/
|
||||
void initialize_model();
|
||||
|
||||
std::vector<Arm> arms_; // 机械臂集合 （2个机械臂）
|
||||
std::vector<Leg> legs_; // 腿部集合（4条腿）
|
||||
};
|
||||
|
||||
} // namespace robot_CONTROL
|
||||
88
src/robot_control/include/services/arm_hardware_service.hpp
Normal file
88
src/robot_control/include/services/arm_hardware_service.hpp
Normal file
@@ -0,0 +1,88 @@
|
||||
/**
|
||||
* @file arm_hardware_service.hpp
|
||||
* @brief 手臂硬件服务，提供使能与错误清除接口
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "control_msgs/msg/dynamic_interface_group_values.hpp"
|
||||
#include "interfaces/srv/set_arm_enable.hpp"
|
||||
#include "interfaces/srv/clear_arm_error.hpp"
|
||||
#include "core/robot_control_manager.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
|
||||
class ArmHardwareService
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数，初始化手臂硬件服务
|
||||
* @param node ROS 2 节点指针
|
||||
* @param manager 机器人控制管理器指针
|
||||
* @param left_gpio_pub 左臂 GPIO 发布器
|
||||
* @param right_gpio_pub 右臂 GPIO 发布器
|
||||
* @param joint_enabled_cache 关节使能缓存指针
|
||||
* @param joint_error_code_cache 关节错误码缓存指针
|
||||
*/
|
||||
ArmHardwareService(
|
||||
rclcpp::Node* node,
|
||||
RobotControlManager* manager,
|
||||
const rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr& left_gpio_pub,
|
||||
const rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr& right_gpio_pub,
|
||||
std::vector<bool>* joint_enabled_cache,
|
||||
std::vector<int32_t>* joint_error_code_cache);
|
||||
|
||||
/**
|
||||
* @brief 处理设置手臂使能服务请求
|
||||
* @param request 服务请求（包含手臂 ID、关节编号及使能标志）
|
||||
* @param response 服务响应（包含操作成功标志）
|
||||
*/
|
||||
void HandleSetArmEnable(
|
||||
const std::shared_ptr<interfaces::srv::SetArmEnable::Request> request,
|
||||
std::shared_ptr<interfaces::srv::SetArmEnable::Response> response);
|
||||
|
||||
/**
|
||||
* @brief 处理清除手臂错误服务请求
|
||||
* @param request 服务请求（包含手臂 ID 和关节编号）
|
||||
* @param response 服务响应（包含操作成功标志）
|
||||
*/
|
||||
void HandleClearArmError(
|
||||
const std::shared_ptr<interfaces::srv::ClearArmError::Request> request,
|
||||
std::shared_ptr<interfaces::srv::ClearArmError::Response> response);
|
||||
|
||||
private:
|
||||
bool validate_arm_and_joint_(int8_t arm_id, int8_t joint_num) const;
|
||||
bool set_motor_enable_by_scope_(int8_t arm_id, int8_t joint_num, bool enable);
|
||||
bool clear_motor_error_by_scope_(int8_t arm_id, int8_t joint_num);
|
||||
bool split_arm_joint_names_(
|
||||
const MotionParameters& motion_params,
|
||||
std::vector<std::string>& left,
|
||||
std::vector<std::string>& right) const;
|
||||
void publish_gpio_command_for_arm_(
|
||||
const rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr& pub,
|
||||
const std::vector<std::string>& joints,
|
||||
const std::string& interface_name,
|
||||
int local_joint_num,
|
||||
double value) const;
|
||||
void update_cached_joint_state_(
|
||||
int8_t arm_id,
|
||||
int8_t joint_num,
|
||||
bool enabled,
|
||||
int32_t error_code);
|
||||
void clear_error_cache_(int8_t arm_id, int8_t joint_num);
|
||||
|
||||
rclcpp::Node* node_{nullptr};
|
||||
RobotControlManager* manager_{nullptr};
|
||||
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr left_gpio_pub_;
|
||||
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr right_gpio_pub_;
|
||||
std::vector<bool>* joint_enabled_cache_{nullptr};
|
||||
std::vector<int32_t>* joint_error_code_cache_{nullptr};
|
||||
};
|
||||
|
||||
} // namespace robot_control
|
||||
62
src/robot_control/include/services/arm_status_service.hpp
Normal file
62
src/robot_control/include/services/arm_status_service.hpp
Normal file
@@ -0,0 +1,62 @@
|
||||
/**
|
||||
* @file arm_status_service.hpp
|
||||
* @brief 手臂状态服务，周期性发布末端位姿
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "control_msgs/msg/dynamic_joint_state.hpp"
|
||||
#include "interfaces/msg/robot_arm_status.hpp"
|
||||
#include "core/robot_control_manager.hpp"
|
||||
#include "services/safety_service.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
|
||||
class ArmStatusService
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数，初始化手臂状态服务
|
||||
* @param node ROS 2 节点指针
|
||||
* @param manager 机器人控制管理器指针
|
||||
* @param safety_service 安全服务指针
|
||||
* @param status_pub 手臂状态发布器
|
||||
* @param joint_enabled_cache 关节使能缓存指针
|
||||
* @param joint_error_code_cache 关节错误码缓存指针
|
||||
*/
|
||||
ArmStatusService(
|
||||
rclcpp::Node* node,
|
||||
RobotControlManager* manager,
|
||||
SafetyService* safety_service,
|
||||
const rclcpp::Publisher<interfaces::msg::RobotArmStatus>::SharedPtr& status_pub,
|
||||
std::vector<bool>* joint_enabled_cache,
|
||||
std::vector<int32_t>* joint_error_code_cache);
|
||||
|
||||
/**
|
||||
* @brief 处理动态关节状态消息回调
|
||||
* @param msg 动态关节状态消息
|
||||
*/
|
||||
void OnDynamicJointStates(const control_msgs::msg::DynamicJointState::SharedPtr msg);
|
||||
|
||||
/**
|
||||
* @brief 发布机器人手臂状态
|
||||
*/
|
||||
void PublishRobotArmStatus();
|
||||
|
||||
private:
|
||||
rclcpp::Node* node_{nullptr};
|
||||
RobotControlManager* manager_{nullptr};
|
||||
SafetyService* safety_service_{nullptr};
|
||||
rclcpp::Publisher<interfaces::msg::RobotArmStatus>::SharedPtr status_pub_;
|
||||
std::vector<bool>* joint_enabled_cache_{nullptr};
|
||||
std::vector<int32_t>* joint_error_code_cache_{nullptr};
|
||||
bool publish_arm_pose_{true};
|
||||
};
|
||||
|
||||
} // namespace robot_control
|
||||
41
src/robot_control/include/services/kinematics_service.hpp
Normal file
41
src/robot_control/include/services/kinematics_service.hpp
Normal file
@@ -0,0 +1,41 @@
|
||||
/**
|
||||
* @file kinematics_service.hpp
|
||||
* @brief 运动学服务，提供 IK 解算 ROS 接口
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "interfaces/srv/inverse_kinematics.hpp"
|
||||
#include "core/robot_control_manager.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
|
||||
class KinematicsService
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数，初始化运动学服务
|
||||
* @param node ROS 2 节点指针
|
||||
* @param manager 机器人控制管理器指针
|
||||
*/
|
||||
KinematicsService(rclcpp::Node* node, RobotControlManager* manager);
|
||||
|
||||
/**
|
||||
* @brief 处理逆运动学求解服务请求
|
||||
* @param request 服务请求（包含目标位姿和手臂 ID）
|
||||
* @param response 服务响应（包含关节角解和成功标志）
|
||||
*/
|
||||
void HandleInverseKinematics(
|
||||
const std::shared_ptr<interfaces::srv::InverseKinematics::Request> request,
|
||||
std::shared_ptr<interfaces::srv::InverseKinematics::Response> response);
|
||||
|
||||
private:
|
||||
rclcpp::Node* node_{nullptr};
|
||||
RobotControlManager* manager_{nullptr};
|
||||
};
|
||||
|
||||
} // namespace robot_control
|
||||
59
src/robot_control/include/services/motor_service.hpp
Normal file
59
src/robot_control/include/services/motor_service.hpp
Normal file
@@ -0,0 +1,59 @@
|
||||
/**
|
||||
* @file motor_service.hpp
|
||||
* @brief 封装 MotorCmd 发布与 MotorParam 服务调用的服务类
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include "interfaces/msg/motor_cmd.hpp"
|
||||
#include "interfaces/srv/motor_param.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
/**
|
||||
* @class MotorService
|
||||
* @brief 负责底层电机相关 ROS 通信（MotorCmd 发布与 MotorParam 服务调用）
|
||||
*
|
||||
* 上层（例如 ActionManager / MoveWheelAction）只和本类交互，
|
||||
* 不直接持有 publisher/client，便于集中管理和今后扩展。
|
||||
*/
|
||||
class MotorService
|
||||
{
|
||||
public:
|
||||
using MotorCmd = interfaces::msg::MotorCmd;
|
||||
using MotorParam = interfaces::srv::MotorParam;
|
||||
|
||||
/**
|
||||
* @brief 构造函数，初始化电机服务并创建发布器与服务客户端
|
||||
* @param node ROS 2 节点指针
|
||||
*/
|
||||
explicit MotorService(rclcpp::Node* node);
|
||||
|
||||
/**
|
||||
* @brief 根据运动距离与比例配置轮子速度（封装 MotorParam 调用逻辑）
|
||||
* @param move_distance 轮子移动距离
|
||||
* @param ratio 速度比例（由 RobotControlManager 提供）
|
||||
* @param is_jog_mode 当前是否处于点动模式
|
||||
*/
|
||||
void ConfigureWheelSpeed(double move_distance, double ratio, bool is_jog_mode);
|
||||
|
||||
/**
|
||||
* @brief 发布轮子角度命令（封装 MotorCmd 发布逻辑）
|
||||
* @param wheel_commands 轮子目标角度（至少包含两个元素）
|
||||
*/
|
||||
void PublishWheelCommand(const std::vector<double>& wheel_commands);
|
||||
|
||||
private:
|
||||
rclcpp::Node* node_{nullptr};
|
||||
rclcpp::Publisher<MotorCmd>::SharedPtr motor_cmd_pub_;
|
||||
rclcpp::Client<MotorParam>::SharedPtr motor_param_client_;
|
||||
};
|
||||
|
||||
} // namespace robot_control
|
||||
|
||||
|
||||
66
src/robot_control/include/services/safety_service.hpp
Normal file
66
src/robot_control/include/services/safety_service.hpp
Normal file
@@ -0,0 +1,66 @@
|
||||
/**
|
||||
* @file safety_service.hpp
|
||||
* @brief 安全服务，管理急停状态与解锁
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "interfaces/msg/e_stop_state.hpp"
|
||||
#include "interfaces/srv/reset_e_stop.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
|
||||
class SafetyService
|
||||
{
|
||||
public:
|
||||
using EStopState = interfaces::msg::EStopState;
|
||||
using ResetEStop = interfaces::srv::ResetEStop;
|
||||
|
||||
/**
|
||||
* @brief 构造函数，初始化安全服务并创建订阅与服务端
|
||||
* @param node ROS 2 节点指针
|
||||
*/
|
||||
explicit SafetyService(rclcpp::Node* node);
|
||||
|
||||
/**
|
||||
* @brief 处理急停状态消息回调
|
||||
* @param msg 急停状态消息
|
||||
*/
|
||||
void OnEStopState(const EStopState::SharedPtr msg);
|
||||
|
||||
/**
|
||||
* @brief 处理急停复位服务请求
|
||||
* @param request 服务请求
|
||||
* @param response 服务响应
|
||||
*/
|
||||
void HandleResetEStop(
|
||||
const std::shared_ptr<ResetEStop::Request> request,
|
||||
std::shared_ptr<ResetEStop::Response> response);
|
||||
|
||||
/**
|
||||
* @brief 查询当前急停是否已锁定
|
||||
* @return true 急停已锁定，false 未锁定
|
||||
*/
|
||||
bool IsEstopLatched() const { return estop_latched_.load(); }
|
||||
|
||||
/**
|
||||
* @brief 获取当前急停生成计数（每次复位后递增）
|
||||
* @return 急停生成计数
|
||||
*/
|
||||
uint32_t GetEstopGeneration() const { return estop_generation_.load(); }
|
||||
|
||||
private:
|
||||
rclcpp::Node* node_{nullptr};
|
||||
rclcpp::Subscription<EStopState>::SharedPtr estop_sub_;
|
||||
rclcpp::Service<ResetEStop>::SharedPtr reset_estop_srv_;
|
||||
std::atomic<bool> estop_state_received_{false};
|
||||
std::atomic<bool> estop_latched_{false};
|
||||
std::atomic<uint32_t> estop_generation_{0};
|
||||
};
|
||||
|
||||
} // namespace robot_control
|
||||
@@ -1,76 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <urdf/model.h>
|
||||
|
||||
#include "robot_model.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
|
||||
class UrdfParser
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数
|
||||
*/
|
||||
UrdfParser() = default;
|
||||
|
||||
/**
|
||||
* @brief 析构函数
|
||||
*/
|
||||
~UrdfParser() = default;
|
||||
|
||||
/**
|
||||
* @brief 从文件解析URDF并构建RobotModel
|
||||
* @param urdf_file_path URDF文件路径
|
||||
* @return 构建的RobotModel共享指针，失败返回nullptr
|
||||
*/
|
||||
std::shared_ptr<RobotModel> parse_from_file(const std::string& urdf_file_path);
|
||||
|
||||
/**
|
||||
* @brief 从XML字符串解析URDF并构建RobotModel
|
||||
* @param urdf_xml URDF XML字符串
|
||||
* @return 构建的RobotModel共享指针，失败返回nullptr
|
||||
*/
|
||||
std::shared_ptr<RobotModel> parse_from_string(const std::string& urdf_xml);
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief 解析URDF模型并构建RobotModel
|
||||
* @param model URDF模型
|
||||
* @return 构建的RobotModel共享指针
|
||||
*/
|
||||
std::shared_ptr<RobotModel> build_robot_model(const urdf::Model& model);
|
||||
|
||||
/**
|
||||
* @brief 解析连杆信息
|
||||
* @param link URDF连杆
|
||||
* @return 构建的Link共享指针
|
||||
*/
|
||||
std::shared_ptr<Link> parse_link(const urdf::LinkConstSharedPtr& link);
|
||||
|
||||
/**
|
||||
* @brief 解析关节信息
|
||||
* @param joint URDF关节
|
||||
* @return 构建的Joint共享指针
|
||||
*/
|
||||
std::shared_ptr<Joint> parse_joint(const urdf::JointConstSharedPtr& joint);
|
||||
|
||||
/**
|
||||
* @brief 识别关节所属部分（手臂、腿部等）
|
||||
* @param joint_name 关节名称
|
||||
* @return 关节所属部分的字符串标识
|
||||
*/
|
||||
std::string identify_joint_part(const std::string& joint_name);
|
||||
|
||||
/**
|
||||
* @brief 识别腿部位置（前左、前右、后左、后右）
|
||||
* @param joint_name 关节名称
|
||||
* @return 腿部位置字符串
|
||||
*/
|
||||
std::string identify_leg_side(const std::string& joint_name);
|
||||
};
|
||||
|
||||
} // namespace robot_control
|
||||
@@ -1,3 +1,8 @@
|
||||
/**
|
||||
* @file deceleration_planner.hpp
|
||||
* @brief 减速规划工具函数
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
@@ -5,6 +10,12 @@
|
||||
#include <cmath>
|
||||
#include <stdexcept>
|
||||
|
||||
/**
|
||||
* @brief 计算从给定初速度以最大减速度减速至零所需的时间
|
||||
* @param initial_velocity 初始速度（m/s 或 rad/s）
|
||||
* @param max_decel 最大减速度（正数，m/s² 或 rad/s²）
|
||||
* @return 所需减速时间（秒）；初速度为零时返回 0
|
||||
*/
|
||||
inline double calculate_decel_time(double initial_velocity, double max_decel) {
|
||||
if (std::fabs(initial_velocity) < 1e-9) { // 速度为0，无需减速
|
||||
return 0.0;
|
||||
31
src/robot_control/include/utils/duration_utils.hpp
Normal file
31
src/robot_control/include/utils/duration_utils.hpp
Normal file
@@ -0,0 +1,31 @@
|
||||
/**
|
||||
* @file duration_utils.hpp
|
||||
* @brief builtin_interfaces::Duration 与 double 秒数的转换，供 arm_control、dual_arm_action、trajectory_blend 等复用
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "builtin_interfaces/msg/duration.hpp"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
|
||||
inline double duration_to_sec(const builtin_interfaces::msg::Duration& d)
|
||||
{
|
||||
return static_cast<double>(d.sec) + static_cast<double>(d.nanosec) * 1e-9;
|
||||
}
|
||||
|
||||
inline void sec_to_duration(double sec, builtin_interfaces::msg::Duration* out)
|
||||
{
|
||||
if (!out) return;
|
||||
if (!std::isfinite(sec) || sec < 0.0) sec = 0.0;
|
||||
const int64_t s = static_cast<int64_t>(std::floor(sec));
|
||||
const double rem = sec - static_cast<double>(s);
|
||||
const uint32_t ns = static_cast<uint32_t>(std::round(rem * 1e9));
|
||||
out->sec = static_cast<int32_t>(s);
|
||||
out->nanosec = ns;
|
||||
}
|
||||
|
||||
} // namespace robot_control
|
||||
32
src/robot_control/include/utils/future_utils.hpp
Normal file
32
src/robot_control/include/utils/future_utils.hpp
Normal file
@@ -0,0 +1,32 @@
|
||||
/**
|
||||
* @file future_utils.hpp
|
||||
* @brief std::future / std::shared_future 轮询就绪的通用工具，供 action 等复用
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <future>
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief 非阻塞检查 future 是否已就绪（wait_for 1ms）
|
||||
*/
|
||||
template <typename T>
|
||||
inline bool is_future_ready(std::future<T>& f)
|
||||
{
|
||||
return f.wait_for(std::chrono::milliseconds(1)) == std::future_status::ready;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 非阻塞检查 shared_future 是否已就绪（wait_for 1ms）
|
||||
*/
|
||||
template <typename T>
|
||||
inline bool is_future_ready(const std::shared_future<T>& f)
|
||||
{
|
||||
return f.wait_for(std::chrono::milliseconds(1)) == std::future_status::ready;
|
||||
}
|
||||
|
||||
} // namespace robot_control
|
||||
216
src/robot_control/include/utils/s_curve_trajectory.hpp
Normal file
216
src/robot_control/include/utils/s_curve_trajectory.hpp
Normal file
@@ -0,0 +1,216 @@
|
||||
/**
|
||||
* @file s_curve_trajectory.hpp
|
||||
* @brief S 曲线轨迹生成器（支持多轴同步）
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief 多轴S型曲线轨迹规划器（支持各轴独立速度/加速度/Jerk约束）
|
||||
* 保证所有轴同时启动、同时结束，各阶段时间完全同步
|
||||
* S型曲线包含7个阶段：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速
|
||||
*/
|
||||
class S_CurveTrajectory
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数（多轴版本）
|
||||
* @param max_velocities 各轴最大速度约束（向量，长度=轴数）
|
||||
* @param max_accelerations 各轴最大加速度约束（向量，长度=轴数）
|
||||
* @param max_jerks 各轴最大Jerk约束（向量，长度=轴数），默认值为max_acceleration*10
|
||||
*/
|
||||
S_CurveTrajectory(const std::vector<double>& max_velocities,
|
||||
const std::vector<double>& max_accelerations,
|
||||
const std::vector<double>& max_jerks = std::vector<double>());
|
||||
|
||||
/**
|
||||
* @brief 构造函数（单轴版本）
|
||||
* @param max_velocity 最大速度约束
|
||||
* @param max_acceleration 最大加速度约束
|
||||
* @param max_jerk 最大Jerk约束，默认值为max_acceleration*10
|
||||
*/
|
||||
S_CurveTrajectory(double max_velocity = 1.0,
|
||||
double max_acceleration = 0.5,
|
||||
double max_jerk = 5.0);
|
||||
|
||||
/**
|
||||
* @brief 析构函数
|
||||
*/
|
||||
~S_CurveTrajectory() = default;
|
||||
|
||||
/**
|
||||
* @brief 初始化多轴轨迹规划
|
||||
* @param start_pos 起始位置（向量，长度=轴数）
|
||||
* @param target_pos 目标位置（向量，长度=轴数）
|
||||
*/
|
||||
void Init(const std::vector<double>& start_pos, const std::vector<double>& target_pos);
|
||||
|
||||
/**
|
||||
* @brief 初始化单轴轨迹规划
|
||||
* @param start_pos 起始位置
|
||||
* @param target_pos 目标位置
|
||||
*/
|
||||
void Init(double start_pos, double target_pos);
|
||||
|
||||
/**
|
||||
* @brief 更新多轴轨迹，计算当前时间的位置
|
||||
* @param time 已运动时间（s）
|
||||
* @return 当前位置向量
|
||||
*/
|
||||
std::vector<double> Update(double time);
|
||||
|
||||
/**
|
||||
* @brief 更新单轴轨迹，计算当前时间的位置
|
||||
* @param time 已运动时间（s）
|
||||
* @return 当前位置
|
||||
*/
|
||||
double UpdateSingle(double time);
|
||||
|
||||
/**
|
||||
* @brief 获取当前速度（多轴）
|
||||
*/
|
||||
std::vector<double> GetVelocity() const { return current_velocity_; }
|
||||
|
||||
/**
|
||||
* @brief 获取当前速度（单轴）
|
||||
*/
|
||||
double GetSingleVelocity() const { return current_velocity_[0]; }
|
||||
|
||||
/**
|
||||
* @brief 获取当前加速度（多轴）
|
||||
*/
|
||||
std::vector<double> GetAcceleration() const { return current_acceleration_; }
|
||||
|
||||
/**
|
||||
* @brief 获取当前加速度（单轴）
|
||||
*/
|
||||
double GetSingleAcceleration() const { return current_acceleration_[0]; }
|
||||
|
||||
/**
|
||||
* @brief 检查轨迹是否已完成
|
||||
* @param time 已运动时间（s）
|
||||
* @return 完成返回true
|
||||
*/
|
||||
bool IsFinished(double time) const { return time >= total_time_; }
|
||||
|
||||
/**
|
||||
* @brief 设置各轴最大速度
|
||||
* @param max_velocities 新的最大速度向量（长度=轴数）
|
||||
*/
|
||||
void SetMaxVelocities(const std::vector<double>& max_velocities);
|
||||
|
||||
/**
|
||||
* @brief 设置单轴最大速度
|
||||
* @param max_velocity 新的最大速度
|
||||
*/
|
||||
void SetMaxVelocity(double max_velocity);
|
||||
|
||||
/**
|
||||
* @brief 设置各轴最大加速度
|
||||
* @param max_accelerations 新的最大加速度向量（长度=轴数）
|
||||
*/
|
||||
void SetMaxAccelerations(const std::vector<double>& max_accelerations);
|
||||
|
||||
/**
|
||||
* @brief 设置单轴最大加速度
|
||||
* @param max_acceleration 新的最大加速度
|
||||
*/
|
||||
void SetMaxAcceleration(double max_acceleration);
|
||||
|
||||
/**
|
||||
* @brief 计算急停轨迹参数（当前状态下减速到停止）
|
||||
*/
|
||||
void CalculateStopTrajectory();
|
||||
|
||||
/**
|
||||
* @brief 更新急停轨迹
|
||||
* @param dt 从急停开始的时间（s）
|
||||
* @return 当前位置
|
||||
*/
|
||||
std::vector<double> UpdateStop(double dt);
|
||||
|
||||
/**
|
||||
* @brief 检查急停是否完成
|
||||
* @param dt 从急停开始的时间（s）
|
||||
* @return 完成返回true
|
||||
*/
|
||||
bool IsStopFinished(double dt) const { return dt >= stop_total_time_; }
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief 计算轨迹核心参数（统一时间+各轴参数）
|
||||
*/
|
||||
void calculateTrajectoryParams();
|
||||
|
||||
/**
|
||||
* @brief 计算单轴的S型曲线阶段时间
|
||||
* @param dist 位移距离（绝对值）
|
||||
* @param max_v 最大速度
|
||||
* @param max_a 最大加速度
|
||||
* @param max_j 最大Jerk
|
||||
* @param[out] T1 加加速阶段时间
|
||||
* @param[out] T2 匀加速阶段时间
|
||||
* @param[out] T3 减加速阶段时间
|
||||
* @param[out] T4 匀速阶段时间
|
||||
* @param[out] T5 加减速阶段时间
|
||||
* @param[out] T6 匀减速阶段时间
|
||||
* @param[out] T7 减减速阶段时间
|
||||
* @param[out] v_max 实际达到的最大速度
|
||||
*/
|
||||
void calculateScurveTimes(double dist, double max_v, double max_a, double max_j,
|
||||
double& T1, double& T2, double& T3, double& T4,
|
||||
double& T5, double& T6, double& T7, double& v_max);
|
||||
|
||||
std::vector<double> start_pos_; // 起始位置（各轴）
|
||||
std::vector<double> target_pos_; // 目标位置（各轴）
|
||||
std::vector<double> total_distance_; // 总位移绝对值（各轴）
|
||||
std::vector<double> current_pos_; // 当前位置（各轴）
|
||||
std::vector<double> current_velocity_; // 当前速度（各轴）
|
||||
std::vector<double> current_acceleration_; // 当前加速度（各轴）
|
||||
|
||||
// 轨迹参数（各轴独立）
|
||||
std::vector<double> max_velocity_; // 最大速度（各轴独立）
|
||||
std::vector<double> max_acceleration_; // 最大加速度（各轴独立）
|
||||
std::vector<double> max_jerk_; // 最大Jerk（各轴独立）
|
||||
|
||||
// S曲线阶段时间（各轴独立，但会统一到最大值）
|
||||
std::vector<double> T1_; // 加加速阶段时间
|
||||
std::vector<double> T2_; // 匀加速阶段时间
|
||||
std::vector<double> T3_; // 减加速阶段时间
|
||||
std::vector<double> T4_; // 匀速阶段时间
|
||||
std::vector<double> T5_; // 加减速阶段时间
|
||||
std::vector<double> T6_; // 匀减速阶段时间
|
||||
std::vector<double> T7_; // 减减速阶段时间
|
||||
std::vector<double> v_max_; // 实际达到的最大速度（各轴）
|
||||
|
||||
// 统一时间参数（所有轴共用，取各轴最大值）
|
||||
double total_time_; // 总运动时间
|
||||
double T1_total_; // 加加速阶段时间
|
||||
double T2_total_; // 匀加速阶段时间
|
||||
double T3_total_; // 减加速阶段时间
|
||||
double T4_total_; // 匀速阶段时间
|
||||
double T5_total_; // 加减速阶段时间
|
||||
double T6_total_; // 匀减速阶段时间
|
||||
double T7_total_; // 减减速阶段时间
|
||||
|
||||
// 急停相关参数
|
||||
bool is_stopping_; // 是否处于急停状态
|
||||
double stop_total_time_; // 急停总时间（统一）
|
||||
std::vector<double> stop_start_pos_; // 急停起始位置
|
||||
std::vector<double> stop_start_vel_; // 急停起始速度
|
||||
std::vector<double> stop_start_acc_; // 急停起始加速度
|
||||
std::vector<double> stop_jerk_; // 急停Jerk（各轴独立，已不使用）
|
||||
std::vector<double> stop_decel_; // 急停减速度（各轴独立，已调整为统一时间）
|
||||
|
||||
bool is_single_joint_; // 是否为单轴模式
|
||||
};
|
||||
|
||||
} // namespace robot_control
|
||||
|
||||
48
src/robot_control/include/utils/trajectory_blend.hpp
Normal file
48
src/robot_control/include/utils/trajectory_blend.hpp
Normal file
@@ -0,0 +1,48 @@
|
||||
/**
|
||||
* @file trajectory_blend.hpp
|
||||
* @brief 两段关节轨迹之间的 blending 工具（关节空间平滑过渡）
|
||||
* @details 用于 MoveIt 规划 + FTJ 下发场景：将两段轨迹在连接处做平滑混合后合并为一条轨迹，
|
||||
* 再通过 FollowJointTrajectory 一次性下发，避免段与段之间停顿或速度突变。
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "trajectory_msgs/msg/joint_trajectory.hpp"
|
||||
#include <algorithm>
|
||||
#include <cstdint>
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief 将两段 JointTrajectory 在连接处做关节空间 blending 后合并为一条轨迹
|
||||
*
|
||||
* 要求：traj1 与 traj2 的 joint_names 一致；traj1 的末点与 traj2 的首点应接近（否则在 blend 区会有较大弯曲）。
|
||||
* blend_ratio 表示参与混合的“比例”：取 traj1 末尾 blend_ratio 比例的点与 traj2 开头 blend_ratio 比例的点
|
||||
* 做平滑过渡（alpha 使用 3u^2-2u^3 保证速度连续）。合并后的时间轴连续。
|
||||
*
|
||||
* @param traj1 第一段轨迹（先执行）
|
||||
* @param traj2 第二段轨迹（后执行）
|
||||
* @param blend_ratio 混合比例 (0, 0.5]，例如 0.2 表示两端各 20% 参与混合；0 表示不混合仅拼接
|
||||
* @param blend_point_count blend 区内插点数（建议 5~20）
|
||||
* @param out 输出：合并后的轨迹（含连续 time_from_start）
|
||||
* @return true 成功，false 参数无效或 joint_names 不一致
|
||||
*/
|
||||
bool BlendTwoTrajectories(
|
||||
const trajectory_msgs::msg::JointTrajectory& traj1,
|
||||
const trajectory_msgs::msg::JointTrajectory& traj2,
|
||||
double blend_ratio,
|
||||
size_t blend_point_count,
|
||||
trajectory_msgs::msg::JointTrajectory* out);
|
||||
|
||||
/**
|
||||
* @brief 将 blend_radius 百分比 [0, 100] 转换为内部使用的 blend_ratio (0, 0.5]
|
||||
* @param blend_radius 接口层百分比，0 表示不混合
|
||||
* @return blend_ratio，0 表示不混合
|
||||
*/
|
||||
inline double BlendRadiusToRatio(int32_t blend_radius)
|
||||
{
|
||||
if (blend_radius <= 0 || blend_radius > 100) return 0.0;
|
||||
return std::min(0.5, 0.01 * static_cast<double>(blend_radius));
|
||||
}
|
||||
} // namespace robot_control
|
||||
369
src/robot_control/include/utils/trajectory_utils.hpp
Normal file
369
src/robot_control/include/utils/trajectory_utils.hpp
Normal file
@@ -0,0 +1,369 @@
|
||||
/**
|
||||
* @file trajectory_utils.hpp
|
||||
* @brief 轨迹通用工具（JointTrajectory 归一化等），供 dual_arm_action、arm_control 等模块复用
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "utils/duration_utils.hpp"
|
||||
#include "trajectory_msgs/msg/joint_trajectory.hpp"
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief 确保轨迹首点不在 t=0，避免 FJT 下发时起步抖动
|
||||
*
|
||||
* 若首点 time_from_start ≈ 0 且有多点，则删除首点；再将整条轨迹整体后移，
|
||||
* 使首点时间 ≥ min_start_sec（默认 0.05s），给控制器留出缓冲。
|
||||
*
|
||||
* @param traj 待处理轨迹（原地修改）
|
||||
* @param min_start_sec 首点最小时间（秒），建议 0.05
|
||||
*/
|
||||
inline void ensure_nonzero_start_time(
|
||||
trajectory_msgs::msg::JointTrajectory* traj,
|
||||
double min_start_sec)
|
||||
{
|
||||
if (!traj || traj->points.empty()) return;
|
||||
min_start_sec = std::max(0.01, min_start_sec);
|
||||
const double t0 = duration_to_sec(traj->points.front().time_from_start);
|
||||
if (t0 <= 1e-9 && traj->points.size() > 1) traj->points.erase(traj->points.begin());
|
||||
if (traj->points.empty()) return;
|
||||
const double t_first = duration_to_sec(traj->points.front().time_from_start);
|
||||
if (t_first >= min_start_sec) return;
|
||||
const double shift = min_start_sec - t_first;
|
||||
for (auto& p : traj->points) {
|
||||
double t = duration_to_sec(p.time_from_start) + shift;
|
||||
sec_to_duration(t, &p.time_from_start);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 构造一条“保持指定关节位置”的单点轨迹，用于软停（如取消时先发再 cancel FJT）
|
||||
*
|
||||
* 单点：positions 为给定关节角，速度/加速度为 0，time_from_start = max(0.05, hold_time_sec)。
|
||||
*
|
||||
* @param joint_names 关节名列表
|
||||
* @param positions 保持的关节位置（弧度），与 joint_names 一一对应
|
||||
* @param hold_time_sec 该点的 time_from_start（秒），至少 0.05
|
||||
*/
|
||||
inline trajectory_msgs::msg::JointTrajectory make_hold_trajectory(
|
||||
const std::vector<std::string>& joint_names,
|
||||
const std::vector<double>& positions,
|
||||
double hold_time_sec)
|
||||
{
|
||||
trajectory_msgs::msg::JointTrajectory traj;
|
||||
traj.joint_names = joint_names;
|
||||
trajectory_msgs::msg::JointTrajectoryPoint p;
|
||||
p.positions = positions;
|
||||
p.velocities.assign(joint_names.size(), 0.0);
|
||||
p.accelerations.assign(joint_names.size(), 0.0);
|
||||
sec_to_duration(std::max(0.05, hold_time_sec), &p.time_from_start);
|
||||
traj.points.push_back(std::move(p));
|
||||
return traj;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 在轨减速停止：从当前 (position, velocity) 按恒定减速度减速到零，生成一段轨迹
|
||||
*
|
||||
* 各关节使用 a_i = -v_i/T，在 T 时刻同时速度为零；T 由“最慢能停下的关节”决定且不超过
|
||||
* max_acceleration，并限制在 [min_decel_sec, max_decel_sec]。若当前速度全为 0 则退化为单点保持。
|
||||
*
|
||||
* @param joint_names 关节名列表
|
||||
* @param positions 当前关节位置（弧度），与 joint_names 一一对应
|
||||
* @param velocities 当前关节速度（rad/s），与 joint_names 一一对应
|
||||
* @param max_accelerations 各关节最大加速度（rad/s²），用于限制减速度，与 joint_names 一一对应
|
||||
* @param min_decel_sec 最小减速时间（秒）
|
||||
* @param max_decel_sec 最大减速时间（秒）
|
||||
* @param num_points 轨迹采样点数（含首尾）
|
||||
* @return 减速轨迹；若参数无效或长度不一致则返回空轨迹（points 为空）
|
||||
*/
|
||||
inline trajectory_msgs::msg::JointTrajectory make_deceleration_trajectory(
|
||||
const std::vector<std::string>& joint_names,
|
||||
const std::vector<double>& positions,
|
||||
const std::vector<double>& velocities,
|
||||
const std::vector<double>& max_accelerations,
|
||||
double min_decel_sec = 0.05,
|
||||
double max_decel_sec = 0.5,
|
||||
size_t num_points = 16)
|
||||
{
|
||||
trajectory_msgs::msg::JointTrajectory traj;
|
||||
traj.joint_names = joint_names;
|
||||
const size_t n = joint_names.size();
|
||||
if (n == 0 || positions.size() != n || velocities.size() != n || max_accelerations.size() != n) {
|
||||
return traj;
|
||||
}
|
||||
min_decel_sec = std::max(0.01, min_decel_sec);
|
||||
max_decel_sec = std::max(min_decel_sec, max_decel_sec);
|
||||
num_points = std::max(size_t(2), std::min(size_t(64), num_points));
|
||||
|
||||
// T >= |v_i|/max_acc_i 且 T 在 [min_decel_sec, max_decel_sec]
|
||||
double T = min_decel_sec;
|
||||
for (size_t i = 0; i < n; ++i) {
|
||||
const double amax = std::max(1e-6, std::abs(max_accelerations[i]));
|
||||
const double vi = velocities[i];
|
||||
if (std::abs(vi) > 1e-12) {
|
||||
double ti = std::abs(vi) / amax;
|
||||
if (ti > T) T = ti;
|
||||
}
|
||||
}
|
||||
if (T > max_decel_sec) T = max_decel_sec;
|
||||
if (T < min_decel_sec) T = min_decel_sec;
|
||||
|
||||
const double eps = 1e-12;
|
||||
bool all_zero = true;
|
||||
for (size_t i = 0; i < n; ++i) {
|
||||
if (std::abs(velocities[i]) > eps) { all_zero = false; break; }
|
||||
}
|
||||
if (all_zero || T < 1e-9) {
|
||||
return make_hold_trajectory(joint_names, positions, std::max(0.05, T));
|
||||
}
|
||||
|
||||
traj.points.reserve(num_points);
|
||||
for (size_t k = 0; k < num_points; ++k) {
|
||||
const double t = (num_points == 1) ? T : (T * static_cast<double>(k) / static_cast<double>(num_points - 1));
|
||||
trajectory_msgs::msg::JointTrajectoryPoint pt;
|
||||
pt.positions.resize(n);
|
||||
pt.velocities.resize(n);
|
||||
pt.accelerations.resize(n);
|
||||
for (size_t i = 0; i < n; ++i) {
|
||||
const double p0 = positions[i];
|
||||
const double v0 = velocities[i];
|
||||
const double a = -v0 / T;
|
||||
pt.positions[i] = p0 + v0 * t + 0.5 * a * t * t;
|
||||
pt.velocities[i] = v0 + a * t;
|
||||
pt.accelerations[i] = a;
|
||||
}
|
||||
sec_to_duration(t, &pt.time_from_start);
|
||||
traj.points.push_back(std::move(pt));
|
||||
}
|
||||
return traj;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 将值限制到 [0, 1]
|
||||
*/
|
||||
inline double clamp01(double v)
|
||||
{
|
||||
if (v < 0.0) return 0.0;
|
||||
if (v > 1.0) return 1.0;
|
||||
return v;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Quintic smoothstep 进度映射（S型）
|
||||
*
|
||||
* 输入/输出均为 [0,1]，且在 0/1 两端的一阶、二阶导均为 0，
|
||||
* 适合做起停平滑的时间重标定。
|
||||
*/
|
||||
inline double s_curve_progress(double u)
|
||||
{
|
||||
u = clamp01(u);
|
||||
const double u2 = u * u;
|
||||
const double u3 = u2 * u;
|
||||
const double u4 = u3 * u;
|
||||
const double u5 = u4 * u;
|
||||
return 6.0 * u5 - 15.0 * u4 + 10.0 * u3;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 在指定时刻对轨迹做关节位置线性插值
|
||||
*
|
||||
* - t 早于首点时，返回首点位置
|
||||
* - t 晚于尾点时，返回尾点位置
|
||||
*/
|
||||
inline bool interpolate_positions_at_time(
|
||||
const trajectory_msgs::msg::JointTrajectory& traj,
|
||||
double t_sec,
|
||||
std::vector<double>* out_positions)
|
||||
{
|
||||
if (!out_positions || traj.points.empty()) return false;
|
||||
const size_t dof = traj.joint_names.size();
|
||||
if (dof == 0) return false;
|
||||
out_positions->assign(dof, 0.0);
|
||||
|
||||
const auto time_of = [](const trajectory_msgs::msg::JointTrajectoryPoint& p) {
|
||||
return static_cast<double>(p.time_from_start.sec) +
|
||||
static_cast<double>(p.time_from_start.nanosec) * 1e-9;
|
||||
};
|
||||
|
||||
const double t0 = time_of(traj.points.front());
|
||||
const double tN = time_of(traj.points.back());
|
||||
if (traj.points.front().positions.size() < dof ||
|
||||
traj.points.back().positions.size() < dof) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (t_sec <= t0 + 1e-9) {
|
||||
*out_positions = traj.points.front().positions;
|
||||
return true;
|
||||
}
|
||||
if (t_sec >= tN - 1e-9) {
|
||||
*out_positions = traj.points.back().positions;
|
||||
return true;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i + 1 < traj.points.size(); ++i) {
|
||||
const double ta = time_of(traj.points[i]);
|
||||
const double tb = time_of(traj.points[i + 1]);
|
||||
if (t_sec <= tb + 1e-9) {
|
||||
if (traj.points[i].positions.size() < dof ||
|
||||
traj.points[i + 1].positions.size() < dof) {
|
||||
return false;
|
||||
}
|
||||
const double den = std::max(1e-9, tb - ta);
|
||||
const double a = clamp01((t_sec - ta) / den);
|
||||
for (size_t j = 0; j < dof; ++j) {
|
||||
const double q0 = traj.points[i].positions[j];
|
||||
const double q1 = traj.points[i + 1].positions[j];
|
||||
(*out_positions)[j] = q0 + a * (q1 - q0);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
*out_positions = traj.points.back().positions;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 基于位置点列回填速度与加速度（数值差分）
|
||||
*/
|
||||
inline void fill_velocity_acceleration(
|
||||
trajectory_msgs::msg::JointTrajectory* traj)
|
||||
{
|
||||
if (!traj || traj->points.empty()) return;
|
||||
const size_t n = traj->points.size();
|
||||
const size_t dof = traj->joint_names.size();
|
||||
if (dof == 0) return;
|
||||
|
||||
const auto time_of = [](const trajectory_msgs::msg::JointTrajectoryPoint& p) {
|
||||
return static_cast<double>(p.time_from_start.sec) +
|
||||
static_cast<double>(p.time_from_start.nanosec) * 1e-9;
|
||||
};
|
||||
|
||||
for (auto& p : traj->points) {
|
||||
if (p.positions.size() < dof) return;
|
||||
p.velocities.assign(dof, 0.0);
|
||||
p.accelerations.assign(dof, 0.0);
|
||||
}
|
||||
if (n < 2) return;
|
||||
|
||||
for (size_t i = 0; i < n; ++i) {
|
||||
const size_t i0 = (i == 0) ? 0 : (i - 1);
|
||||
const size_t i1 = (i + 1 >= n) ? (n - 1) : (i + 1);
|
||||
const double t0 = time_of(traj->points[i0]);
|
||||
const double t1 = time_of(traj->points[i1]);
|
||||
const double dt = std::max(1e-6, t1 - t0);
|
||||
for (size_t j = 0; j < dof; ++j) {
|
||||
const double q0 = traj->points[i0].positions[j];
|
||||
const double q1 = traj->points[i1].positions[j];
|
||||
traj->points[i].velocities[j] = (q1 - q0) / dt;
|
||||
}
|
||||
}
|
||||
for (size_t i = 0; i < n; ++i) {
|
||||
const size_t i0 = (i == 0) ? 0 : (i - 1);
|
||||
const size_t i1 = (i + 1 >= n) ? (n - 1) : (i + 1);
|
||||
const double t0 = time_of(traj->points[i0]);
|
||||
const double t1 = time_of(traj->points[i1]);
|
||||
const double dt = std::max(1e-6, t1 - t0);
|
||||
for (size_t j = 0; j < dof; ++j) {
|
||||
const double v0 = traj->points[i0].velocities[j];
|
||||
const double v1 = traj->points[i1].velocities[j];
|
||||
traj->points[i].accelerations[j] = (v1 - v0) / dt;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 双臂轨迹统一时间轴重采样（可选S型进度）
|
||||
*
|
||||
* 目标是让左右臂输出轨迹拥有相同的时间戳序列，实现同步执行。
|
||||
* @param use_s_curve_progress true 时对进度使用 S 型映射，降低起停冲击
|
||||
*/
|
||||
inline bool synchronize_dual_arm_trajectory_timebase(
|
||||
const trajectory_msgs::msg::JointTrajectory& left_in,
|
||||
const trajectory_msgs::msg::JointTrajectory& right_in,
|
||||
trajectory_msgs::msg::JointTrajectory* left_out,
|
||||
trajectory_msgs::msg::JointTrajectory* right_out,
|
||||
double* out_total_time,
|
||||
std::string* error_msg = nullptr,
|
||||
bool use_s_curve_progress = true,
|
||||
double min_dt = 0.01,
|
||||
size_t max_samples = 500)
|
||||
{
|
||||
if (!left_out || !right_out || !out_total_time) {
|
||||
if (error_msg) *error_msg = "Null output pointer";
|
||||
return false;
|
||||
}
|
||||
if (left_in.points.empty() || right_in.points.empty()) {
|
||||
if (error_msg) *error_msg = "Empty left/right trajectory";
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto total_time = [](const trajectory_msgs::msg::JointTrajectory& t) {
|
||||
const auto& p = t.points.back();
|
||||
return static_cast<double>(p.time_from_start.sec) +
|
||||
static_cast<double>(p.time_from_start.nanosec) * 1e-9;
|
||||
};
|
||||
const double left_T = total_time(left_in);
|
||||
const double right_T = total_time(right_in);
|
||||
const double T = std::max(left_T, right_T);
|
||||
*out_total_time = T;
|
||||
|
||||
left_out->header = left_in.header;
|
||||
right_out->header = right_in.header;
|
||||
left_out->joint_names = left_in.joint_names;
|
||||
right_out->joint_names = right_in.joint_names;
|
||||
left_out->points.clear();
|
||||
right_out->points.clear();
|
||||
|
||||
if (T <= 1e-6) {
|
||||
left_out->points.push_back(left_in.points.back());
|
||||
right_out->points.push_back(right_in.points.back());
|
||||
fill_velocity_acceleration(left_out);
|
||||
fill_velocity_acceleration(right_out);
|
||||
return true;
|
||||
}
|
||||
|
||||
min_dt = std::max(0.001, min_dt);
|
||||
max_samples = std::max<size_t>(3, max_samples);
|
||||
|
||||
size_t samples = std::max(left_in.points.size(), right_in.points.size());
|
||||
samples = std::max<size_t>(samples, 3);
|
||||
const size_t min_samples_by_dt = static_cast<size_t>(std::ceil(T / min_dt)) + 1;
|
||||
if (samples < min_samples_by_dt) samples = min_samples_by_dt;
|
||||
if (samples > max_samples) samples = max_samples;
|
||||
|
||||
left_out->points.reserve(samples);
|
||||
right_out->points.reserve(samples);
|
||||
for (size_t k = 0; k < samples; ++k) {
|
||||
const double u = (samples == 1) ? 1.0 : (static_cast<double>(k) / static_cast<double>(samples - 1));
|
||||
const double progress = use_s_curve_progress ? s_curve_progress(u) : u;
|
||||
const double sample_t = progress * T;
|
||||
const double output_t = u * T;
|
||||
|
||||
trajectory_msgs::msg::JointTrajectoryPoint lp;
|
||||
trajectory_msgs::msg::JointTrajectoryPoint rp;
|
||||
if (!interpolate_positions_at_time(left_in, sample_t, &lp.positions) ||
|
||||
!interpolate_positions_at_time(right_in, sample_t, &rp.positions)) {
|
||||
if (error_msg) *error_msg = "Trajectory interpolation failed";
|
||||
return false;
|
||||
}
|
||||
|
||||
sec_to_duration(output_t, &lp.time_from_start);
|
||||
sec_to_duration(output_t, &rp.time_from_start);
|
||||
left_out->points.push_back(std::move(lp));
|
||||
right_out->points.push_back(std::move(rp));
|
||||
}
|
||||
|
||||
fill_velocity_acceleration(left_out);
|
||||
fill_velocity_acceleration(right_out);
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace robot_control
|
||||
@@ -1,8 +1,14 @@
|
||||
/**
|
||||
* @file trapezoidal_trajectory.hpp
|
||||
* @brief 梯形速度轨迹生成器（支持多轴同步）
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include <vector>
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
|
||||
@@ -39,7 +45,7 @@ public:
|
||||
* @param target_pos 目标位置（向量，长度=轴数）
|
||||
* @param duration 期望运动时间（若>0且>最小时间则强制使用该时间）
|
||||
*/
|
||||
void init(const std::vector<double>& start_pos, const std::vector<double>& target_pos);
|
||||
void Init(const std::vector<double>& start_pos, const std::vector<double>& target_pos);
|
||||
|
||||
/**
|
||||
* @brief 初始化单轴轨迹规划
|
||||
@@ -47,91 +53,91 @@ public:
|
||||
* @param target_pos 目标位置
|
||||
* @param duration 期望运动时间（若>0且>最小时间则强制使用该时间）
|
||||
*/
|
||||
void init(double start_pos, double target_pos);
|
||||
void Init(double start_pos, double target_pos);
|
||||
|
||||
/**
|
||||
* @brief 更新多轴轨迹，计算当前时间的位置
|
||||
* @param time 已运动时间（s）
|
||||
* @return 当前位置向量
|
||||
*/
|
||||
std::vector<double> update(double time);
|
||||
std::vector<double> Update(double time);
|
||||
|
||||
/**
|
||||
* @brief 更新单轴轨迹，计算当前时间的位置
|
||||
* @param time 已运动时间（s）
|
||||
* @return 当前位置
|
||||
*/
|
||||
double updateSingle(double time);
|
||||
double UpdateSingle(double time);
|
||||
|
||||
/**
|
||||
* @brief 获取当前速度（多轴）
|
||||
*/
|
||||
std::vector<double> getVelocity() const { return current_velocity_; }
|
||||
std::vector<double> GetVelocity() const { return current_velocity_; }
|
||||
|
||||
/**
|
||||
* @brief 获取当前速度（单轴）
|
||||
*/
|
||||
double getSingleVelocity() const { return current_velocity_[0]; }
|
||||
double GetSingleVelocity() const { return current_velocity_[0]; }
|
||||
|
||||
/**
|
||||
* @brief 获取当前加速度（多轴）
|
||||
*/
|
||||
std::vector<double> getAcceleration() const { return current_acceleration_; }
|
||||
std::vector<double> GetAcceleration() const { return current_acceleration_; }
|
||||
|
||||
/**
|
||||
* @brief 获取当前加速度（单轴）
|
||||
*/
|
||||
double getSingleAcceleration() const { return current_acceleration_[0]; }
|
||||
double GetSingleAcceleration() const { return current_acceleration_[0]; }
|
||||
|
||||
/**
|
||||
* @brief 检查轨迹是否已完成
|
||||
* @param time 已运动时间（s）
|
||||
* @return 完成返回true
|
||||
*/
|
||||
bool isFinished(double time) const { return time >= total_time_; }
|
||||
bool IsFinished(double time) const { return time >= total_time_; }
|
||||
|
||||
/**
|
||||
* @brief 设置各轴最大速度
|
||||
* @param max_velocities 新的最大速度向量（长度=轴数）
|
||||
*/
|
||||
void setMaxVelocities(const std::vector<double>& max_velocities);
|
||||
void SetMaxVelocities(const std::vector<double>& max_velocities);
|
||||
|
||||
/**
|
||||
* @brief 设置单轴最大速度
|
||||
* @param max_velocity 新的最大速度
|
||||
*/
|
||||
void setMaxVelocity(double max_velocity);
|
||||
void SetMaxVelocity(double max_velocity);
|
||||
|
||||
/**
|
||||
* @brief 设置各轴最大加速度
|
||||
* @param max_accelerations 新的最大加速度向量（长度=轴数）
|
||||
*/
|
||||
void setMaxAccelerations(const std::vector<double>& max_accelerations);
|
||||
void SetMaxAccelerations(const std::vector<double>& max_accelerations);
|
||||
|
||||
/**
|
||||
* @brief 设置单轴最大加速度
|
||||
* @param max_acceleration 新的最大加速度
|
||||
*/
|
||||
void setMaxAcceleration(double max_acceleration);
|
||||
void SetMaxAcceleration(double max_acceleration);
|
||||
|
||||
/**
|
||||
* @brief 计算急停轨迹参数（当前状态下减速到停止）
|
||||
*/
|
||||
void calculateStopTrajectory();
|
||||
void CalculateStopTrajectory();
|
||||
|
||||
/**
|
||||
* @brief 更新急停轨迹
|
||||
* @param dt 从急停开始的时间（s）
|
||||
* @return 当前位置
|
||||
*/
|
||||
std::vector<double> updateStop(double dt);
|
||||
std::vector<double> UpdateStop(double dt);
|
||||
|
||||
/**
|
||||
* @brief 检查急停是否完成
|
||||
* @param dt 从急停开始的时间（s）
|
||||
* @return 完成返回true
|
||||
*/
|
||||
bool isStopFinished(double dt) const { return dt >= stop_total_time_; }
|
||||
bool IsStopFinished(double dt) const { return dt >= stop_total_time_; }
|
||||
|
||||
private:
|
||||
/**
|
||||
36
src/robot_control/include/utils/velocity_scale_utils.hpp
Normal file
36
src/robot_control/include/utils/velocity_scale_utils.hpp
Normal file
@@ -0,0 +1,36 @@
|
||||
/**
|
||||
* @file velocity_scale_utils.hpp
|
||||
* @brief 速度缩放参数校验与转换（如 goal 中的 1–100% 转为 0.01–1.0）
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief 将 goal 中的 velocity_scaling（1–100%）转为内部使用的缩放因子 (0.01, 1.0]
|
||||
* @param goal_value 客户端下发的速度缩放（百分比 1–100）
|
||||
* @param out_scale 输出缩放因子
|
||||
* @param error_msg 失败时的错误信息（可传 nullptr）
|
||||
* @return true 成功，false 参数无效
|
||||
*/
|
||||
inline bool goal_velocity_to_scale(double goal_value, double* out_scale, std::string* error_msg)
|
||||
{
|
||||
if (!out_scale) {
|
||||
if (error_msg) *error_msg = "Null out_scale";
|
||||
return false;
|
||||
}
|
||||
if (!std::isfinite(goal_value) || goal_value < 1.0 || goal_value > 100.0) {
|
||||
if (error_msg) *error_msg = "velocity_scaling must be in [1, 100]%: " + std::to_string(goal_value);
|
||||
return false;
|
||||
}
|
||||
*out_scale = std::clamp(goal_value / 100.0, 0.01, 1.0);
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace robot_control
|
||||
@@ -1,34 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <Eigen/Dense>
|
||||
#include "control_base.hpp" // 包含父类头文件
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
class WaistControl : public ControlBase
|
||||
{
|
||||
public:
|
||||
WaistControl(
|
||||
size_t total_joints,
|
||||
const std::vector<double>& lengthParameters,
|
||||
const std::vector<double>& maxSpeed,
|
||||
const std::vector<double>& maxAcc,
|
||||
const std::vector<double>& minLimits,
|
||||
const std::vector<double>& maxLimits,
|
||||
const std::vector<double>& home_positions,
|
||||
const std::vector<double>& zero_positions,
|
||||
const std::vector<int>& joint_directions
|
||||
);
|
||||
|
||||
~WaistControl() override;
|
||||
|
||||
// void SetHomePositions(const std::vector<double>& home_positions) override;
|
||||
|
||||
bool SetMoveWaistParametersInternal(double movepitch, double moveyaw);
|
||||
|
||||
bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
|
||||
|
||||
bool MoveWaist(std::vector<double>& joint_positions, double dt);
|
||||
};
|
||||
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user