initial
This commit is contained in:
5
.gitignore
vendored
5
.gitignore
vendored
@ -31,11 +31,6 @@ AMENT_IGNORE
|
||||
|
||||
# ---> VisualStudioCode
|
||||
.vscode/*
|
||||
!.vscode/settings.json
|
||||
!.vscode/tasks.json
|
||||
!.vscode/launch.json
|
||||
!.vscode/extensions.json
|
||||
!.vscode/*.code-snippets
|
||||
|
||||
# Local History for Visual Studio Code
|
||||
.history/
|
||||
|
||||
18
LICENSE
18
LICENSE
@ -1,18 +0,0 @@
|
||||
MIT License
|
||||
|
||||
Copyright (c) 2026 YikaiFu-cart
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and
|
||||
associated documentation files (the "Software"), to deal in the Software without restriction, including
|
||||
without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the
|
||||
following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all copies or substantial
|
||||
portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT
|
||||
LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO
|
||||
EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
|
||||
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
|
||||
USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
155
README.md
Normal file → Executable file
155
README.md
Normal file → Executable file
@ -1,2 +1,155 @@
|
||||
# acRealman_xr
|
||||
# XR-RM75 双臂阶段一遥操作 ROS2 工作空间
|
||||
|
||||
这个 `src/` 目录面向 **Ubuntu 22.04 + ROS2 Humble**，用于先跑通阶段一闭环：
|
||||
|
||||
```text
|
||||
PICO 4 Ultra 双手柄 UDP 位姿/按键
|
||||
-> /xr/left_controller 与 /xr/right_controller
|
||||
-> 左右 RM75 相对位移速度遥操
|
||||
-> 右手 trigger 输出 OmniPicker 归一化力控指令
|
||||
-> PC 端查看 D405/D405/D435 视频，后续再接记录节点
|
||||
```
|
||||
|
||||
当前配置参考了最新 `acDual-arm-YikaiFuTest` 中的双臂 acRealman 参数，但本项目仍然采用 PICO 手柄相对位移遥操，不直接搬用戴盟的绝对 `PoseStamped` retarget 链路。
|
||||
|
||||
## 当前范围
|
||||
|
||||
- 已纳入：PICO 双手柄输入、左右 RM75 相对位移控制、右手 OmniPicker trigger 指令出口。
|
||||
- 暂不纳入：AUBO 移动底盘、升降柱、剪刀手、XR 视频流、自动采摘算法。
|
||||
|
||||
## 关键配置来源
|
||||
|
||||
来自 `acDual-arm-YikaiFuTest/realman_pkg/core/arg_cfg.py`：
|
||||
|
||||
- 左臂 IP：`192.168.192.18`
|
||||
- 右臂 IP：`192.168.192.19`
|
||||
- 左右臂 TCP 端口：`8080`
|
||||
- 左右臂工作空间：`x[-0.50, 0.50] y[-0.60, -0.20] z[0.10, 0.50]`
|
||||
- 左右臂工作空间和 IP 沿用最新 acDual-arm 配置。
|
||||
|
||||
`dual_arm_rm75.yaml` 里的左右 `xr_to_robot_matrix` 依据论文中的 OpenXR/PICO 手柄坐标和现场双臂 base 坐标图设置：
|
||||
|
||||
- PICO/OpenXR：`+X` 向右，`+Y` 向上，`+Z` 向后。
|
||||
- 左臂：`+X` 向下，`+Z` 向右，前方工作区对应 `-Y`。
|
||||
- 右臂：`+X` 向上，`+Z` 向左，前方工作区对应 `-Y`。
|
||||
|
||||
真机上仍需低速确认方向。
|
||||
|
||||
## 构建
|
||||
|
||||
在包含 `src/` 的工作空间根目录执行：
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
rosdep update
|
||||
rosdep install --from-paths src -y --ignore-src
|
||||
colcon build --symlink-install
|
||||
source install/setup.bash
|
||||
```
|
||||
|
||||
## 优先验证：双臂 mock
|
||||
|
||||
mock 模式不会连接真实 RM75，适合先验证 UDP 输入、左右手柄路由、相对位移控制和 trigger 夹爪指令。
|
||||
|
||||
终端 1：
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 launch xr_rm_bringup dual_arm_mock.launch.py
|
||||
```
|
||||
|
||||
终端 2：
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000
|
||||
```
|
||||
|
||||
检查话题：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /xr/left_controller
|
||||
ros2 topic echo /xr/right_controller
|
||||
ros2 topic echo /omnipicker/right/force_ratio
|
||||
```
|
||||
|
||||
## 连接双 RM75
|
||||
|
||||
先安装睿尔曼 Python API2，并确认两台控制器网络可达。
|
||||
|
||||
```bash
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 launch xr_rm_bringup dual_arm_realman.launch.py \
|
||||
left_robot_ip:=192.168.192.18 \
|
||||
right_robot_ip:=192.168.192.19
|
||||
```
|
||||
|
||||
单臂入口仍保留，默认按右臂配置：
|
||||
|
||||
```bash
|
||||
ros2 launch xr_rm_bringup single_arm_mock.launch.py
|
||||
ros2 launch xr_rm_bringup single_arm_realman.launch.py robot_ip:=192.168.192.19
|
||||
```
|
||||
|
||||
## UDP JSON 格式
|
||||
|
||||
单个手柄数据：
|
||||
|
||||
```json
|
||||
{
|
||||
"hand": "right",
|
||||
"grip": true,
|
||||
"trigger": 0.2,
|
||||
"pos": [0.12, 1.05, -0.32],
|
||||
"quat": [0.0, 0.0, 0.0, 1.0]
|
||||
}
|
||||
```
|
||||
|
||||
也支持一次发送左右手柄：
|
||||
|
||||
```json
|
||||
{
|
||||
"controllers": {
|
||||
"left": {
|
||||
"grip": true,
|
||||
"trigger": 0.0,
|
||||
"pos": [-0.12, 1.05, -0.32],
|
||||
"quat": [0.0, 0.0, 0.0, 1.0]
|
||||
},
|
||||
"right": {
|
||||
"grip": true,
|
||||
"trigger": 0.4,
|
||||
"pos": [0.12, 1.05, -0.32],
|
||||
"quat": [0.0, 0.0, 0.0, 1.0]
|
||||
}
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
`grip` 是机械臂运动使能：按下时锁定当前手柄和 TCP 起点，之后只跟随相对位移；松开、超时或节点退出都会发送零速度。
|
||||
|
||||
## OmniPicker 接口
|
||||
|
||||
`gripper_trigger_bridge` 会把右手 `trigger` 映射为：
|
||||
|
||||
```text
|
||||
/omnipicker/right/force_ratio std_msgs/Float32
|
||||
```
|
||||
|
||||
默认范围是 `0.0` 开爪、`0.15-0.45` 柔性闭合/保持。实际接 OmniPicker 驱动前，请根据番茄压伤测试收紧 `dual_arm_rm75.yaml` 里的 `max_close_command`。
|
||||
|
||||
## 真机前方向验证
|
||||
|
||||
第一轮保持 `max_linear_speed <= 0.04`，先只上右臂：
|
||||
|
||||
1. 按住右手 `grip`，手柄缓慢上移，确认 TCP 方向。
|
||||
2. 手柄缓慢前推/后拉，确认 TCP 方向。
|
||||
3. 手柄缓慢左移/右移，确认 TCP 方向。
|
||||
4. 若任一方向相反，只改 `dual_arm_rm75.yaml` 中对应臂的 `xr_to_robot_matrix` 符号。
|
||||
5. 右臂确认后再开放左臂。
|
||||
6. 最后接 OmniPicker，并从低力控上限开始抓取番茄。
|
||||
|
||||
当前双臂是两个独立控制节点，没有跨臂碰撞模型；首次测试请让两臂工作区物理分离。
|
||||
|
||||
12
xr_rm_bringup/CMakeLists.txt
Executable file
12
xr_rm_bringup/CMakeLists.txt
Executable file
@ -0,0 +1,12 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(xr_rm_bringup)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(DIRECTORY
|
||||
config
|
||||
launch
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
94
xr_rm_bringup/config/dual_arm_rm75.yaml
Executable file
94
xr_rm_bringup/config/dual_arm_rm75.yaml
Executable file
@ -0,0 +1,94 @@
|
||||
# 阶段一：PICO 遥操作双 RM75 平台配置。
|
||||
#
|
||||
# 参数来源：acDual-arm-YikaiFuTest/realman_pkg/core/arg_cfg.py
|
||||
# - 左臂 IP：192.168.192.18
|
||||
# - 右臂 IP：192.168.192.19
|
||||
# - 左右臂工作空间和 IP 沿用最新 acDual-arm 配置。
|
||||
# - PICO 手柄位移映射根据论文 OpenXR 坐标和现场双臂 base 坐标图单独设置。
|
||||
#
|
||||
# 注意：当前项目仍然采用“手柄相对位移”控制方式。
|
||||
# 按下握持键时锁定当前手柄位姿和 TCP 位姿，之后只跟随相对位移。
|
||||
# acDual-arm 项目使用的是戴盟绝对 PoseStamped 重映射，因此这里只迁移
|
||||
# 坐标标定和安全参数，不迁移其绝对位姿控制链路。
|
||||
|
||||
left_arm_teleop:
|
||||
ros__parameters:
|
||||
arm_name: left_rm75
|
||||
controller_topic: /xr/left_controller
|
||||
control_rate_hz: 50.0
|
||||
command_timeout_sec: 0.12
|
||||
|
||||
# 实验台番茄测试先采用保守参数；完成三轴方向检查和实体急停测试后再提高速度。
|
||||
scale: 0.75
|
||||
kp_linear: 1.8
|
||||
deadband_m: 0.002
|
||||
low_pass_alpha: 0.35
|
||||
max_linear_speed: 0.04
|
||||
enable_position_axes: [true, true, true]
|
||||
|
||||
# 来自 acDual-arm 的 bounds_p[left]：
|
||||
# x[-0.50, 0.50]，y[-0.60, -0.20]，z[0.10, 0.50]。
|
||||
workspace_min: [-0.50, -0.60, 0.10]
|
||||
workspace_max: [0.50, -0.20, 0.50]
|
||||
|
||||
# PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。
|
||||
# 左臂 base 坐标：+X 向下，+Z 向右，前方工作区对应 -Y。
|
||||
# 映射关系：机器人位移增量 = [-手柄y, -手柄z, 手柄x]。
|
||||
xr_to_robot_matrix: [0.0, -1.0, 0.0,
|
||||
0.0, 0.0, -1.0,
|
||||
1.0, 0.0, 0.0]
|
||||
|
||||
use_mock: false
|
||||
mock_initial_pose: [-0.1560, -0.3476, 0.2689, -3.1100, -0.2840, -1.3180]
|
||||
robot_ip: 192.168.192.18
|
||||
robot_port: 8080
|
||||
avoid_singularity: 1
|
||||
frame_type: 1
|
||||
follow: false
|
||||
|
||||
right_arm_teleop:
|
||||
ros__parameters:
|
||||
arm_name: right_rm75
|
||||
controller_topic: /xr/right_controller
|
||||
control_rate_hz: 50.0
|
||||
command_timeout_sec: 0.12
|
||||
|
||||
scale: 0.75
|
||||
kp_linear: 1.8
|
||||
deadband_m: 0.002
|
||||
low_pass_alpha: 0.35
|
||||
max_linear_speed: 0.04
|
||||
enable_position_axes: [true, true, true]
|
||||
|
||||
# 来自 acDual-arm 的 bounds_p[right]。
|
||||
workspace_min: [-0.50, -0.60, 0.10]
|
||||
workspace_max: [0.50, -0.20, 0.50]
|
||||
|
||||
# PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。
|
||||
# 右臂 base 坐标：+X 向上，+Z 向左，前方工作区对应 -Y。
|
||||
# 映射关系：机器人位移增量 = [手柄y, -手柄z, -手柄x]。
|
||||
xr_to_robot_matrix: [0.0, 1.0, 0.0,
|
||||
0.0, 0.0, -1.0,
|
||||
-1.0, 0.0, 0.0]
|
||||
|
||||
use_mock: false
|
||||
mock_initial_pose: [0.1924, -0.3025, 0.2923, 3.0560, 0.1310, 0.9070]
|
||||
robot_ip: 192.168.192.19
|
||||
robot_port: 8080
|
||||
avoid_singularity: 1
|
||||
frame_type: 1
|
||||
follow: false
|
||||
|
||||
right_omnipicker_trigger_bridge:
|
||||
ros__parameters:
|
||||
# 阶段一只使用右手两指夹爪；右手扳机键输出归一化力控指令。
|
||||
controller_topic: /xr/right_controller
|
||||
command_topic: /omnipicker/right/force_ratio
|
||||
publish_rate_hz: 30.0
|
||||
stale_timeout_sec: 0.25
|
||||
require_grip: true
|
||||
trigger_deadband: 0.05
|
||||
open_command: 0.0
|
||||
min_close_command: 0.15
|
||||
max_close_command: 0.45
|
||||
low_pass_alpha: 0.5
|
||||
33
xr_rm_bringup/config/single_arm_rm75.yaml
Executable file
33
xr_rm_bringup/config/single_arm_rm75.yaml
Executable file
@ -0,0 +1,33 @@
|
||||
# 单臂真机 RM75 入口配置。
|
||||
# 默认按最新 acDual-arm 配置中的右臂参数运行，便于先单独验证右臂方向和急停。
|
||||
|
||||
single_arm_velocity_teleop:
|
||||
ros__parameters:
|
||||
arm_name: right_rm75
|
||||
controller_topic: /xr/right_controller
|
||||
control_rate_hz: 50.0
|
||||
command_timeout_sec: 0.12
|
||||
|
||||
scale: 0.75
|
||||
kp_linear: 2.0
|
||||
deadband_m: 0.002
|
||||
low_pass_alpha: 0.35
|
||||
max_linear_speed: 0.04
|
||||
enable_position_axes: [true, true, true]
|
||||
|
||||
workspace_min: [-0.50, -0.60, 0.10]
|
||||
workspace_max: [0.50, -0.20, 0.50]
|
||||
|
||||
# 右臂 PICO/OpenXR 相对位移到 RM75 base 坐标的初始映射：
|
||||
# 机器人位移增量 = [手柄y, -手柄z, -手柄x]。
|
||||
xr_to_robot_matrix: [0.0, 1.0, 0.0,
|
||||
0.0, 0.0, -1.0,
|
||||
-1.0, 0.0, 0.0]
|
||||
|
||||
use_mock: false
|
||||
mock_initial_pose: [0.1924, -0.3025, 0.2923, 3.0560, 0.1310, 0.9070]
|
||||
robot_ip: 192.168.192.19
|
||||
robot_port: 8080
|
||||
avoid_singularity: 1
|
||||
frame_type: 1
|
||||
follow: false
|
||||
55
xr_rm_bringup/launch/dual_arm_mock.launch.py
Executable file
55
xr_rm_bringup/launch/dual_arm_mock.launch.py
Executable file
@ -0,0 +1,55 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
config_file = PathJoinSubstitution([
|
||||
FindPackageShare("xr_rm_bringup"),
|
||||
"config",
|
||||
"dual_arm_rm75.yaml",
|
||||
])
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
|
||||
DeclareLaunchArgument("udp_port", default_value="15000"),
|
||||
# 接收 PICO/XR 侧 UDP 数据，并按左右手字段分发到对应手柄话题。
|
||||
Node(
|
||||
package="xr_rm_input",
|
||||
executable="udp_controller_receiver",
|
||||
name="udp_controller_receiver",
|
||||
output="screen",
|
||||
parameters=[{
|
||||
"udp_host": LaunchConfiguration("udp_host"),
|
||||
"udp_port": LaunchConfiguration("udp_port"),
|
||||
"left_topic": "/xr/left_controller",
|
||||
"right_topic": "/xr/right_controller",
|
||||
}],
|
||||
),
|
||||
# 左臂模拟节点：不连接真实 RM75，只积分速度命令用于验证坐标方向。
|
||||
Node(
|
||||
package="xr_rm_teleop",
|
||||
executable="single_arm_velocity_teleop",
|
||||
name="left_arm_teleop",
|
||||
output="screen",
|
||||
parameters=[config_file, {"use_mock": True}],
|
||||
),
|
||||
# 右臂模拟节点：与左臂共用同一配置文件，但读取右手柄话题。
|
||||
Node(
|
||||
package="xr_rm_teleop",
|
||||
executable="single_arm_velocity_teleop",
|
||||
name="right_arm_teleop",
|
||||
output="screen",
|
||||
parameters=[config_file, {"use_mock": True}],
|
||||
),
|
||||
# 右手扳机键到 OmniPicker 归一化力控指令的桥接，模拟阶段用于检查话题输出。
|
||||
Node(
|
||||
package="xr_rm_teleop",
|
||||
executable="gripper_trigger_bridge",
|
||||
name="right_omnipicker_trigger_bridge",
|
||||
output="screen",
|
||||
parameters=[config_file],
|
||||
),
|
||||
])
|
||||
69
xr_rm_bringup/launch/dual_arm_realman.launch.py
Executable file
69
xr_rm_bringup/launch/dual_arm_realman.launch.py
Executable file
@ -0,0 +1,69 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
config_file = PathJoinSubstitution([
|
||||
FindPackageShare("xr_rm_bringup"),
|
||||
"config",
|
||||
LaunchConfiguration("robot_config"),
|
||||
])
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
|
||||
DeclareLaunchArgument("udp_port", default_value="15000"),
|
||||
DeclareLaunchArgument("left_robot_ip", default_value="192.168.192.18"),
|
||||
DeclareLaunchArgument("right_robot_ip", default_value="192.168.192.19"),
|
||||
DeclareLaunchArgument("robot_port", default_value="8080"),
|
||||
DeclareLaunchArgument("robot_config", default_value="dual_arm_rm75.yaml"),
|
||||
Node(
|
||||
package="xr_rm_input",
|
||||
executable="udp_controller_receiver",
|
||||
name="udp_controller_receiver",
|
||||
output="screen",
|
||||
parameters=[{
|
||||
"udp_host": LaunchConfiguration("udp_host"),
|
||||
"udp_port": LaunchConfiguration("udp_port"),
|
||||
"left_topic": "/xr/left_controller",
|
||||
"right_topic": "/xr/right_controller",
|
||||
}],
|
||||
),
|
||||
Node(
|
||||
package="xr_rm_teleop",
|
||||
executable="single_arm_velocity_teleop",
|
||||
name="left_arm_teleop",
|
||||
output="screen",
|
||||
parameters=[
|
||||
config_file,
|
||||
{
|
||||
"use_mock": False,
|
||||
"robot_ip": LaunchConfiguration("left_robot_ip"),
|
||||
"robot_port": LaunchConfiguration("robot_port"),
|
||||
},
|
||||
],
|
||||
),
|
||||
Node(
|
||||
package="xr_rm_teleop",
|
||||
executable="single_arm_velocity_teleop",
|
||||
name="right_arm_teleop",
|
||||
output="screen",
|
||||
parameters=[
|
||||
config_file,
|
||||
{
|
||||
"use_mock": False,
|
||||
"robot_ip": LaunchConfiguration("right_robot_ip"),
|
||||
"robot_port": LaunchConfiguration("robot_port"),
|
||||
},
|
||||
],
|
||||
),
|
||||
Node(
|
||||
package="xr_rm_teleop",
|
||||
executable="gripper_trigger_bridge",
|
||||
name="right_omnipicker_trigger_bridge",
|
||||
output="screen",
|
||||
parameters=[config_file],
|
||||
),
|
||||
])
|
||||
36
xr_rm_bringup/launch/single_arm_mock.launch.py
Executable file
36
xr_rm_bringup/launch/single_arm_mock.launch.py
Executable file
@ -0,0 +1,36 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
config_file = PathJoinSubstitution([
|
||||
FindPackageShare("xr_rm_teleop"),
|
||||
"config",
|
||||
"single_arm.yaml",
|
||||
])
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
|
||||
DeclareLaunchArgument("udp_port", default_value="15000"),
|
||||
Node(
|
||||
package="xr_rm_input",
|
||||
executable="udp_controller_receiver",
|
||||
name="udp_controller_receiver",
|
||||
output="screen",
|
||||
parameters=[{
|
||||
"udp_host": LaunchConfiguration("udp_host"),
|
||||
"udp_port": LaunchConfiguration("udp_port"),
|
||||
"topic": "/xr/right_controller",
|
||||
}],
|
||||
),
|
||||
Node(
|
||||
package="xr_rm_teleop",
|
||||
executable="single_arm_velocity_teleop",
|
||||
name="single_arm_velocity_teleop",
|
||||
output="screen",
|
||||
parameters=[config_file, {"use_mock": True}],
|
||||
),
|
||||
])
|
||||
46
xr_rm_bringup/launch/single_arm_realman.launch.py
Executable file
46
xr_rm_bringup/launch/single_arm_realman.launch.py
Executable file
@ -0,0 +1,46 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
config_file = PathJoinSubstitution([
|
||||
FindPackageShare("xr_rm_bringup"),
|
||||
"config",
|
||||
LaunchConfiguration("robot_config"),
|
||||
])
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
|
||||
DeclareLaunchArgument("udp_port", default_value="15000"),
|
||||
DeclareLaunchArgument("robot_ip", default_value="192.168.192.19"),
|
||||
DeclareLaunchArgument("robot_port", default_value="8080"),
|
||||
DeclareLaunchArgument("robot_config", default_value="single_arm_rm75.yaml"),
|
||||
Node(
|
||||
package="xr_rm_input",
|
||||
executable="udp_controller_receiver",
|
||||
name="udp_controller_receiver",
|
||||
output="screen",
|
||||
parameters=[{
|
||||
"udp_host": LaunchConfiguration("udp_host"),
|
||||
"udp_port": LaunchConfiguration("udp_port"),
|
||||
"topic": "/xr/right_controller",
|
||||
}],
|
||||
),
|
||||
Node(
|
||||
package="xr_rm_teleop",
|
||||
executable="single_arm_velocity_teleop",
|
||||
name="single_arm_velocity_teleop",
|
||||
output="screen",
|
||||
parameters=[
|
||||
config_file,
|
||||
{
|
||||
"use_mock": False,
|
||||
"robot_ip": LaunchConfiguration("robot_ip"),
|
||||
"robot_port": LaunchConfiguration("robot_port"),
|
||||
},
|
||||
],
|
||||
),
|
||||
])
|
||||
18
xr_rm_bringup/package.xml
Executable file
18
xr_rm_bringup/package.xml
Executable file
@ -0,0 +1,18 @@
|
||||
<?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>xr_rm_bringup</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Launch and configuration package for XR RealMan teleoperation.</description>
|
||||
<maintainer email="user@example.com">Yikai Fu</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<exec_depend>xr_rm_input</exec_depend>
|
||||
<exec_depend>xr_rm_teleop</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
23
xr_rm_input/launch/udp_receiver.launch.py
Executable file
23
xr_rm_input/launch/udp_receiver.launch.py
Executable file
@ -0,0 +1,23 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
|
||||
DeclareLaunchArgument("udp_port", default_value="15000"),
|
||||
DeclareLaunchArgument("topic", default_value="/xr/right_controller"),
|
||||
Node(
|
||||
package="xr_rm_input",
|
||||
executable="udp_controller_receiver",
|
||||
name="udp_controller_receiver",
|
||||
output="screen",
|
||||
parameters=[{
|
||||
"udp_host": LaunchConfiguration("udp_host"),
|
||||
"udp_port": LaunchConfiguration("udp_port"),
|
||||
"topic": LaunchConfiguration("topic"),
|
||||
}],
|
||||
),
|
||||
])
|
||||
23
xr_rm_input/package.xml
Executable file
23
xr_rm_input/package.xml
Executable file
@ -0,0 +1,23 @@
|
||||
<?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>xr_rm_input</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Receives XR controller pose packets and publishes left/right ROS2 controller messages.</description>
|
||||
<maintainer email="user@example.com">Yikai Fu</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>xr_rm_interfaces</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
1
xr_rm_input/resource/xr_rm_input
Executable file
1
xr_rm_input/resource/xr_rm_input
Executable file
@ -0,0 +1 @@
|
||||
|
||||
4
xr_rm_input/setup.cfg
Executable file
4
xr_rm_input/setup.cfg
Executable file
@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/xr_rm_input
|
||||
[install]
|
||||
install_scripts=$base/lib/xr_rm_input
|
||||
28
xr_rm_input/setup.py
Executable file
28
xr_rm_input/setup.py
Executable file
@ -0,0 +1,28 @@
|
||||
from glob import glob
|
||||
from setuptools import setup
|
||||
|
||||
package_name = "xr_rm_input"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.1.0",
|
||||
packages=[package_name],
|
||||
data_files=[
|
||||
("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
|
||||
(f"share/{package_name}", ["package.xml"]),
|
||||
(f"share/{package_name}/launch", glob("launch/*.launch.py")),
|
||||
],
|
||||
install_requires=["setuptools"],
|
||||
zip_safe=True,
|
||||
maintainer="Yikai Fu",
|
||||
maintainer_email="user@example.com",
|
||||
description="Receives XR controller pose packets and publishes left/right ROS2 messages.",
|
||||
license="Apache-2.0",
|
||||
tests_require=["pytest"],
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
"udp_controller_receiver = xr_rm_input.udp_controller_receiver:main",
|
||||
"sample_udp_sender = xr_rm_input.sample_udp_sender:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
1
xr_rm_input/xr_rm_input/__init__.py
Executable file
1
xr_rm_input/xr_rm_input/__init__.py
Executable file
@ -0,0 +1 @@
|
||||
|
||||
58
xr_rm_input/xr_rm_input/sample_udp_sender.py
Executable file
58
xr_rm_input/xr_rm_input/sample_udp_sender.py
Executable file
@ -0,0 +1,58 @@
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import socket
|
||||
import time
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="通过 UDP 发送一段模拟 XR 左/右手柄数据。")
|
||||
parser.add_argument("--host", default="127.0.0.1")
|
||||
parser.add_argument("--port", type=int, default=15000)
|
||||
parser.add_argument("--seconds", type=float, default=5.0)
|
||||
parser.add_argument("--hz", type=float, default=60.0)
|
||||
parser.add_argument("--amplitude", type=float, default=0.04)
|
||||
parser.add_argument("--hand", choices=("left", "right", "both"), default="right")
|
||||
args = parser.parse_args()
|
||||
|
||||
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
dt = 1.0 / args.hz
|
||||
t0 = time.monotonic()
|
||||
|
||||
hands = ("left", "right") if args.hand == "both" else (args.hand,)
|
||||
packets = {
|
||||
hand: {
|
||||
"t": 0.0,
|
||||
"hand": hand,
|
||||
"grip": False,
|
||||
"trigger": 0.0,
|
||||
"pos": [0.0, 1.0, 0.0],
|
||||
"quat": [0.0, 0.0, 0.0, 1.0],
|
||||
}
|
||||
for hand in hands
|
||||
}
|
||||
|
||||
while time.monotonic() - t0 < args.seconds:
|
||||
t = time.monotonic() - t0
|
||||
# 模拟按住握持键后缓慢移动手柄，用来测试相对位移控制链路。
|
||||
for index, packet in enumerate(packets.values()):
|
||||
phase = index * math.pi
|
||||
packet["t"] = t
|
||||
packet["grip"] = True
|
||||
packet["trigger"] = max(0.0, math.sin(2.0 * math.pi * 0.1 * t))
|
||||
packet["pos"] = [
|
||||
args.amplitude * math.sin(2.0 * math.pi * 0.2 * t + phase),
|
||||
1.0,
|
||||
0.0,
|
||||
]
|
||||
sock.sendto(json.dumps(packet).encode("utf-8"), (args.host, args.port))
|
||||
time.sleep(dt)
|
||||
|
||||
for packet in packets.values():
|
||||
packet["grip"] = False
|
||||
packet["trigger"] = 0.0
|
||||
sock.sendto(json.dumps(packet).encode("utf-8"), (args.host, args.port))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
215
xr_rm_input/xr_rm_input/udp_controller_receiver.py
Executable file
215
xr_rm_input/xr_rm_input/udp_controller_receiver.py
Executable file
@ -0,0 +1,215 @@
|
||||
import json
|
||||
import socket
|
||||
from collections.abc import Iterable, Mapping
|
||||
from typing import Any
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
|
||||
from xr_rm_interfaces.msg import XrController
|
||||
|
||||
|
||||
class UdpControllerReceiver(Node):
|
||||
"""将轻量级 XR UDP 数据包转换成 ROS2 左/右手柄消息。"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("udp_controller_receiver")
|
||||
|
||||
self.declare_parameter("udp_host", "0.0.0.0")
|
||||
self.declare_parameter("udp_port", 15000)
|
||||
self.declare_parameter("topic", "/xr/right_controller")
|
||||
self.declare_parameter("left_topic", "/xr/left_controller")
|
||||
self.declare_parameter("right_topic", "")
|
||||
self.declare_parameter("default_hand", "right")
|
||||
self.declare_parameter("timer_hz", 100.0)
|
||||
self.declare_parameter("quat_order", "xyzw")
|
||||
|
||||
udp_host = self.get_parameter("udp_host").value
|
||||
udp_port = int(self.get_parameter("udp_port").value)
|
||||
legacy_topic = str(self.get_parameter("topic").value)
|
||||
left_topic = str(self.get_parameter("left_topic").value)
|
||||
right_topic = str(self.get_parameter("right_topic").value) or legacy_topic
|
||||
timer_hz = float(self.get_parameter("timer_hz").value)
|
||||
|
||||
self._default_hand = self._normalize_hand(
|
||||
self.get_parameter("default_hand").value,
|
||||
fallback="right",
|
||||
)
|
||||
self._quat_order = str(self.get_parameter("quat_order").value).lower()
|
||||
|
||||
# UDP 接收使用非阻塞模式，避免没有手柄数据时卡住 ROS2 定时器。
|
||||
self._publishers = {
|
||||
"left": self.create_publisher(XrController, left_topic, 10),
|
||||
"right": self.create_publisher(XrController, right_topic, 10),
|
||||
}
|
||||
self._socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
self._socket.bind((udp_host, udp_port))
|
||||
self._socket.setblocking(False)
|
||||
|
||||
self.create_timer(1.0 / timer_hz, self._poll_socket)
|
||||
self.get_logger().info(
|
||||
"正在监听 XR UDP 数据："
|
||||
f"{udp_host}:{udp_port} -> left={left_topic}, right={right_topic}"
|
||||
)
|
||||
|
||||
def _poll_socket(self) -> None:
|
||||
while True:
|
||||
try:
|
||||
data, _ = self._socket.recvfrom(4096)
|
||||
except BlockingIOError:
|
||||
return
|
||||
|
||||
try:
|
||||
payload = json.loads(data.decode("utf-8"))
|
||||
controller_payloads = list(self._iter_controller_payloads(payload))
|
||||
except (UnicodeDecodeError, json.JSONDecodeError, KeyError, TypeError, ValueError) as exc:
|
||||
self.get_logger().warn(f"XR 数据包格式错误：{exc}", throttle_duration_sec=1.0)
|
||||
continue
|
||||
|
||||
for hand, controller_payload in controller_payloads:
|
||||
try:
|
||||
msg = self._payload_to_msg(controller_payload, hand)
|
||||
except (KeyError, TypeError, ValueError) as exc:
|
||||
self.get_logger().warn(f"XR 手柄字段错误：{exc}", throttle_duration_sec=1.0)
|
||||
continue
|
||||
self._publishers[msg.hand].publish(msg)
|
||||
|
||||
def _iter_controller_payloads(self, payload: Any) -> Iterable[tuple[str, Mapping[str, Any]]]:
|
||||
if isinstance(payload, list):
|
||||
for item in payload:
|
||||
yield from self._iter_controller_payloads(item)
|
||||
return
|
||||
|
||||
if not isinstance(payload, Mapping):
|
||||
raise TypeError("expected JSON object or array")
|
||||
|
||||
if self._contains_pose(payload):
|
||||
hand = self._normalize_hand(payload.get("hand", self._default_hand))
|
||||
yield hand, payload
|
||||
return
|
||||
|
||||
controllers = payload.get("controllers", payload.get("controller"))
|
||||
if isinstance(controllers, Mapping):
|
||||
if self._contains_pose(controllers):
|
||||
hand = self._normalize_hand(controllers.get("hand", self._default_hand))
|
||||
yield hand, controllers
|
||||
return
|
||||
for hand_key, controller_payload in controllers.items():
|
||||
if isinstance(controller_payload, Mapping):
|
||||
hand = self._normalize_hand(controller_payload.get("hand", hand_key))
|
||||
yield hand, controller_payload
|
||||
return
|
||||
|
||||
if isinstance(controllers, list):
|
||||
for controller_payload in controllers:
|
||||
yield from self._iter_controller_payloads(controller_payload)
|
||||
return
|
||||
|
||||
for hand_key in ("left", "right", "left_controller", "right_controller"):
|
||||
controller_payload = payload.get(hand_key)
|
||||
if isinstance(controller_payload, Mapping):
|
||||
hand = self._normalize_hand(controller_payload.get("hand", hand_key))
|
||||
yield hand, controller_payload
|
||||
|
||||
def _payload_to_msg(self, payload: Mapping[str, Any], hand: str) -> XrController:
|
||||
pos, quat = self._extract_pose(payload)
|
||||
if len(pos) != 3 or len(quat) != 4:
|
||||
raise ValueError("expected pos[3] and quat[4]")
|
||||
|
||||
msg = XrController()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = str(payload.get("frame_id", "xr_world"))
|
||||
msg.hand = hand
|
||||
msg.grip = self._as_bool(payload.get("grip", payload.get("grip_button", False)))
|
||||
msg.trigger = self._clamp_float(payload.get("trigger", 0.0), 0.0, 1.0)
|
||||
msg.pose.position.x = float(pos[0])
|
||||
msg.pose.position.y = float(pos[1])
|
||||
msg.pose.position.z = float(pos[2])
|
||||
|
||||
# Unity/OpenXR 项目里的四元数顺序可能不同，这里用参数统一转换成 ROS 的 xyzw。
|
||||
if self._quat_order == "wxyz":
|
||||
w, x, y, z = quat
|
||||
else:
|
||||
x, y, z, w = quat
|
||||
msg.pose.orientation.x = float(x)
|
||||
msg.pose.orientation.y = float(y)
|
||||
msg.pose.orientation.z = float(z)
|
||||
msg.pose.orientation.w = float(w)
|
||||
return msg
|
||||
|
||||
@staticmethod
|
||||
def _contains_pose(payload: Mapping[str, Any]) -> bool:
|
||||
return any(key in payload for key in ("pos", "position", "p", "pose"))
|
||||
|
||||
def _extract_pose(self, payload: Mapping[str, Any]) -> tuple[list[float], list[float]]:
|
||||
pose = payload.get("pose")
|
||||
if isinstance(pose, Mapping):
|
||||
pos = pose.get("position", pose.get("pos", pose.get("p")))
|
||||
quat = pose.get("orientation", pose.get("quat", pose.get("q", [0.0, 0.0, 0.0, 1.0])))
|
||||
elif isinstance(pose, list) and len(pose) >= 7:
|
||||
pos = pose[:3]
|
||||
quat = pose[3:7]
|
||||
else:
|
||||
pos = payload.get("pos", payload.get("position", payload.get("p")))
|
||||
quat = payload.get("quat", payload.get("orientation", payload.get("q", [0.0, 0.0, 0.0, 1.0])))
|
||||
|
||||
if pos is None:
|
||||
raise KeyError("missing pos/position/p")
|
||||
return self._vector3(pos), self._quaternion(quat)
|
||||
|
||||
@staticmethod
|
||||
def _vector3(value: Any) -> list[float]:
|
||||
if isinstance(value, Mapping):
|
||||
return [float(value[key]) for key in ("x", "y", "z")]
|
||||
vector = list(value)
|
||||
if len(vector) != 3:
|
||||
raise ValueError("expected 3D position")
|
||||
return [float(item) for item in vector]
|
||||
|
||||
def _quaternion(self, value: Any) -> list[float]:
|
||||
if isinstance(value, Mapping):
|
||||
if self._quat_order == "wxyz":
|
||||
return [float(value[key]) for key in ("w", "x", "y", "z")]
|
||||
return [float(value[key]) for key in ("x", "y", "z", "w")]
|
||||
quat = list(value)
|
||||
if len(quat) != 4:
|
||||
raise ValueError("expected quaternion")
|
||||
return [float(item) for item in quat]
|
||||
|
||||
def _normalize_hand(self, hand: Any, fallback: str | None = None) -> str:
|
||||
value = str(hand).lower()
|
||||
if value in ("left", "l", "left_controller"):
|
||||
return "left"
|
||||
if value in ("right", "r", "right_controller"):
|
||||
return "right"
|
||||
fallback_hand = fallback or self._default_hand
|
||||
self.get_logger().warn(
|
||||
f"未知手柄标识 {hand!r}，使用默认 {fallback_hand}",
|
||||
throttle_duration_sec=1.0,
|
||||
)
|
||||
return fallback_hand
|
||||
|
||||
@staticmethod
|
||||
def _as_bool(value: Any) -> bool:
|
||||
if isinstance(value, str):
|
||||
return value.lower() in ("1", "true", "yes", "on")
|
||||
return bool(value)
|
||||
|
||||
@staticmethod
|
||||
def _clamp_float(value: Any, low: float, high: float) -> float:
|
||||
number = float(value)
|
||||
return min(max(number, low), high)
|
||||
|
||||
|
||||
def main(args: list[str] | None = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = UdpControllerReceiver()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
19
xr_rm_interfaces/CMakeLists.txt
Executable file
19
xr_rm_interfaces/CMakeLists.txt
Executable file
@ -0,0 +1,19 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(xr_rm_interfaces)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/XrController.msg"
|
||||
DEPENDENCIES geometry_msgs std_msgs
|
||||
)
|
||||
|
||||
ament_export_dependencies(rosidl_default_runtime)
|
||||
ament_package()
|
||||
5
xr_rm_interfaces/msg/XrController.msg
Executable file
5
xr_rm_interfaces/msg/XrController.msg
Executable file
@ -0,0 +1,5 @@
|
||||
std_msgs/Header header
|
||||
string hand
|
||||
bool grip
|
||||
float32 trigger
|
||||
geometry_msgs/Pose pose
|
||||
24
xr_rm_interfaces/package.xml
Executable file
24
xr_rm_interfaces/package.xml
Executable file
@ -0,0 +1,24 @@
|
||||
<?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>xr_rm_interfaces</name>
|
||||
<version>0.1.0</version>
|
||||
<description>ROS2 interfaces for XR controller based RealMan teleoperation.</description>
|
||||
<maintainer email="user@example.com">Yikai Fu</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>rosidl_default_generators</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
34
xr_rm_teleop/config/single_arm.yaml
Executable file
34
xr_rm_teleop/config/single_arm.yaml
Executable file
@ -0,0 +1,34 @@
|
||||
# 单臂模拟/默认配置。
|
||||
# 默认按最新 acDual-arm 配置中的右臂参数运行，使模拟和真机单臂测试使用同一套坐标约定。
|
||||
|
||||
single_arm_velocity_teleop:
|
||||
ros__parameters:
|
||||
arm_name: right_rm75
|
||||
controller_topic: /xr/right_controller
|
||||
control_rate_hz: 50.0
|
||||
command_timeout_sec: 0.12
|
||||
|
||||
scale: 0.75
|
||||
kp_linear: 2.0
|
||||
deadband_m: 0.002
|
||||
low_pass_alpha: 0.35
|
||||
max_linear_speed: 0.04
|
||||
enable_position_axes: [true, true, true]
|
||||
|
||||
workspace_min: [-0.50, -0.60, 0.10]
|
||||
workspace_max: [0.50, -0.20, 0.50]
|
||||
|
||||
# 右臂 PICO/OpenXR 相对位移到 RM75 base 坐标的初始映射：
|
||||
# 机器人位移增量 = [手柄y, -手柄z, -手柄x]。
|
||||
xr_to_robot_matrix: [0.0, 1.0, 0.0,
|
||||
0.0, 0.0, -1.0,
|
||||
-1.0, 0.0, 0.0]
|
||||
|
||||
use_mock: true
|
||||
mock_initial_pose: [0.1924, -0.3025, 0.2923, 3.0560, 0.1310, 0.9070]
|
||||
|
||||
robot_ip: 192.168.192.19
|
||||
robot_port: 8080
|
||||
avoid_singularity: 1
|
||||
frame_type: 1
|
||||
follow: false
|
||||
23
xr_rm_teleop/package.xml
Executable file
23
xr_rm_teleop/package.xml
Executable file
@ -0,0 +1,23 @@
|
||||
<?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>xr_rm_teleop</name>
|
||||
<version>0.1.0</version>
|
||||
<description>XR relative-motion teleoperation controllers for RealMan RM75 arms.</description>
|
||||
<maintainer email="user@example.com">Yikai Fu</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>xr_rm_interfaces</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
1
xr_rm_teleop/resource/xr_rm_teleop
Executable file
1
xr_rm_teleop/resource/xr_rm_teleop
Executable file
@ -0,0 +1 @@
|
||||
|
||||
4
xr_rm_teleop/setup.cfg
Executable file
4
xr_rm_teleop/setup.cfg
Executable file
@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/xr_rm_teleop
|
||||
[install]
|
||||
install_scripts=$base/lib/xr_rm_teleop
|
||||
28
xr_rm_teleop/setup.py
Executable file
28
xr_rm_teleop/setup.py
Executable file
@ -0,0 +1,28 @@
|
||||
from glob import glob
|
||||
from setuptools import setup
|
||||
|
||||
package_name = "xr_rm_teleop"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.1.0",
|
||||
packages=[package_name],
|
||||
data_files=[
|
||||
("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
|
||||
(f"share/{package_name}", ["package.xml"]),
|
||||
(f"share/{package_name}/config", glob("config/*.yaml")),
|
||||
],
|
||||
install_requires=["setuptools"],
|
||||
zip_safe=True,
|
||||
maintainer="Yikai Fu",
|
||||
maintainer_email="user@example.com",
|
||||
description="XR relative-motion teleoperation controllers for RealMan RM75 arms.",
|
||||
license="Apache-2.0",
|
||||
tests_require=["pytest"],
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
"gripper_trigger_bridge = xr_rm_teleop.gripper_trigger_bridge:main",
|
||||
"single_arm_velocity_teleop = xr_rm_teleop.single_arm_velocity_teleop:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
1
xr_rm_teleop/xr_rm_teleop/__init__.py
Executable file
1
xr_rm_teleop/xr_rm_teleop/__init__.py
Executable file
@ -0,0 +1 @@
|
||||
|
||||
112
xr_rm_teleop/xr_rm_teleop/gripper_trigger_bridge.py
Executable file
112
xr_rm_teleop/xr_rm_teleop/gripper_trigger_bridge.py
Executable file
@ -0,0 +1,112 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import Float32
|
||||
|
||||
from xr_rm_interfaces.msg import XrController
|
||||
|
||||
|
||||
def _clamp(value: float, low: float, high: float) -> float:
|
||||
return min(max(value, low), high)
|
||||
|
||||
|
||||
class GripperTriggerBridge(Node):
|
||||
"""将 PICO 扳机键映射为 OmniPicker 可订阅的归一化力控指令。"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("gripper_trigger_bridge")
|
||||
|
||||
self.declare_parameter("controller_topic", "/xr/right_controller")
|
||||
self.declare_parameter("command_topic", "/omnipicker/right/force_ratio")
|
||||
self.declare_parameter("publish_rate_hz", 30.0)
|
||||
self.declare_parameter("stale_timeout_sec", 0.25)
|
||||
self.declare_parameter("require_grip", True)
|
||||
# 扳机键输入小于死区时保持开爪，避免手柄轻微误触导致夹爪闭合。
|
||||
self.declare_parameter("trigger_deadband", 0.05)
|
||||
self.declare_parameter("open_command", 0.0)
|
||||
# 阶段一以保护番茄为先，闭合力控上限应从低值开始实测。
|
||||
self.declare_parameter("min_close_command", 0.15)
|
||||
self.declare_parameter("max_close_command", 0.45)
|
||||
self.declare_parameter("low_pass_alpha", 0.5)
|
||||
|
||||
controller_topic = str(self.get_parameter("controller_topic").value)
|
||||
command_topic = str(self.get_parameter("command_topic").value)
|
||||
publish_rate_hz = float(self.get_parameter("publish_rate_hz").value)
|
||||
|
||||
self._stale_timeout_sec = float(self.get_parameter("stale_timeout_sec").value)
|
||||
self._require_grip = bool(self.get_parameter("require_grip").value)
|
||||
self._trigger_deadband = float(self.get_parameter("trigger_deadband").value)
|
||||
self._open_command = float(self.get_parameter("open_command").value)
|
||||
self._min_close_command = float(self.get_parameter("min_close_command").value)
|
||||
self._max_close_command = float(self.get_parameter("max_close_command").value)
|
||||
self._low_pass_alpha = float(self.get_parameter("low_pass_alpha").value)
|
||||
self._validate_parameters()
|
||||
|
||||
self._last_msg: XrController | None = None
|
||||
self._last_msg_time = None
|
||||
self._filtered_command = self._open_command
|
||||
|
||||
self._publisher = self.create_publisher(Float32, command_topic, 10)
|
||||
self.create_subscription(XrController, controller_topic, self._on_controller, 10)
|
||||
self.create_timer(1.0 / publish_rate_hz, self._publish_command)
|
||||
|
||||
self.get_logger().info(
|
||||
f"夹爪 trigger 桥接已启动：{controller_topic} -> {command_topic}"
|
||||
)
|
||||
|
||||
def _on_controller(self, msg: XrController) -> None:
|
||||
self._last_msg = msg
|
||||
self._last_msg_time = self.get_clock().now()
|
||||
|
||||
def _publish_command(self) -> None:
|
||||
command = self._open_command
|
||||
|
||||
if self._last_msg is not None and self._last_msg_time is not None:
|
||||
age = (self.get_clock().now() - self._last_msg_time).nanoseconds * 1e-9
|
||||
if age <= self._stale_timeout_sec:
|
||||
command = self._command_from_trigger(self._last_msg)
|
||||
|
||||
# 对夹爪指令做一阶平滑，避免扳机键抖动直接传到力控命令。
|
||||
alpha = self._low_pass_alpha
|
||||
self._filtered_command = alpha * command + (1.0 - alpha) * self._filtered_command
|
||||
msg = Float32()
|
||||
msg.data = float(self._filtered_command)
|
||||
self._publisher.publish(msg)
|
||||
|
||||
def _command_from_trigger(self, msg: XrController) -> float:
|
||||
if self._require_grip and not msg.grip:
|
||||
return self._open_command
|
||||
|
||||
trigger = _clamp(float(msg.trigger), 0.0, 1.0)
|
||||
if trigger <= self._trigger_deadband:
|
||||
return self._open_command
|
||||
|
||||
ratio = (trigger - self._trigger_deadband) / (1.0 - self._trigger_deadband)
|
||||
return self._min_close_command + ratio * (self._max_close_command - self._min_close_command)
|
||||
|
||||
def _validate_parameters(self) -> None:
|
||||
if not 0.0 <= self._trigger_deadband < 1.0:
|
||||
raise ValueError("trigger_deadband must be in [0, 1)")
|
||||
if self._min_close_command < self._open_command:
|
||||
raise ValueError("min_close_command must be >= open_command")
|
||||
if self._max_close_command < self._min_close_command:
|
||||
raise ValueError("max_close_command must be >= min_close_command")
|
||||
if not 0.0 <= self._low_pass_alpha <= 1.0:
|
||||
raise ValueError("low_pass_alpha must be in [0, 1]")
|
||||
|
||||
|
||||
def main(args: list[str] | None = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = GripperTriggerBridge()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
158
xr_rm_teleop/xr_rm_teleop/realman_adapter.py
Executable file
158
xr_rm_teleop/xr_rm_teleop/realman_adapter.py
Executable file
@ -0,0 +1,158 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from numbers import Number
|
||||
from typing import Any
|
||||
|
||||
|
||||
@dataclass
|
||||
class ArmPose:
|
||||
x: float
|
||||
y: float
|
||||
z: float
|
||||
rx: float = 0.0
|
||||
ry: float = 0.0
|
||||
rz: float = 0.0
|
||||
|
||||
def xyz(self) -> list[float]:
|
||||
return [self.x, self.y, self.z]
|
||||
|
||||
|
||||
class MockRealManAdapter:
|
||||
"""无机械臂时使用的运动学模拟器，用于验证 ROS2 遥操链路。"""
|
||||
|
||||
def __init__(self, initial_pose: list[float], dt: float) -> None:
|
||||
self._pose = ArmPose(*initial_pose[:6])
|
||||
self._dt = dt
|
||||
self.last_velocity = [0.0] * 6
|
||||
|
||||
def connect(self) -> None:
|
||||
return
|
||||
|
||||
def get_current_pose(self) -> ArmPose:
|
||||
return self._pose
|
||||
|
||||
def send_cartesian_velocity(self, velocity: list[float], follow: bool) -> None:
|
||||
del follow
|
||||
self.last_velocity = velocity
|
||||
# 模拟模式只做简单积分，便于观察控制器是否在按预期更新末端位置。
|
||||
self._pose.x += velocity[0] * self._dt
|
||||
self._pose.y += velocity[1] * self._dt
|
||||
self._pose.z += velocity[2] * self._dt
|
||||
self._pose.rx += velocity[3] * self._dt
|
||||
self._pose.ry += velocity[4] * self._dt
|
||||
self._pose.rz += velocity[5] * self._dt
|
||||
|
||||
def stop(self) -> None:
|
||||
self.last_velocity = [0.0] * 6
|
||||
|
||||
def close(self) -> None:
|
||||
self.stop()
|
||||
|
||||
|
||||
class RealManAdapter:
|
||||
"""睿尔曼 Python API2 的笛卡尔速度透传适配层。"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
robot_ip: str,
|
||||
robot_port: int,
|
||||
dt: float,
|
||||
avoid_singularity: int,
|
||||
frame_type: int,
|
||||
) -> None:
|
||||
self._robot_ip = robot_ip
|
||||
self._robot_port = robot_port
|
||||
self._dt_ms = int(round(dt * 1000.0))
|
||||
self._avoid_singularity = avoid_singularity
|
||||
self._frame_type = frame_type
|
||||
self._arm: Any | None = None
|
||||
|
||||
def connect(self) -> None:
|
||||
try:
|
||||
from Robotic_Arm.rm_robot_interface import RoboticArm, rm_thread_mode_e
|
||||
except ImportError as exc:
|
||||
raise RuntimeError(
|
||||
"未安装睿尔曼 Python API2。请安装厂商 SDK，或用 use_mock:=true 先跑模拟模式。"
|
||||
) from exc
|
||||
|
||||
self._arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
|
||||
ret = self._arm.rm_create_robot_arm(self._robot_ip, self._robot_port)
|
||||
self._check_return(ret, "rm_create_robot_arm")
|
||||
# 速度透传初始化必须和控制循环周期一致，避免真实机械臂出现周期不稳定。
|
||||
ret = self._arm.rm_set_movev_canfd_init(
|
||||
self._avoid_singularity,
|
||||
self._frame_type,
|
||||
self._dt_ms,
|
||||
)
|
||||
self._check_return(ret, "rm_set_movev_canfd_init")
|
||||
|
||||
def get_current_pose(self) -> ArmPose:
|
||||
self._require_arm()
|
||||
state = self._arm.rm_get_current_arm_state()
|
||||
pose = self._find_pose(state)
|
||||
if pose is None:
|
||||
raise RuntimeError(f"无法从睿尔曼状态中解析当前 TCP 位姿：{state!r}")
|
||||
return ArmPose(*pose[:6])
|
||||
|
||||
def send_cartesian_velocity(self, velocity: list[float], follow: bool) -> None:
|
||||
self._require_arm()
|
||||
ret = self._arm.rm_movev_canfd(velocity, follow, 0, 0)
|
||||
self._check_return(ret, "rm_movev_canfd")
|
||||
|
||||
def stop(self) -> None:
|
||||
if self._arm is None:
|
||||
return
|
||||
try:
|
||||
self._arm.rm_movev_canfd([0.0] * 6, False, 0, 0)
|
||||
self._arm.rm_set_arm_slow_stop()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def close(self) -> None:
|
||||
if self._arm is None:
|
||||
return
|
||||
self.stop()
|
||||
try:
|
||||
self._arm.rm_delete_robot_arm()
|
||||
finally:
|
||||
self._arm = None
|
||||
|
||||
def _require_arm(self) -> None:
|
||||
if self._arm is None:
|
||||
raise RuntimeError("睿尔曼机械臂尚未连接")
|
||||
|
||||
@staticmethod
|
||||
def _check_return(ret: Any, name: str) -> None:
|
||||
code = ret[0] if isinstance(ret, tuple) and ret else ret
|
||||
if isinstance(code, int) and code != 0:
|
||||
raise RuntimeError(f"{name} failed with code {code}: {ret!r}")
|
||||
|
||||
@classmethod
|
||||
def _find_pose(cls, obj: Any) -> list[float] | None:
|
||||
# 不同 SDK 版本返回字段可能略有差异，因此递归查找常见 TCP 位姿字段。
|
||||
if isinstance(obj, dict):
|
||||
for key in ("pose", "tool_pose", "tcp_pose", "current_pose"):
|
||||
pose = cls._as_pose(obj.get(key))
|
||||
if pose is not None:
|
||||
return pose
|
||||
for value in obj.values():
|
||||
pose = cls._find_pose(value)
|
||||
if pose is not None:
|
||||
return pose
|
||||
elif isinstance(obj, (list, tuple)):
|
||||
pose = cls._as_pose(obj)
|
||||
if pose is not None:
|
||||
return pose
|
||||
for value in obj:
|
||||
pose = cls._find_pose(value)
|
||||
if pose is not None:
|
||||
return pose
|
||||
return None
|
||||
|
||||
@staticmethod
|
||||
def _as_pose(value: Any) -> list[float] | None:
|
||||
if isinstance(value, (list, tuple)) and len(value) >= 6:
|
||||
if all(isinstance(item, Number) for item in value[:6]):
|
||||
return [float(item) for item in value[:6]]
|
||||
return None
|
||||
243
xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
Executable file
243
xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
Executable file
@ -0,0 +1,243 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from typing import Iterable
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
|
||||
from xr_rm_interfaces.msg import XrController
|
||||
|
||||
from .realman_adapter import MockRealManAdapter, RealManAdapter
|
||||
|
||||
|
||||
def _norm(values: Iterable[float]) -> float:
|
||||
return math.sqrt(sum(value * value for value in values))
|
||||
|
||||
|
||||
def _clamp(value: float, low: float, high: float) -> float:
|
||||
return min(max(value, low), high)
|
||||
|
||||
|
||||
class SingleArmVelocityTeleop(Node):
|
||||
"""基于 XR 手柄相对位移的 RM75 单臂笛卡尔速度遥操节点。"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("single_arm_velocity_teleop")
|
||||
|
||||
self.declare_parameter("arm_name", "rm75")
|
||||
self.declare_parameter("controller_topic", "/xr/right_controller")
|
||||
self.declare_parameter("control_rate_hz", 50.0)
|
||||
self.declare_parameter("command_timeout_sec", 0.12)
|
||||
self.declare_parameter("scale", 1.0)
|
||||
self.declare_parameter("kp_linear", 2.0)
|
||||
self.declare_parameter("deadband_m", 0.002)
|
||||
self.declare_parameter("low_pass_alpha", 0.35)
|
||||
self.declare_parameter("max_linear_speed", 0.05)
|
||||
self.declare_parameter("enable_position_axes", [True, True, True])
|
||||
self.declare_parameter("workspace_min", [0.20, -0.35, 0.10])
|
||||
self.declare_parameter("workspace_max", [0.65, 0.35, 0.60])
|
||||
self.declare_parameter("xr_to_robot_matrix", [0.0, 0.0, -1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0])
|
||||
self.declare_parameter("use_mock", True)
|
||||
self.declare_parameter("mock_initial_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0])
|
||||
self.declare_parameter("robot_ip", "192.168.1.18")
|
||||
self.declare_parameter("robot_port", 8080)
|
||||
self.declare_parameter("avoid_singularity", 1)
|
||||
self.declare_parameter("frame_type", 1)
|
||||
self.declare_parameter("follow", False)
|
||||
|
||||
self._arm_name = str(self.get_parameter("arm_name").value)
|
||||
topic = self.get_parameter("controller_topic").value
|
||||
control_rate_hz = float(self.get_parameter("control_rate_hz").value)
|
||||
self._dt = 1.0 / control_rate_hz
|
||||
self._command_timeout_sec = float(self.get_parameter("command_timeout_sec").value)
|
||||
self._scale = float(self.get_parameter("scale").value)
|
||||
self._kp_linear = float(self.get_parameter("kp_linear").value)
|
||||
self._deadband_m = float(self.get_parameter("deadband_m").value)
|
||||
self._low_pass_alpha = float(self.get_parameter("low_pass_alpha").value)
|
||||
self._max_linear_speed = float(self.get_parameter("max_linear_speed").value)
|
||||
self._enable_position_axes = self._bool_list_parameter("enable_position_axes", 3)
|
||||
self._workspace_min = self._float_list_parameter("workspace_min", 3)
|
||||
self._workspace_max = self._float_list_parameter("workspace_max", 3)
|
||||
self._xr_to_robot_matrix = self._float_list_parameter("xr_to_robot_matrix", 9)
|
||||
self._follow = bool(self.get_parameter("follow").value)
|
||||
self._validate_workspace()
|
||||
|
||||
self._last_msg: XrController | None = None
|
||||
self._last_msg_time = None
|
||||
self._active = False
|
||||
self._controller_start: list[float] | None = None
|
||||
self._robot_start: list[float] | None = None
|
||||
self._filtered_velocity = [0.0, 0.0, 0.0]
|
||||
|
||||
self._adapter = self._make_adapter()
|
||||
self._adapter.connect()
|
||||
|
||||
self.create_subscription(XrController, topic, self._on_controller, 10)
|
||||
self.create_timer(self._dt, self._control_tick)
|
||||
self.get_logger().info(f"{self._arm_name} 速度遥操节点已启动，监听话题：{topic}")
|
||||
|
||||
def _make_adapter(self):
|
||||
if bool(self.get_parameter("use_mock").value):
|
||||
return MockRealManAdapter(
|
||||
[float(v) for v in self.get_parameter("mock_initial_pose").value],
|
||||
self._dt,
|
||||
)
|
||||
|
||||
return RealManAdapter(
|
||||
robot_ip=self.get_parameter("robot_ip").value,
|
||||
robot_port=int(self.get_parameter("robot_port").value),
|
||||
dt=self._dt,
|
||||
avoid_singularity=int(self.get_parameter("avoid_singularity").value),
|
||||
frame_type=int(self.get_parameter("frame_type").value),
|
||||
)
|
||||
|
||||
def _on_controller(self, msg: XrController) -> None:
|
||||
self._last_msg = msg
|
||||
self._last_msg_time = self.get_clock().now()
|
||||
|
||||
def _control_tick(self) -> None:
|
||||
if self._last_msg is None or self._last_msg_time is None:
|
||||
self._safe_stop()
|
||||
return
|
||||
|
||||
age = (self.get_clock().now() - self._last_msg_time).nanoseconds * 1e-9
|
||||
if age > self._command_timeout_sec:
|
||||
self.get_logger().warn(f"{self._arm_name} XR 手柄数据超时，机械臂停止。", throttle_duration_sec=1.0)
|
||||
self._safe_stop()
|
||||
self._active = False
|
||||
return
|
||||
|
||||
if not self._last_msg.grip:
|
||||
if self._active:
|
||||
self.get_logger().info(f"{self._arm_name} Grip 松开，退出相对位移遥操。")
|
||||
self._safe_stop()
|
||||
self._active = False
|
||||
self._controller_start = None
|
||||
self._robot_start = None
|
||||
return
|
||||
|
||||
controller_now = self._controller_xyz(self._last_msg)
|
||||
try:
|
||||
robot_now = self._adapter.get_current_pose().xyz()
|
||||
except Exception as exc:
|
||||
self.get_logger().error(
|
||||
f"{self._arm_name} 读取 TCP 位姿失败，停止输出：{exc}",
|
||||
throttle_duration_sec=1.0,
|
||||
)
|
||||
self._safe_stop()
|
||||
self._active = False
|
||||
return
|
||||
|
||||
if not self._active:
|
||||
# 握持键按下的第一帧只锁定手柄和 TCP 起点，不立即运动，避免启停跳变。
|
||||
self._active = True
|
||||
self._controller_start = controller_now
|
||||
self._robot_start = robot_now
|
||||
self._filtered_velocity = [0.0, 0.0, 0.0]
|
||||
self.get_logger().info(f"{self._arm_name} Grip 按下，已锁定手柄和机械臂初始位姿。")
|
||||
self._safe_stop()
|
||||
return
|
||||
|
||||
assert self._controller_start is not None
|
||||
assert self._robot_start is not None
|
||||
|
||||
# 核心控制：手柄相对位移 -> 机器人坐标系相对位移 -> 目标 TCP -> 速度命令。
|
||||
controller_delta = [controller_now[i] - self._controller_start[i] for i in range(3)]
|
||||
robot_delta = self._map_xr_delta_to_robot(controller_delta)
|
||||
target = [self._robot_start[i] + self._scale * robot_delta[i] for i in range(3)]
|
||||
target = self._clamp_workspace(target)
|
||||
error = [target[i] - robot_now[i] for i in range(3)]
|
||||
error = [error[i] if self._enable_position_axes[i] else 0.0 for i in range(3)]
|
||||
|
||||
if _norm(error) < self._deadband_m:
|
||||
velocity = [0.0, 0.0, 0.0]
|
||||
else:
|
||||
velocity = [self._kp_linear * value for value in error]
|
||||
velocity = self._clamp_vector_norm(velocity, self._max_linear_speed)
|
||||
|
||||
# 低通滤波可以削弱 XR 追踪抖动，真实机械臂测试时不要关掉。
|
||||
alpha = self._low_pass_alpha
|
||||
self._filtered_velocity = [
|
||||
alpha * velocity[i] + (1.0 - alpha) * self._filtered_velocity[i]
|
||||
for i in range(3)
|
||||
]
|
||||
self._send_cartesian_velocity(self._filtered_velocity + [0.0, 0.0, 0.0])
|
||||
|
||||
@staticmethod
|
||||
def _controller_xyz(msg: XrController) -> list[float]:
|
||||
return [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]
|
||||
|
||||
def _map_xr_delta_to_robot(self, delta: list[float]) -> list[float]:
|
||||
matrix = self._xr_to_robot_matrix
|
||||
return [
|
||||
matrix[0] * delta[0] + matrix[1] * delta[1] + matrix[2] * delta[2],
|
||||
matrix[3] * delta[0] + matrix[4] * delta[1] + matrix[5] * delta[2],
|
||||
matrix[6] * delta[0] + matrix[7] * delta[1] + matrix[8] * delta[2],
|
||||
]
|
||||
|
||||
def _clamp_workspace(self, target: list[float]) -> list[float]:
|
||||
return [
|
||||
_clamp(target[i], self._workspace_min[i], self._workspace_max[i])
|
||||
for i in range(3)
|
||||
]
|
||||
|
||||
@staticmethod
|
||||
def _clamp_vector_norm(vector: list[float], max_norm: float) -> list[float]:
|
||||
norm = _norm(vector)
|
||||
if norm <= max_norm or norm <= 1e-9:
|
||||
return vector
|
||||
scale = max_norm / norm
|
||||
return [value * scale for value in vector]
|
||||
|
||||
def _safe_stop(self) -> None:
|
||||
# 所有断连、松手、退出路径都走这里，确保发送零速度。
|
||||
self._filtered_velocity = [0.0, 0.0, 0.0]
|
||||
self._send_cartesian_velocity([0.0] * 6)
|
||||
|
||||
def _send_cartesian_velocity(self, velocity: list[float]) -> None:
|
||||
try:
|
||||
self._adapter.send_cartesian_velocity(velocity, self._follow)
|
||||
except Exception as exc:
|
||||
self.get_logger().error(
|
||||
f"{self._arm_name} 发送速度命令失败：{exc}",
|
||||
throttle_duration_sec=1.0,
|
||||
)
|
||||
self._active = False
|
||||
|
||||
def _float_list_parameter(self, name: str, expected_length: int) -> list[float]:
|
||||
values = [float(value) for value in self.get_parameter(name).value]
|
||||
if len(values) != expected_length:
|
||||
raise ValueError(f"{name} must contain {expected_length} values")
|
||||
return values
|
||||
|
||||
def _bool_list_parameter(self, name: str, expected_length: int) -> list[bool]:
|
||||
values = [bool(value) for value in self.get_parameter(name).value]
|
||||
if len(values) != expected_length:
|
||||
raise ValueError(f"{name} must contain {expected_length} values")
|
||||
return values
|
||||
|
||||
def _validate_workspace(self) -> None:
|
||||
for axis, (low, high) in enumerate(zip(self._workspace_min, self._workspace_max)):
|
||||
if low >= high:
|
||||
raise ValueError(f"workspace_min[{axis}] must be smaller than workspace_max[{axis}]")
|
||||
|
||||
def destroy_node(self) -> bool:
|
||||
self._adapter.close()
|
||||
return super().destroy_node()
|
||||
|
||||
|
||||
def main(args: list[str] | None = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = SingleArmVelocityTeleop()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user