diff --git a/README.md b/README.md index 4b7cb4b..b8e7924 100755 --- a/README.md +++ b/README.md @@ -7,11 +7,12 @@ PICO/XR 双手柄 UDP JSON -> xr_rm_input/udp_controller_receiver -> /xr/left_controller 与 /xr/right_controller -> xr_rm_teleop/single_arm_velocity_teleop - -> 左右 RM75 笛卡尔相对位姿透传控制 + -> Placo QP 单步逆解 + -> 左右 RM75 七关节角透传控制 -> /xr_rm//current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题 ``` -当前控制方式是“手柄相对位姿透传”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP，经过工作空间限幅、目标低通、姿态低通和单帧步长限制后，通过 `rm_movep_canfd` 下发目标位姿。松开 `grip`、UDP 超时或节点退出时会请求机械臂慢停。 +当前控制方式是“手柄相对位姿 + 单步 QP”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP，经过工作空间限幅、目标低通、姿态低通和单帧步长限制后，每个控制周期执行一次 Placo QP，并通过 `rm_movej_canfd` 下发 7 个关节目标。松开 `grip`、UDP 或关节反馈超时、节点退出时会请求机械臂慢停。 ## 当前范围 @@ -19,7 +20,8 @@ PICO/XR 双手柄 UDP JSON - PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。 - 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。 -- RM75 真机连接适配，包含 `rm_movep_canfd` 位姿透传、安全速度/加速度配置、可选初始化点位移动。 +- RM75 真机连接适配，包含关节反馈缓存、`rm_movej_canfd` 关节透传、安全速度/加速度配置、可选初始化点位移动。 +- Placo 0.9.4 RM75 QP 逆解；收到首帧有效关节反馈后才启用，求解失败时保留上一组有效关节目标。 - 真机模式下，点击对应手柄 `trigger` 可切换并保持对应夹爪开/关状态。 - Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。 - 自定义 PICO 4 Ultra UDP Sender Unity 工程，负责发送左右手柄 pose、`grip`、`trigger` 和 pose 诊断字段。 @@ -67,7 +69,9 @@ src/ │ └── msg/ │ └── XrController.msg # hand/grip/trigger/pose └── xr_rm_teleop/ + ├── models/rm75/ # RM75 URDF 与网格 └── xr_rm_teleop/ + ├── placo_ik_solver.py # Placo 0.9.4 单步 QP 逆解 ├── single_arm_velocity_teleop.py ├── realman_adapter.py └── fun_peripheral.py @@ -90,6 +94,17 @@ source install/setup.bash 真机模式还需要安装睿尔曼 Python API2。若未安装，mock 模式仍可正常使用；真机启动时会提示缺少 `Robotic_Arm` 包。 +遥操作节点固定由 `/home/robot/miniconda3/envs/xr/bin/python` 启动，并复用其中的 Python 3.10、Placo 0.9.4、Pinocchio 3.7.0 和 NumPy 2.2.6。`ros2`、`colcon` 和 `udp_controller_receiver` 仍使用系统 Python。禁止通过 `pip --user`、`sudo pip` 或系统安装升级 Placo、Pinocchio、EigenPy 和 NumPy。 + +只读检查 Placo 版本： + +```bash +/home/robot/miniconda3/envs/xr/bin/python -c \ + "import importlib.metadata; print(importlib.metadata.version('placo'))" +``` + +输出必须为 `0.9.4`。 + 如果希望 `launcher_ui.py` 从任意目录找到工作空间，可以设置： ```bash @@ -150,17 +165,25 @@ sudo update-alternatives --config x-terminal-emulator 打开 `launcher_ui.py`，点击 `Check Env`。如果 `install/setup.bash` 缺失，先回工作空间根目录重新执行 `colcon build --symlink-install`。 -第二步：跑 mock 闭环。 +第二步：分别跑左、右臂 mock 闭环。 -在 `Simulation` 模式运行 `One-Click Dual Mock Demo`，或分开运行： +分两个终端依次验证左臂： ```bash -ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true -ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000 \ - --pattern axis_sweep --seconds 60 --both-mode staggered +ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true +ros2 run xr_rm_input sample_udp_sender --hand left --host 127.0.0.1 --port 15000 \ + --pattern axis_sweep --seconds 30 ``` -`sample_udp_sender` 默认使用 `axis_sweep` 成对扫轴轨迹，并在终端打印 `XR +X/-X/+Y/-Y/+Z/-Z` 标签。`--hand both --both-mode staggered --seconds 60` 会先左后右，适合肉眼确认左右臂方向；如果只想左右同时动，可用 `--both-mode synchronized`。需要检查末端姿态时可增加 `--rotation-pattern rpy_steps --rotation-amplitude-deg 25`。 +停止左臂进程后，再分别验证右臂： + +```bash +ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true +ros2 run xr_rm_input sample_udp_sender --hand right --host 127.0.0.1 --port 15000 \ + --pattern axis_sweep --seconds 30 +``` + +`sample_udp_sender` 默认使用 `axis_sweep` 扫轴轨迹，并在终端打印 `XR +X/-X/+Y/-Y/+Z/-Z` 标签。需要检查末端姿态时可增加 `--rotation-pattern rpy_steps --rotation-amplitude-deg 25`。 观察： @@ -182,8 +205,8 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false ``` -单臂真机默认执行配置文件中的 `movej(initial_joint_pose)`；现场需要跳过时，显式传入 -`move_to_initial_pose_on_connect:=false`。 +所有配置默认都不会执行 `movej(initial_joint_pose)`；只有确认安全区清空后，才可显式传入 +`move_to_initial_pose_on_connect:=true`。 第四步：双臂真机。 @@ -216,15 +239,14 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \ - `robot_port`：RM75 TCP 端口，默认 `8080`。 - `left_avoid_singularity` / `right_avoid_singularity`：左右臂避奇异参数，默认左 `0`、右 `1`。 - `avoid_singularity`：非空时覆盖左右臂避奇异参数。 -- `frame_type`：`rm_movep_canfd` 坐标系类型，默认 `1`。 -- `control_rate_hz`：`rm_movep_canfd` 目标位姿发送频率，默认 `90.0`。 -- `follow`：传给 `rm_movep_canfd` 的跟随标志，默认 `false`。 +- `control_rate_hz`：同步关节反馈、执行一次 QP 并发送一次关节目标的频率，默认 `90.0`。 +- `follow`：传给 `rm_movej_canfd` 的跟随标志，默认 `false`。 - `configure_safety_limits`：连接真机后是否配置速度/加速度安全参数，默认 `true`。 - `enable_tool_control`：是否在遥操作节点内启用末端工具控制 topic，默认 `true`。 - `enable_trigger_gripper_control`：是否允许用 `trigger` 点击切换对应夹爪状态，默认 `true`。 - `trigger_close_threshold`：trigger 点击判定阈值，默认 `0.95`。 - `configure_peripheral_on_connect`：遥操作节点连接真机后是否配置末端外设，默认 `true`；工具控制会复用同一个 RealMan 连接，避免两个进程同时抢占同一机械臂。 -- `move_to_initial_pose_on_connect`：连接后是否执行 `movej(initial_joint_pose)`；默认 `auto`，单臂配置启用、双臂配置禁用，也可显式传 `true`/`false` 覆盖。 +- `move_to_initial_pose_on_connect`：连接后是否执行 `movej(initial_joint_pose)`；默认 `auto`，沿用 YAML 中的 `false`，也可显式传 `true`/`false` 覆盖。 ## 配置文件说明 @@ -235,7 +257,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \ `left_arm_rm75.yaml` 和 `right_arm_rm75.yaml` 用于 `arm_debug.launch.py arm:=left/right` 的单臂调试，因为单臂节点名是 `single_arm_velocity_teleop`。 -`xr_rm_bringup/config/peripherals_rm75.yaml` 保存末端工具坐标、负载和左右臂外设选择。当前配置为左臂 `scissorgripper=2`、右臂 `scissorgripper=1`，真机连接阶段会初始化外设，后续开合命令复用同一个 RealMan 连接。 +`xr_rm_bringup/config/peripherals_rm75.yaml` 保存末端工具坐标、负载和左右臂外设选择。当前左臂使用 `minisci`（沿工具局部 Z 偏移 `0.19 m`），右臂使用 `omnipic`（沿工具局部 Z 偏移 `0.16 m`）。URDF 仍只建模到法兰 `link_7`；QP 在 TCP 目标与法兰目标之间应用完整工具刚体变换。真机连接阶段会初始化外设，关节反馈、关节指令、慢停和开合命令复用该单臂节点的同一个 RealMan 连接。 重点控制参数： @@ -250,9 +272,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \ - `workspace_min` / `workspace_max`：笛卡尔工作空间边界。 - `cyl_radius_limit`：基座圆柱半径限制。 - `xr_to_robot_matrix`：`/xr/*_controller` Project 位移到 RM75 base 坐标的映射矩阵。 -- `current_pose_poll_hz`：低频读取真机当前 TCP 的频率；控制中不再每帧阻塞读取状态。 -- `mock_initial_pose`：mock 模式初始 TCP 位姿。 -- `initial_joint_pose`：可选真机初始关节角。 +- `initial_joint_pose`：mock 的初始关节反馈，以及显式开启初始化移动时的真机初始关节角。 当前 `/xr/*_controller` 的 Project 坐标约定： diff --git a/xr_rm_bringup/config/dual_arm_rm75.yaml b/xr_rm_bringup/config/dual_arm_rm75.yaml index 634c7b2..59c9379 100755 --- a/xr_rm_bringup/config/dual_arm_rm75.yaml +++ b/xr_rm_bringup/config/dual_arm_rm75.yaml @@ -1,10 +1,10 @@ # 阶段一：PICO 遥操作双 RM75 平台配置。 # -# 当前控制方式是“相对位姿透传”： +# 当前控制方式是“相对 TCP + 单步 QP”： # 按下 grip 时锁定当前手柄位姿和 TCP 位姿，之后将手柄相对位移和相对旋转 # 映射为目标 TCP 位姿，经过工作空间限幅、目标低通、姿态低通和单帧步长 -# 限制后，通过 rm_movep_canfd 下发。cmd_vel 仅作为目标位姿变化率调试话题， -# 不是机械臂执行命令。 +# 限制后，通过 Placo 单步 QP 和 rm_movej_canfd 下发 7 个关节目标。 +# cmd_vel 仅作为目标位姿变化率调试话题，不是机械臂执行命令。 # 末端外设由 peripherals_rm75.yaml 配置，真机连接阶段初始化后由遥操作节点复用。 left_arm_teleop: @@ -27,8 +27,6 @@ left_arm_teleop: orientation_deadband_rad: 0.005 orientation_filter_alpha: 0.65 max_orientation_speed: 0.6 - current_pose_poll_hz: 10.0 - workspace_min: [-0.70, -0.60, 0.10] workspace_max: [0.70, 0.40, 0.70] cyl_radius_limit: [0.20, 0.60] @@ -42,11 +40,9 @@ left_arm_teleop: -1.0, 0.0, 0.0] use_mock: false - mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400] robot_ip: 192.168.192.18 robot_port: 8080 avoid_singularity: 0 - frame_type: 1 follow: false canfd_trajectory_mode: 2 canfd_radio: 0 @@ -81,8 +77,6 @@ right_arm_teleop: orientation_deadband_rad: 0.005 orientation_filter_alpha: 0.65 max_orientation_speed: 0.6 - current_pose_poll_hz: 10.0 - workspace_min: [-0.70, -0.60, 0.10] workspace_max: [0.70, 0.40, 0.70] cyl_radius_limit: [0.20, 0.60] @@ -96,11 +90,9 @@ right_arm_teleop: 1.0, 0.0, 0.0] use_mock: false - mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910] robot_ip: 192.168.192.19 robot_port: 8080 avoid_singularity: 1 - frame_type: 1 follow: false canfd_trajectory_mode: 2 canfd_radio: 0 diff --git a/xr_rm_bringup/config/left_arm_rm75.yaml b/xr_rm_bringup/config/left_arm_rm75.yaml index bbe721b..5705fcf 100644 --- a/xr_rm_bringup/config/left_arm_rm75.yaml +++ b/xr_rm_bringup/config/left_arm_rm75.yaml @@ -1,4 +1,4 @@ -# 左臂单独调试配置：XR 相对位姿透传控制 RM75 TCP。 +# 左臂单独调试配置：XR TCP 目标经 Placo QP 转换为 RM75 关节目标。 # 末端外设由 peripherals_rm75.yaml 配置，真机连接阶段初始化后由遥操作节点复用。 single_arm_velocity_teleop: @@ -21,8 +21,6 @@ single_arm_velocity_teleop: orientation_deadband_rad: 0.005 orientation_filter_alpha: 0.65 max_orientation_speed: 0.6 - current_pose_poll_hz: 10.0 - workspace_min: [-0.70, -0.60, 0.10] workspace_max: [0.70, 0.40, 0.70] cyl_radius_limit: [0.20, 0.60] @@ -35,11 +33,9 @@ single_arm_velocity_teleop: -1.0, 0.0, 0.0] use_mock: false - mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400] robot_ip: 192.168.192.18 robot_port: 8080 avoid_singularity: 0 - frame_type: 1 follow: false canfd_trajectory_mode: 2 canfd_radio: 0 @@ -50,7 +46,7 @@ single_arm_velocity_teleop: max_angular_acc: 2.0 joint_max_speed: 180.0 joint_max_acc: 180.0 - move_to_initial_pose_on_connect: true + move_to_initial_pose_on_connect: false initial_joint_pose: [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52] init_move_speed: 20 debug_topic_prefix: /xr_rm diff --git a/xr_rm_bringup/config/right_arm_rm75.yaml b/xr_rm_bringup/config/right_arm_rm75.yaml index b088d18..0acefa5 100644 --- a/xr_rm_bringup/config/right_arm_rm75.yaml +++ b/xr_rm_bringup/config/right_arm_rm75.yaml @@ -1,4 +1,4 @@ -# 右臂单独调试配置：XR 相对位姿透传控制 RM75 TCP。 +# 右臂单独调试配置：XR TCP 目标经 Placo QP 转换为 RM75 关节目标。 # 末端外设由 peripherals_rm75.yaml 配置，真机连接阶段初始化后由遥操作节点复用。 single_arm_velocity_teleop: @@ -20,8 +20,6 @@ single_arm_velocity_teleop: orientation_deadband_rad: 0.005 orientation_filter_alpha: 0.65 max_orientation_speed: 0.5 - current_pose_poll_hz: 10.0 - workspace_min: [-0.60, -0.60, 0.10] workspace_max: [0.60, 0.70, 0.55] cyl_radius_limit: [0.10, 0.70] @@ -34,22 +32,20 @@ single_arm_velocity_teleop: 1.0, 0.0, 0.0] use_mock: false - mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910] robot_ip: 192.168.192.19 robot_port: 8080 avoid_singularity: 1 - frame_type: 1 follow: false canfd_trajectory_mode: 2 canfd_radio: 0 configure_safety_limits: true - max_line_speed: 1.0 - max_angular_speed: 1.5 - max_line_acc: 1.0 - max_angular_acc: 2.0 + max_line_speed: 0.25 + max_angular_speed: 0.6 + max_line_acc: 1.3 + max_angular_acc: 3.0 joint_max_speed: 180.0 - joint_max_acc: 180.0 - move_to_initial_pose_on_connect: true + joint_max_acc: 300.0 + move_to_initial_pose_on_connect: True initial_joint_pose: [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04] init_move_speed: 20 debug_topic_prefix: /xr_rm diff --git a/xr_rm_bringup/launch/arm_debug.launch.py b/xr_rm_bringup/launch/arm_debug.launch.py index b2d2483..1b40d0c 100644 --- a/xr_rm_bringup/launch/arm_debug.launch.py +++ b/xr_rm_bringup/launch/arm_debug.launch.py @@ -5,6 +5,8 @@ 手柄接收节点，再根据 `arm:=left|right|both` 选择对应的遥操作节点。 """ +from pathlib import Path + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.substitutions import LaunchConfiguration, PathJoinSubstitution @@ -12,6 +14,9 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +XR_PYTHON = "/home/robot/miniconda3/envs/xr/bin/python" + + def _as_bool(value: str) -> bool: """把 launch 字符串参数转换成 Python bool，便于在 OpaqueFunction 中分支。""" return value.strip().lower() in ("1", "true", "yes", "on") @@ -26,6 +31,15 @@ def _config_file(name: str) -> PathJoinSubstitution: ]) +def _rm75_urdf() -> PathJoinSubstitution: + return PathJoinSubstitution([ + FindPackageShare("xr_rm_teleop"), + "models", + "rm75", + "RM75-B.urdf", + ]) + + def _initial_pose_override(value: str) -> dict[str, bool]: return {} if value == "auto" else {"move_to_initial_pose_on_connect": _as_bool(value)} @@ -52,7 +66,6 @@ def _single_arm_node( use_mock: bool, move_to_initial_pose: str, avoid_singularity: int, - frame_type: int, control_rate_hz: float, follow: bool, configure_safety_limits: bool, @@ -70,6 +83,7 @@ def _single_arm_node( executable="single_arm_velocity_teleop", name="single_arm_velocity_teleop", output="screen", + prefix=[XR_PYTHON], parameters=[ _config_file(config_name), { @@ -77,7 +91,7 @@ def _single_arm_node( "robot_ip": robot_ip, "robot_port": LaunchConfiguration("robot_port"), "avoid_singularity": avoid_singularity, - "frame_type": frame_type, + "robot_urdf_path": _rm75_urdf(), "control_rate_hz": control_rate_hz, "follow": follow, "configure_safety_limits": configure_safety_limits, @@ -103,7 +117,6 @@ def _dual_arm_nodes( move_to_initial_pose: str, left_avoid_singularity: int, right_avoid_singularity: int, - frame_type: int, control_rate_hz: float, follow: bool, configure_safety_limits: bool, @@ -120,6 +133,7 @@ def _dual_arm_nodes( executable="single_arm_velocity_teleop", name="left_arm_teleop", output="screen", + prefix=[XR_PYTHON], parameters=[ config_file, { @@ -127,7 +141,7 @@ def _dual_arm_nodes( "robot_ip": LaunchConfiguration("left_robot_ip"), "robot_port": LaunchConfiguration("robot_port"), "avoid_singularity": left_avoid_singularity, - "frame_type": frame_type, + "robot_urdf_path": _rm75_urdf(), "control_rate_hz": control_rate_hz, "follow": follow, "configure_safety_limits": configure_safety_limits, @@ -147,6 +161,7 @@ def _dual_arm_nodes( executable="single_arm_velocity_teleop", name="right_arm_teleop", output="screen", + prefix=[XR_PYTHON], parameters=[ config_file, { @@ -154,7 +169,7 @@ def _dual_arm_nodes( "robot_ip": LaunchConfiguration("right_robot_ip"), "robot_port": LaunchConfiguration("robot_port"), "avoid_singularity": right_avoid_singularity, - "frame_type": frame_type, + "robot_urdf_path": _rm75_urdf(), "control_rate_hz": control_rate_hz, "follow": follow, "configure_safety_limits": configure_safety_limits, @@ -175,6 +190,11 @@ def _dual_arm_nodes( def _launch_setup(context, *args, **kwargs): """运行时读取 launch 参数，决定启动单臂还是双臂。""" del args, kwargs + if not Path(XR_PYTHON).is_file(): + raise RuntimeError( + f"XR Python not found: {XR_PYTHON}; " + "Placo 0.9.4 must not be installed globally" + ) arm = LaunchConfiguration("arm").perform(context).strip().lower() use_mock = _as_bool(LaunchConfiguration("use_mock").perform(context)) move_to_initial_pose = LaunchConfiguration( @@ -187,7 +207,6 @@ def _launch_setup(context, *args, **kwargs): right_avoid_singularity = int( avoid_override or LaunchConfiguration("right_avoid_singularity").perform(context) ) - frame_type = int(LaunchConfiguration("frame_type").perform(context)) control_rate_hz = float(LaunchConfiguration("control_rate_hz").perform(context)) follow = _as_bool(LaunchConfiguration("follow").perform(context)) configure_safety_limits = _as_bool( @@ -217,7 +236,6 @@ def _launch_setup(context, *args, **kwargs): move_to_initial_pose, left_avoid_singularity, right_avoid_singularity, - frame_type, control_rate_hz, follow, configure_safety_limits, @@ -235,7 +253,6 @@ def _launch_setup(context, *args, **kwargs): use_mock, move_to_initial_pose, avoid_singularity, - frame_type, control_rate_hz, follow, configure_safety_limits, @@ -268,8 +285,7 @@ def generate_launch_description() -> LaunchDescription: DeclareLaunchArgument("right_avoid_singularity", default_value="1"), # 非空时作为左右臂全局覆盖，例如 avoid_singularity:=0。 DeclareLaunchArgument("avoid_singularity", default_value=""), - DeclareLaunchArgument("frame_type", default_value="1"), - # 现场调参入口：默认按 PICO 90Hz 输入节奏发送 rm_movep_canfd。 + # 每周期同步一次实际关节反馈、执行一次 Placo QP，再发送 rm_movej_canfd。 DeclareLaunchArgument("control_rate_hz", default_value="90.0"), # 默认低跟随；高跟随请确认控制器和网络能稳定满足厂商周期要求后再打开。 DeclareLaunchArgument("follow", default_value="false"), diff --git a/xr_rm_teleop/models/rm75/RM75-B.urdf b/xr_rm_teleop/models/rm75/RM75-B.urdf new file mode 100644 index 0000000..6ecc903 --- /dev/null +++ b/xr_rm_teleop/models/rm75/RM75-B.urdf @@ -0,0 +1,453 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/xr_rm_teleop/models/rm75/meshes/base_link.STL b/xr_rm_teleop/models/rm75/meshes/base_link.STL new file mode 100644 index 0000000..d5a35d5 Binary files /dev/null and b/xr_rm_teleop/models/rm75/meshes/base_link.STL differ diff --git a/xr_rm_teleop/models/rm75/meshes/link_1.STL b/xr_rm_teleop/models/rm75/meshes/link_1.STL new file mode 100644 index 0000000..8b2bc5f Binary files /dev/null and b/xr_rm_teleop/models/rm75/meshes/link_1.STL differ diff --git a/xr_rm_teleop/models/rm75/meshes/link_2.STL b/xr_rm_teleop/models/rm75/meshes/link_2.STL new file mode 100644 index 0000000..83d21b3 Binary files /dev/null and b/xr_rm_teleop/models/rm75/meshes/link_2.STL differ diff --git a/xr_rm_teleop/models/rm75/meshes/link_3.STL b/xr_rm_teleop/models/rm75/meshes/link_3.STL new file mode 100644 index 0000000..5f6baba Binary files /dev/null and b/xr_rm_teleop/models/rm75/meshes/link_3.STL differ diff --git a/xr_rm_teleop/models/rm75/meshes/link_4.STL b/xr_rm_teleop/models/rm75/meshes/link_4.STL new file mode 100644 index 0000000..949dc0f Binary files /dev/null and b/xr_rm_teleop/models/rm75/meshes/link_4.STL differ diff --git a/xr_rm_teleop/models/rm75/meshes/link_5.STL b/xr_rm_teleop/models/rm75/meshes/link_5.STL new file mode 100644 index 0000000..bfaf4f9 Binary files /dev/null and b/xr_rm_teleop/models/rm75/meshes/link_5.STL differ diff --git a/xr_rm_teleop/models/rm75/meshes/link_6.STL b/xr_rm_teleop/models/rm75/meshes/link_6.STL new file mode 100644 index 0000000..3ed9d50 Binary files /dev/null and b/xr_rm_teleop/models/rm75/meshes/link_6.STL differ diff --git a/xr_rm_teleop/models/rm75/meshes/link_7.STL b/xr_rm_teleop/models/rm75/meshes/link_7.STL new file mode 100644 index 0000000..412c292 Binary files /dev/null and b/xr_rm_teleop/models/rm75/meshes/link_7.STL differ diff --git a/xr_rm_teleop/setup.py b/xr_rm_teleop/setup.py index 4358e7b..e981e1f 100755 --- a/xr_rm_teleop/setup.py +++ b/xr_rm_teleop/setup.py @@ -1,8 +1,10 @@ """xr_rm_teleop 包安装配置。 -该包提供基于 XR 相对位姿的 RM75 笛卡尔位姿透传遥操作节点。 +该包提供基于 XR 相对位姿和 Placo QP 的 RM75 遥操作节点。 """ +from glob import glob + from setuptools import setup package_name = "xr_rm_teleop" @@ -14,6 +16,14 @@ setup( data_files=[ ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), (f"share/{package_name}", ["package.xml"]), + ( + f"share/{package_name}/models/rm75", + ["models/rm75/RM75-B.urdf"], + ), + ( + f"share/{package_name}/models/rm75/meshes", + glob("models/rm75/meshes/*.STL"), + ), ], install_requires=["setuptools"], zip_safe=True, diff --git a/xr_rm_teleop/test/placo_ik_smoke.py b/xr_rm_teleop/test/placo_ik_smoke.py new file mode 100644 index 0000000..0e8a6f0 --- /dev/null +++ b/xr_rm_teleop/test/placo_ik_smoke.py @@ -0,0 +1,79 @@ +from __future__ import annotations + +import math +import sys +import time +from pathlib import Path + +import numpy as np + +from xr_rm_teleop.placo_ik_solver import PlacoIkSolver +from xr_rm_teleop.realman_adapter import ArmPose + + +CASES = { + "left": ( + [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52], + [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0], + ), + "right": ( + [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04], + [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0], + ), +} + + +def angle_error(actual: list[float], target: list[float]) -> float: + deltas = [ + math.atan2(math.sin(a - b), math.cos(a - b)) + for a, b in zip(actual, target) + ] + return math.sqrt(sum(value * value for value in deltas)) + + +def main() -> None: + urdf_path = Path(sys.argv[1]).resolve() + for arm, (joint_degrees, tool_pose) in CASES.items(): + solver = PlacoIkSolver(str(urdf_path), tool_pose, 1.0 / 90.0) + joints = np.deg2rad(joint_degrees).tolist() + current = solver.update_joint_state(joints) + target = ArmPose( + current.x + 0.01, + current.y, + current.z, + current.rx, + current.ry, + current.rz + 0.05, + ) + + solve_durations = [] + for _ in range(45): + solver.update_joint_state(joints) + started_at = time.perf_counter() + joints = solver.solve(target) + solve_durations.append(time.perf_counter() - started_at) + + actual = solver.update_joint_state(joints) + position_error = np.linalg.norm( + np.asarray(actual.xyz()) - np.asarray(target.xyz()) + ) + orientation_error = angle_error(actual.rpy(), target.rpy()) + assert len(joints) == 7 + assert np.isfinite(joints).all() + assert np.allclose( + solver.base_configuration, + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], + ) + assert position_error <= 0.005 + assert orientation_error <= math.radians(2.0) + print( + f"{arm}: position_error={position_error:.6f}m, " + f"orientation_error={math.degrees(orientation_error):.3f}deg, " + f"solve_avg={1000.0 * np.mean(solve_durations):.3f}ms, " + f"solve_max={1000.0 * max(solve_durations):.3f}ms, " + f"solve_overruns={sum(value > 1.0 / 90.0 for value in solve_durations)}" + ) + + +if __name__ == "__main__": + main() diff --git a/xr_rm_teleop/test/test_initial_joint_pose.py b/xr_rm_teleop/test/test_initial_joint_pose.py index 8355108..a80c927 100644 --- a/xr_rm_teleop/test/test_initial_joint_pose.py +++ b/xr_rm_teleop/test/test_initial_joint_pose.py @@ -1,4 +1,10 @@ +import math + +import pytest + from xr_rm_teleop.realman_adapter import RealManAdapter +from xr_rm_teleop.realman_adapter import MockRealManAdapter +from xr_rm_teleop.fun_peripheral import PeripheralConfig def test_initial_pose_uses_joint_move_only() -> None: @@ -17,3 +23,66 @@ def test_initial_pose_uses_joint_move_only() -> None: adapter._move_to_initial_pose() assert adapter._arm.calls == [(joints, 20, 0, 0, 1)] + + +def test_peripheral_config_exposes_selected_tool() -> None: + config = PeripheralConfig( + scissorgripper=1, + tools_in_ee={ + "first": [[0.0] * 7, [0.0] * 7], + "second": [[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0], [0.0] * 7], + }, + ) + + assert config.tool_name == "second" + assert config.tool_pose == [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0] + + +def test_joint_feedback_is_cached_in_radians() -> None: + class FakeArm: + def rm_get_joint_degree(self): + return 0, [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0] + + adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01) + adapter._arm = FakeArm() + + adapter._read_joint_state_once() + snapshot = adapter.get_latest_joint_state() + + assert snapshot is not None + assert snapshot.positions == pytest.approx( + [math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]] + ) + + +def test_joint_target_uses_movej_canfd_in_degrees() -> None: + class FakeArm: + def __init__(self) -> None: + self.calls = [] + + def rm_movej_canfd(self, *args): + self.calls.append(args) + return 0 + + adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01) + adapter._arm = FakeArm() + target = [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]] + + adapter.send_joint_target(target, follow=False) + + assert len(adapter._arm.calls) == 1 + degrees, follow, expand, trajectory_mode, radio = adapter._arm.calls[0] + assert degrees == pytest.approx([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0]) + assert (follow, expand, trajectory_mode, radio) == (False, 0, 2, 0) + + +def test_mock_joint_feedback_is_available_without_vendor_sdk() -> None: + adapter = MockRealManAdapter([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0]) + + adapter.connect() + snapshot = adapter.get_latest_joint_state() + + assert snapshot is not None + assert snapshot.positions == pytest.approx( + [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]] + ) diff --git a/xr_rm_teleop/test/test_joint_control.py b/xr_rm_teleop/test/test_joint_control.py new file mode 100644 index 0000000..58d19ee --- /dev/null +++ b/xr_rm_teleop/test/test_joint_control.py @@ -0,0 +1,164 @@ +import time +from types import SimpleNamespace + +import pytest + +from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot +from xr_rm_teleop.single_arm_velocity_teleop import SingleArmVelocityTeleop + + +class FakeLogger: + def warn(self, *args, **kwargs): + del args, kwargs + + def error(self, *args, **kwargs): + del args, kwargs + + +class FakeTime: + def __sub__(self, other): + del other + return SimpleNamespace(nanoseconds=0) + + +def test_missing_or_stale_feedback_does_not_enable_qp() -> None: + teleop = object.__new__(SingleArmVelocityTeleop) + teleop._command_timeout_sec = 0.12 + teleop._adapter = SimpleNamespace(get_latest_joint_state=lambda: None) + + assert teleop._fresh_joint_state() is None + + teleop._adapter = SimpleNamespace( + get_latest_joint_state=lambda: JointStateSnapshot( + [0.0] * 7, + time.monotonic() - 1.0, + ) + ) + assert teleop._fresh_joint_state() is None + + +def test_stale_feedback_stops_before_active_control() -> None: + stopped = [] + entered = [] + teleop = object.__new__(SingleArmVelocityTeleop) + teleop._adapter = SimpleNamespace( + get_latest_joint_state=lambda: JointStateSnapshot( + [0.0] * 7, + time.monotonic() - 1.0, + ) + ) + teleop._command_timeout_sec = 0.12 + teleop._joint_feedback_ready = True + teleop._arm_name = "right_rm75" + teleop._last_msg = SimpleNamespace( + grip=True, + pose=SimpleNamespace( + position=SimpleNamespace(x=0.0, y=0.0, z=0.0), + orientation=SimpleNamespace(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ) + teleop._last_msg_time = FakeTime() + teleop._active = False + teleop._enable_orientation_control = False + teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime()) + teleop.get_logger = lambda: FakeLogger() + teleop._safe_stop = lambda reset_active: stopped.append(reset_active) + teleop._enter_active_control = lambda *args: entered.append(args) + + teleop._control_tick() + + assert stopped == [True] + assert entered == [] + + +def test_first_feedback_initializes_last_valid_target_without_solving() -> None: + class FakeSolver: + def __init__(self) -> None: + self.solve_calls = 0 + + def update_joint_state(self, joints): + assert joints == [0.1] * 7 + return ArmPose(0.3, 0.0, 0.2) + + def solve(self, target): + del target + self.solve_calls += 1 + return [0.2] * 7 + + teleop = object.__new__(SingleArmVelocityTeleop) + teleop._ik_solver = FakeSolver() + teleop._active = False + teleop._last_valid_joint_target = None + teleop._last_current_pose = None + + pose = teleop._sync_joint_feedback( + JointStateSnapshot([0.1] * 7, time.monotonic()) + ) + + assert pose == ArmPose(0.3, 0.0, 0.2) + assert teleop._last_valid_joint_target == [0.1] * 7 + assert teleop._ik_solver.solve_calls == 0 + + +def test_qp_failure_returns_last_known_good_target() -> None: + class FailingSolver: + def solve(self, target): + del target + raise RuntimeError("NaN in QP solution") + + teleop = object.__new__(SingleArmVelocityTeleop) + teleop._ik_solver = FailingSolver() + teleop._last_valid_joint_target = [0.1] * 7 + teleop._arm_name = "right_rm75" + teleop.get_logger = lambda: FakeLogger() + + target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2)) + + assert target == pytest.approx([0.1] * 7) + assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7) + + +def test_qp_success_updates_last_known_good_target() -> None: + class SuccessfulSolver: + def solve(self, target): + del target + return [0.2] * 7 + + teleop = object.__new__(SingleArmVelocityTeleop) + teleop._ik_solver = SuccessfulSolver() + teleop._last_valid_joint_target = [0.1] * 7 + teleop._arm_name = "left_rm75" + teleop.get_logger = lambda: FakeLogger() + + target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2)) + + assert target == pytest.approx([0.2] * 7) + assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7) + + +def test_joint_send_failure_requests_slow_stop_and_resets_control() -> None: + class FailingAdapter: + def __init__(self) -> None: + self.stop_calls = 0 + + def send_joint_target(self, joints, follow): + del joints, follow + raise RuntimeError("send failed") + + def stop(self): + self.stop_calls += 1 + + reset_calls = [] + teleop = object.__new__(SingleArmVelocityTeleop) + teleop._adapter = FailingAdapter() + teleop._follow = False + teleop._arm_name = "left_rm75" + teleop._stop_sent = False + teleop.get_logger = lambda: FakeLogger() + teleop._safe_stop = lambda reset_active: reset_calls.append(reset_active) + + sent = teleop._send_joint_target([0.1] * 7) + + assert not sent + assert teleop._adapter.stop_calls == 1 + assert reset_calls == [True] diff --git a/xr_rm_teleop/test/test_orientation_control.py b/xr_rm_teleop/test/test_orientation_control.py index 9fb3bc9..4e10409 100644 --- a/xr_rm_teleop/test/test_orientation_control.py +++ b/xr_rm_teleop/test/test_orientation_control.py @@ -1,9 +1,10 @@ import math +import time from types import SimpleNamespace import pytest -from xr_rm_teleop.realman_adapter import ArmPose, MockRealManAdapter +from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot from xr_rm_teleop.single_arm_velocity_teleop import ( SingleArmVelocityTeleop, _euler_to_quaternion, @@ -95,6 +96,19 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None: teleop._arm_name = "test_rm75" teleop._command_timeout_sec = 0.12 teleop._enable_orientation_control = True + teleop._adapter = SimpleNamespace( + get_latest_joint_state=lambda: JointStateSnapshot( + [0.1] * 7, + time.monotonic(), + ) + ) + teleop._ik_solver = SimpleNamespace( + update_joint_state=lambda joints: ArmPose(0.3, 0.0, 0.2) + ) + teleop._active = False + teleop._last_valid_joint_target = None + teleop._last_current_pose = None + teleop._joint_feedback_ready = True stopped = [] teleop.get_clock = lambda: FakeClock() teleop.get_logger = lambda: FakeLogger() @@ -113,12 +127,3 @@ def test_quaternion_roundtrip_for_small_rpy() -> None: def test_zero_quaternion_is_invalid() -> None: with pytest.raises(ValueError): _normalize_quaternion([0.0, 0.0, 0.0, 0.0]) - - -def test_mock_adapter_uses_shortest_angular_velocity() -> None: - adapter = MockRealManAdapter([0.0, 0.0, 0.0, 3.13, 0.0, -3.13], 0.1) - - adapter.send_cartesian_target(ArmPose(0.0, 0.0, 0.0, -3.13, 0.0, 3.13), False) - - assert abs(adapter.last_velocity[3]) < 1.0 - assert abs(adapter.last_velocity[5]) < 1.0 diff --git a/xr_rm_teleop/test/test_placo_transforms.py b/xr_rm_teleop/test/test_placo_transforms.py new file mode 100644 index 0000000..ed75908 --- /dev/null +++ b/xr_rm_teleop/test/test_placo_transforms.py @@ -0,0 +1,49 @@ +import math + +import numpy as np +import pytest + +from xr_rm_teleop.placo_ik_solver import ( + PlacoIkSolver, + _arm_pose_to_transform, + _tool_pose_to_transform, + _transform_to_arm_pose, +) +from xr_rm_teleop.realman_adapter import ArmPose + + +def test_tool_offset_rotates_with_flange_and_roundtrips() -> None: + flange_pose = ArmPose(0.30, -0.10, 0.20, 0.0, math.pi / 2.0, 0.0) + tool_pose = [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0] + + base_to_flange = _arm_pose_to_transform(flange_pose) + flange_to_tool = _tool_pose_to_transform(tool_pose) + base_to_tool = base_to_flange @ flange_to_tool + recovered_flange = base_to_tool @ np.linalg.inv(flange_to_tool) + + assert base_to_tool[:3, 3] == pytest.approx([0.49, -0.10, 0.20]) + assert recovered_flange == pytest.approx(base_to_flange) + + +def test_transform_to_arm_pose_roundtrip() -> None: + expected = ArmPose(0.25, -0.30, 0.40, 0.20, -0.30, 0.40) + + actual = _transform_to_arm_pose(_arm_pose_to_transform(expected)) + + assert actual.xyz() == pytest.approx(expected.xyz()) + assert actual.rpy() == pytest.approx(expected.rpy()) + + +def test_qp_result_rejects_nan_position_and_velocity_violations() -> None: + solver = object.__new__(PlacoIkSolver) + solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7) + solver._velocity_limits = np.ones(7) + solver._dt = 0.1 + solver._actual_joints = np.zeros(7) + + with pytest.raises(ValueError, match="finite"): + solver._validate_result(np.full(7, np.nan)) + with pytest.raises(ValueError, match="position"): + solver._validate_result(np.full(7, 2.0)) + with pytest.raises(ValueError, match="velocity"): + solver._validate_result(np.full(7, 0.2)) diff --git a/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py b/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py index e752ff1..a56434b 100644 --- a/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py +++ b/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py @@ -25,6 +25,14 @@ class PeripheralConfig: tools_in_ee: dict[str, list[list[float]]] set_initial_tool_state: bool = False + @property + def tool_name(self) -> str: + return list(self.tools_in_ee)[self.scissorgripper] + + @property + def tool_pose(self) -> list[float]: + return list(self.tools_in_ee[self.tool_name][0]) + def load_peripheral_config(config_file: str, arm: str) -> PeripheralConfig: """从 bringup YAML 读取指定左右臂的外设配置。""" diff --git a/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py b/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py new file mode 100644 index 0000000..e66bf03 --- /dev/null +++ b/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py @@ -0,0 +1,209 @@ +"""RM75 的 Placo 0.9.4 单步 QP 逆解。""" + +from __future__ import annotations + +import math +from importlib.metadata import PackageNotFoundError, version +from pathlib import Path + +import numpy as np + +from .realman_adapter import ArmPose + + +EXPECTED_PLACO_VERSION = "0.9.4" +RM75_JOINT_NAMES = [f"joint_{index}" for index in range(1, 8)] +RM75_Q_SLICE = slice(7, 14) + + +def _rpy_to_rotation(roll: float, pitch: float, yaw: float) -> np.ndarray: + cr, sr = math.cos(roll), math.sin(roll) + cp, sp = math.cos(pitch), math.sin(pitch) + cy, sy = math.cos(yaw), math.sin(yaw) + return np.array( + [ + [cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr], + [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr], + [-sp, cp * sr, cp * cr], + ], + dtype=float, + ) + + +def _rotation_to_rpy(rotation: np.ndarray) -> tuple[float, float, float]: + pitch = math.asin(-float(np.clip(rotation[2, 0], -1.0, 1.0))) + if abs(math.cos(pitch)) > 1e-9: + roll = math.atan2(float(rotation[2, 1]), float(rotation[2, 2])) + yaw = math.atan2(float(rotation[1, 0]), float(rotation[0, 0])) + else: + roll = math.atan2(-float(rotation[1, 2]), float(rotation[1, 1])) + yaw = 0.0 + return roll, pitch, yaw + + +def _arm_pose_to_transform(pose: ArmPose) -> np.ndarray: + transform = np.eye(4) + transform[:3, :3] = _rpy_to_rotation(pose.rx, pose.ry, pose.rz) + transform[:3, 3] = pose.xyz() + return transform + + +def _tool_pose_to_transform(tool_pose: list[float]) -> np.ndarray: + values = np.asarray(tool_pose, dtype=float) + if values.shape != (7,) or not np.isfinite(values).all(): + raise ValueError("tool pose must contain 7 finite values") + x, y, z, qx, qy, qz, qw = values + norm = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw) + if norm <= 1e-9: + raise ValueError("tool quaternion norm must be positive") + qx, qy, qz, qw = qx / norm, qy / norm, qz / norm, qw / norm + transform = np.eye(4) + transform[:3, :3] = np.array( + [ + [ + 1 - 2 * (qy * qy + qz * qz), + 2 * (qx * qy - qz * qw), + 2 * (qx * qz + qy * qw), + ], + [ + 2 * (qx * qy + qz * qw), + 1 - 2 * (qx * qx + qz * qz), + 2 * (qy * qz - qx * qw), + ], + [ + 2 * (qx * qz - qy * qw), + 2 * (qy * qz + qx * qw), + 1 - 2 * (qx * qx + qy * qy), + ], + ] + ) + transform[:3, 3] = [x, y, z] + return transform + + +def _transform_to_arm_pose(transform: np.ndarray) -> ArmPose: + roll, pitch, yaw = _rotation_to_rpy(transform[:3, :3]) + return ArmPose( + float(transform[0, 3]), + float(transform[1, 3]), + float(transform[2, 3]), + roll, + pitch, + yaw, + ) + + +class PlacoIkSolver: + def __init__( + self, + urdf_path: str, + tool_pose: list[float], + dt: float, + ) -> None: + if dt <= 0.0: + raise ValueError("dt must be positive") + try: + installed_version = version("placo") + import placo + except (ImportError, PackageNotFoundError) as exc: + raise RuntimeError( + "Placo 0.9.4 must come from " + "/home/robot/miniconda3/envs/xr" + ) from exc + if installed_version != EXPECTED_PLACO_VERSION: + raise RuntimeError( + f"Placo {EXPECTED_PLACO_VERSION} is required, got {installed_version}" + ) + + model_path = Path(urdf_path).expanduser().resolve() + if not model_path.is_file(): + raise FileNotFoundError(f"RM75 URDF not found: {model_path}") + + self._dt = dt + self._robot = placo.RobotWrapper(str(model_path)) + if self._robot.state.q.shape != (14,): + raise RuntimeError( + f"expected Placo q shape (14,), got {self._robot.state.q.shape}" + ) + if list(self._robot.joint_names()) != RM75_JOINT_NAMES: + raise RuntimeError( + f"unexpected RM75 joint order: {list(self._robot.joint_names())}" + ) + offsets = [ + self._robot.get_joint_offset(name) for name in RM75_JOINT_NAMES + ] + if offsets != list(range(7, 14)): + raise RuntimeError(f"unexpected RM75 q offsets: {offsets}") + + self._joint_limits = np.asarray( + [self._robot.get_joint_limits(name) for name in RM75_JOINT_NAMES] + ) + velocity_offsets = [ + self._robot.get_joint_v_offset(name) for name in RM75_JOINT_NAMES + ] + self._velocity_limits = np.asarray( + [ + self._robot.model.velocityLimit[index] + for index in velocity_offsets + ] + ) + self._tool_transform = _tool_pose_to_transform(tool_pose) + self._tool_inverse = np.linalg.inv(self._tool_transform) + self._actual_joints: np.ndarray | None = None + + self._solver = placo.KinematicsSolver(self._robot) + self._solver.dt = dt + self._solver.mask_fbase(True) + self._solver.enable_velocity_limits(True) + self._frame_task = self._solver.add_frame_task("link_7", np.eye(4)) + self._frame_task.configure("rm75_frame", "soft", 1.0) + manipulability = self._solver.add_manipulability_task( + "link_7", + "both", + 1.0, + ) + manipulability.configure("rm75_manipulability", "soft", 5e-2) + self._solver.add_kinetic_energy_regularization_task(1e-6) + + @property + def base_configuration(self) -> list[float]: + return self._robot.state.q[:7].tolist() + + def update_joint_state(self, joints: list[float]) -> ArmPose: + values = np.asarray(joints, dtype=float) + if values.shape != (7,) or not np.isfinite(values).all(): + raise ValueError("joint state must contain 7 finite values") + is_first_feedback = self._actual_joints is None + self._actual_joints = values.copy() + self._robot.state.q[RM75_Q_SLICE] = values + self._robot.update_kinematics() + base_to_flange = self._robot.get_T_world_frame("link_7") + if is_first_feedback: + self._frame_task.T_world_frame = base_to_flange.copy() + base_to_tool = base_to_flange @ self._tool_transform + return _transform_to_arm_pose(base_to_tool) + + def solve(self, target_tool_pose: ArmPose) -> list[float]: + if self._actual_joints is None: + raise RuntimeError("joint state must be initialized before QP solve") + self._frame_task.T_world_frame = ( + _arm_pose_to_transform(target_tool_pose) @ self._tool_inverse + ) + self._solver.solve(True) + result = np.asarray( + self._robot.state.q[RM75_Q_SLICE], + dtype=float, + ).copy() + self._validate_result(result) + return result.tolist() + + def _validate_result(self, result: np.ndarray) -> None: + if result.shape != (7,) or not np.isfinite(result).all(): + raise ValueError("QP result must contain 7 finite values") + lower = self._joint_limits[:, 0] + upper = self._joint_limits[:, 1] + if np.any(result < lower - 1e-9) or np.any(result > upper + 1e-9): + raise ValueError("QP result violates RM75 joint position limits") + max_step = self._velocity_limits * self._dt + 1e-9 + if np.any(np.abs(result - self._actual_joints) > max_step): + raise ValueError("QP result violates RM75 one-cycle velocity limits") diff --git a/xr_rm_teleop/xr_rm_teleop/realman_adapter.py b/xr_rm_teleop/xr_rm_teleop/realman_adapter.py index 8b6c962..bdbf757 100755 --- a/xr_rm_teleop/xr_rm_teleop/realman_adapter.py +++ b/xr_rm_teleop/xr_rm_teleop/realman_adapter.py @@ -1,21 +1,15 @@ -"""RM75 机械臂适配层。 - -对上提供统一的当前位姿读取、笛卡尔位姿目标发送和停止接口；对下根据配置 -选择 mock 积分模拟器或睿尔曼 Python API2 真机通信。 -""" +"""RM75 机械臂关节反馈、关节透传和停止适配层。""" from __future__ import annotations import math +import threading +import time from dataclasses import dataclass from numbers import Number from typing import Any -def _angle_delta(target: float, current: float) -> float: - return math.atan2(math.sin(target - current), math.cos(target - current)) - - @dataclass class ArmPose: x: float @@ -32,55 +26,64 @@ class ArmPose: return [self.rx, self.ry, self.rz] -class MockRealManAdapter: - """无机械臂时使用的运动学模拟器，用于验证 ROS2 遥操链路。""" +@dataclass(frozen=True) +class JointStateSnapshot: + positions: list[float] + received_at: float - 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 + +class MockRealManAdapter: + """不导入厂商 SDK 的关节状态 mock。""" + + def __init__(self, initial_joint_degrees: list[float]) -> None: + if len(initial_joint_degrees) != 7 or not all( + math.isfinite(value) for value in initial_joint_degrees + ): + raise ValueError("initial joint pose must contain 7 finite values") + self._joint_positions = [ + math.radians(value) for value in initial_joint_degrees + ] + self.last_joint_target: list[float] | None = None self.last_tool_open: bool | None = None def connect(self) -> None: return - def get_current_pose(self) -> ArmPose: - return self._pose + def get_latest_joint_state(self) -> JointStateSnapshot: + return JointStateSnapshot( + list(self._joint_positions), + time.monotonic(), + ) - def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None: + def send_joint_target(self, joints: list[float], follow: bool) -> None: del follow - self.last_velocity = [ - (pose.x - self._pose.x) / self._dt, - (pose.y - self._pose.y) / self._dt, - (pose.z - self._pose.z) / self._dt, - _angle_delta(pose.rx, self._pose.rx) / self._dt, - _angle_delta(pose.ry, self._pose.ry) / self._dt, - _angle_delta(pose.rz, self._pose.rz) / self._dt, - ] - self._pose = pose + if len(joints) != 7 or not all(math.isfinite(value) for value in joints): + raise ValueError("joint target must contain 7 finite values") + self._joint_positions = list(joints) + self.last_joint_target = list(joints) def stop(self) -> None: - self.last_velocity = [0.0] * 6 + return def close(self) -> None: self.stop() - def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None: - del config_file, peripheral_arm + def configure_peripheral(self, config: Any, peripheral_arm: str) -> None: + del config, peripheral_arm def set_tool_enabled(self, open_tool: bool) -> None: self.last_tool_open = open_tool class RealManAdapter: - """睿尔曼 Python API2 的笛卡尔位姿透传适配层。""" + """复用一个睿尔曼 Python API2 连接的关节适配层。""" def __init__( self, robot_ip: str, robot_port: int, avoid_singularity: int, - frame_type: int, + feedback_period: float, logger: Any | None = None, configure_safety_limits: bool = True, max_line_speed: float = 1.0, @@ -98,7 +101,9 @@ class RealManAdapter: self._robot_ip = robot_ip self._robot_port = robot_port self._avoid_singularity = avoid_singularity - self._frame_type = frame_type + if feedback_period <= 0.0: + raise ValueError("feedback_period must be positive") + self._feedback_period = feedback_period self._logger = logger self._configure_safety_limits = configure_safety_limits self._max_line_speed = max_line_speed @@ -114,6 +119,11 @@ class RealManAdapter: self._canfd_radio = canfd_radio self._scissorgripper: int | None = None self._arm: Any | None = None + self._joint_state_lock = threading.Lock() + self._latest_joint_state: JointStateSnapshot | None = None + self._feedback_stop = threading.Event() + self._feedback_thread: threading.Thread | None = None + self._feedback_fault_logged = False def connect(self) -> None: try: @@ -130,38 +140,48 @@ class RealManAdapter: "RealMan connected: " f"ip={self._robot_ip}, port={self._robot_port}, " f"avoid_singularity={self._avoid_singularity}, " - f"frame_type={self._frame_type}, command=rm_movep_canfd" + "command=rm_movej_canfd" ) if self._configure_safety_limits: self._apply_safety_limits() if self._move_to_initial_pose_on_connect: self._move_to_initial_pose() + self._feedback_stop.clear() + self._feedback_thread = threading.Thread( + target=self._feedback_loop, + name=f"rm75_feedback_{self._robot_ip}", + daemon=True, + ) + self._feedback_thread.start() - 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 get_latest_joint_state(self) -> JointStateSnapshot | None: + with self._joint_state_lock: + if self._latest_joint_state is None: + return None + return JointStateSnapshot( + list(self._latest_joint_state.positions), + self._latest_joint_state.received_at, + ) - def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None: + def send_joint_target(self, joints: list[float], follow: bool) -> None: self._require_arm() - ret = self._arm.rm_movep_canfd( - [pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz], + if len(joints) != 7 or not all(math.isfinite(value) for value in joints): + raise ValueError("joint target must contain 7 finite values") + ret = self._arm.rm_movej_canfd( + [math.degrees(value) for value in joints], follow, + 0, self._canfd_trajectory_mode, self._canfd_radio, ) - self._check_return(ret, "rm_movep_canfd") + self._check_return(ret, "rm_movej_canfd") - def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None: + def configure_peripheral(self, config: Any, peripheral_arm: str) -> None: self._require_arm() - from .fun_peripheral import load_peripheral_config, peripheral_cfg + from .fun_peripheral import peripheral_cfg - config = load_peripheral_config(config_file, peripheral_arm) self._scissorgripper = config.scissorgripper - tool_name = list(config.tools_in_ee.keys())[config.scissorgripper] + tool_name = config.tool_name self._log_info( "开始配置 RealMan 末端外设：" f"arm={peripheral_arm}, scissorgripper={config.scissorgripper}, " @@ -196,6 +216,12 @@ class RealManAdapter: if self._arm is None: return self.stop() + self._feedback_stop.set() + if self._feedback_thread is not None: + self._feedback_thread.join(timeout=3.0) + if self._feedback_thread.is_alive(): + self._log_warn("RealMan 关节反馈线程未在 3 秒内退出。") + self._feedback_thread = None try: self._arm.rm_delete_robot_arm() finally: @@ -205,6 +231,37 @@ class RealManAdapter: if self._arm is None: raise RuntimeError("睿尔曼机械臂尚未连接") + def _feedback_loop(self) -> None: + while not self._feedback_stop.is_set(): + try: + self._read_joint_state_once() + self._feedback_fault_logged = False + except Exception as exc: + if not self._feedback_fault_logged: + self._log_warn(f"RealMan 关节反馈读取失败：{exc}") + self._feedback_fault_logged = True + self._feedback_stop.wait(self._feedback_period) + + def _read_joint_state_once(self) -> None: + self._require_arm() + result = self._arm.rm_get_joint_degree() + self._check_return(result, "rm_get_joint_degree") + if not isinstance(result, tuple) or len(result) < 2: + raise RuntimeError(f"rm_get_joint_degree 返回格式错误：{result!r}") + degrees = result[1] + if ( + not isinstance(degrees, (list, tuple)) + or len(degrees) != 7 + or not all(isinstance(value, Number) for value in degrees) + ): + raise RuntimeError(f"RM75 关节反馈必须包含 7 个数值：{degrees!r}") + positions = [math.radians(float(value)) for value in degrees] + if not all(math.isfinite(value) for value in positions): + raise RuntimeError("RM75 关节反馈包含 NaN/Inf") + snapshot = JointStateSnapshot(positions, time.monotonic()) + with self._joint_state_lock: + self._latest_joint_state = snapshot + def _apply_safety_limits(self) -> None: # 真机安全限幅尽量下发到控制器；不支持的 SDK 接口会在 _try_call 中降级为警告。 self._try_call("rm_set_avoid_singularity_mode", int(self._avoid_singularity)) @@ -260,74 +317,3 @@ class RealManAdapter: @staticmethod def _return_code(ret: Any) -> Any: return ret[0] if isinstance(ret, tuple) and ret else ret - - @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 - elif hasattr(obj, "to_dictionary"): - try: - return cls._find_pose(obj.to_dictionary(7)) - except TypeError: - return cls._find_pose(obj.to_dictionary()) - elif hasattr(obj, "to_dict"): - return cls._find_pose(obj.to_dict()) - else: - for key in ("pose", "tool_pose", "tcp_pose", "current_pose"): - if hasattr(obj, key): - pose = cls._as_pose(getattr(obj, key)) - 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]] - if isinstance(value, dict): - position = value.get("position") - euler = value.get("euler") - if isinstance(position, dict) and isinstance(euler, dict): - keys = ("x", "y", "z") - rpy_keys = ("rx", "ry", "rz") - if all(key in position for key in keys) and all(key in euler for key in rpy_keys): - return [ - float(position["x"]), - float(position["y"]), - float(position["z"]), - float(euler["rx"]), - float(euler["ry"]), - float(euler["rz"]), - ] - if all(hasattr(value, attr) for attr in ("position", "euler")): - position = getattr(value, "position") - euler = getattr(value, "euler") - if all(hasattr(position, key) for key in ("x", "y", "z")) and all( - hasattr(euler, key) for key in ("rx", "ry", "rz") - ): - return [ - float(position.x), - float(position.y), - float(position.z), - float(euler.rx), - float(euler.ry), - float(euler.rz), - ] - return None diff --git a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py b/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py index 74440d0..3885416 100755 --- a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py +++ b/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py @@ -1,7 +1,7 @@ -"""RM75 单臂 XR 相对位姿透传遥操作节点。 +"""RM75 单臂 XR 相对位姿 QP 遥操作节点。 节点订阅左/右手柄位姿，在 grip 按下时锁定手柄和 TCP 起点，把手柄相对位姿 -映射成机器人坐标系中的目标 TCP，并通过 rm_movep_canfd 持续下发目标位姿。 +映射成机器人坐标系中的目标 TCP，再通过 Placo 和 rm_movej_canfd 下发关节目标。 """ from __future__ import annotations @@ -9,6 +9,7 @@ from __future__ import annotations import math import queue import threading +import time from typing import Iterable import rclpy @@ -19,7 +20,14 @@ from std_msgs.msg import Bool from xr_rm_interfaces.msg import XrController -from .realman_adapter import ArmPose, MockRealManAdapter, RealManAdapter +from .fun_peripheral import load_peripheral_config +from .placo_ik_solver import PlacoIkSolver +from .realman_adapter import ( + ArmPose, + JointStateSnapshot, + MockRealManAdapter, + RealManAdapter, +) def _norm(values: Iterable[float]) -> float: @@ -176,13 +184,11 @@ class SingleArmVelocityTeleop(Node): self.declare_parameter("low_z_threshold", 0.20) self.declare_parameter("low_z_min_radius", 0.21) 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("current_pose_poll_hz", 10.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_urdf_path", "") 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.declare_parameter("configure_safety_limits", True) self.declare_parameter("max_line_speed", 1.0) @@ -232,7 +238,6 @@ class SingleArmVelocityTeleop(Node): self._low_z_threshold = float(self.get_parameter("low_z_threshold").value) self._low_z_min_radius = float(self.get_parameter("low_z_min_radius").value) self._xr_to_robot_matrix = self._float_list_parameter("xr_to_robot_matrix", 9) - self._current_pose_poll_hz = float(self.get_parameter("current_pose_poll_hz").value) self._follow = self._bool_parameter("follow") self._enable_tool_control = self._bool_parameter("enable_tool_control") self._enable_trigger_gripper_control = self._bool_parameter("enable_trigger_gripper_control") @@ -254,7 +259,8 @@ class SingleArmVelocityTeleop(Node): self._last_sent_orientation: list[float] | None = None self._last_command_time: Time | None = None self._last_current_pose: ArmPose | None = None - self._last_current_pose_time: Time | None = None + self._last_valid_joint_target: list[float] | None = None + self._joint_feedback_ready = False self._stop_sent = True self._trigger_tool_open = True self._last_trigger_pressed: bool | None = None @@ -262,6 +268,17 @@ class SingleArmVelocityTeleop(Node): self._tool_worker_stop = threading.Event() self._tool_worker_thread: threading.Thread | None = None + peripheral_arm = self._peripheral_arm_name() + config_file = str(self.get_parameter("peripheral_config_file").value) + self._peripheral_config = load_peripheral_config( + config_file, + peripheral_arm, + ) + self._ik_solver = PlacoIkSolver( + str(self.get_parameter("robot_urdf_path").value), + self._peripheral_config.tool_pose, + self._dt, + ) self._adapter = self._make_adapter() self._adapter.connect() self._setup_tool_control() @@ -276,24 +293,24 @@ class SingleArmVelocityTeleop(Node): 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}, " + f"{self._arm_name} Placo QP 遥操节点已启动，监听话题：{topic}, " f"dt={self._dt:.4f}s, follow={self._follow}, " f"orientation_control={self._enable_orientation_control}" ) def _make_adapter(self): - # mock 和真机共享同一位姿目标链路，只在适配层切换执行方式。 + initial_joint_pose = self._float_list_parameter( + "initial_joint_pose", + 7, + ) if self._bool_parameter("use_mock"): - return MockRealManAdapter( - [float(v) for v in self.get_parameter("mock_initial_pose").value], - self._dt, - ) + return MockRealManAdapter(initial_joint_pose) return RealManAdapter( robot_ip=self.get_parameter("robot_ip").value, robot_port=int(self.get_parameter("robot_port").value), avoid_singularity=int(self.get_parameter("avoid_singularity").value), - frame_type=int(self.get_parameter("frame_type").value), + feedback_period=self._dt, logger=self.get_logger(), configure_safety_limits=self._bool_parameter("configure_safety_limits"), max_line_speed=float(self.get_parameter("max_line_speed").value), @@ -303,13 +320,20 @@ class SingleArmVelocityTeleop(Node): joint_max_speed=float(self.get_parameter("joint_max_speed").value), joint_max_acc=float(self.get_parameter("joint_max_acc").value), move_to_initial_pose_on_connect=self._bool_parameter("move_to_initial_pose_on_connect"), - initial_joint_pose=self._float_list_parameter("initial_joint_pose", 7), + initial_joint_pose=initial_joint_pose, init_move_speed=int(self.get_parameter("init_move_speed").value), canfd_trajectory_mode=int(self.get_parameter("canfd_trajectory_mode").value), canfd_radio=int(self.get_parameter("canfd_radio").value), ) def _setup_tool_control(self) -> None: + peripheral_arm = self._peripheral_arm_name() + if self._bool_parameter("configure_peripheral_on_connect"): + self._adapter.configure_peripheral( + self._peripheral_config, + peripheral_arm, + ) + if not self._enable_tool_control: if self._enable_trigger_gripper_control: self.get_logger().warn( @@ -317,11 +341,6 @@ class SingleArmVelocityTeleop(Node): ) return - peripheral_arm = self._peripheral_arm_name() - config_file = str(self.get_parameter("peripheral_config_file").value) - if self._bool_parameter("configure_peripheral_on_connect"): - self._adapter.configure_peripheral(config_file, peripheral_arm) - self._start_tool_worker() topic = str(self.get_parameter("tool_command_topic").value).strip() @@ -436,6 +455,32 @@ class SingleArmVelocityTeleop(Node): def _control_tick(self) -> None: now = self.get_clock().now() + snapshot = self._fresh_joint_state() + if snapshot is None: + if self._joint_feedback_ready: + self.get_logger().warn( + f"{self._arm_name} 关节反馈缺失或过期，机械臂停止。", + throttle_duration_sec=1.0, + ) + self._joint_feedback_ready = False + self._safe_stop(reset_active=True) + return + try: + current_pose = self._sync_joint_feedback(snapshot) + except Exception as exc: + self.get_logger().error( + f"{self._arm_name} 关节反馈同步到 Placo 失败：{exc}", + throttle_duration_sec=1.0, + ) + self._joint_feedback_ready = False + self._safe_stop(reset_active=True) + return + if not self._joint_feedback_ready: + self.get_logger().info( + f"{self._arm_name} 已收到首帧有效关节反馈，QP 可以启用。" + ) + self._joint_feedback_ready = True + if self._last_msg is None or self._last_msg_time is None: self._safe_stop(reset_active=True) return @@ -469,13 +514,17 @@ class SingleArmVelocityTeleop(Node): self._safe_stop(reset_active=True) return if not self._active: - self._enter_active_control(controller_now, controller_quat, now) + self._enter_active_control( + controller_now, + controller_quat, + current_pose, + now, + ) return assert self._controller_start is not None assert self._robot_start_pose is not None - self._maybe_refresh_current_pose(now) raw_target_xyz = self._raw_target_from_controller(controller_now) raw_target_rpy = self._raw_orientation_from_controller(controller_quat) workspace_target, workspace_clamped = self._clamp_workspace_with_flag(raw_target_xyz) @@ -507,7 +556,8 @@ class SingleArmVelocityTeleop(Node): ) self._publish_debug(raw_target_pose, target_pose, velocity, target_clamped) - if self._send_cartesian_target(target_pose): + joint_target = self._solve_joint_target(target_pose) + if self._send_joint_target(joint_target): self._last_sent_target = sent_target self._last_sent_orientation = sent_orientation self._last_command_time = now @@ -517,18 +567,9 @@ class SingleArmVelocityTeleop(Node): self, controller_now: list[float], controller_quat: tuple[float, float, float, float], + robot_pose: ArmPose, now: Time, ) -> None: - try: - robot_pose = self._read_current_pose_for_control(now) - except Exception as exc: - self.get_logger().error( - f"{self._arm_name} 读取 TCP 位姿失败，停止输出：{exc}", - throttle_duration_sec=1.0, - ) - self._safe_stop(reset_active=True) - return - robot_xyz = robot_pose.xyz() self._active = True self._controller_start = controller_now @@ -755,27 +796,45 @@ class SingleArmVelocityTeleop(Node): for i in range(3) ] - def _read_current_pose_for_control(self, now: Time) -> ArmPose: - pose = self._adapter.get_current_pose() - self._last_current_pose = pose - self._last_current_pose_time = now - return pose + def _fresh_joint_state(self) -> JointStateSnapshot | None: + snapshot = self._adapter.get_latest_joint_state() + if snapshot is None: + return None + age = time.monotonic() - snapshot.received_at + if age < 0.0 or age > self._command_timeout_sec: + return None + if ( + len(snapshot.positions) != 7 + or not all(math.isfinite(value) for value in snapshot.positions) + ): + return None + return snapshot - def _maybe_refresh_current_pose(self, now: Time) -> None: - if self._current_pose_poll_hz <= 0.0: - return - if self._last_current_pose_time is not None: - age = (now - self._last_current_pose_time).nanoseconds * 1e-9 - if age < 1.0 / self._current_pose_poll_hz: - return + def _sync_joint_feedback( + self, + snapshot: JointStateSnapshot, + ) -> ArmPose: + current_pose = self._ik_solver.update_joint_state( + snapshot.positions + ) + self._last_current_pose = current_pose + if not self._active or self._last_valid_joint_target is None: + self._last_valid_joint_target = list(snapshot.positions) + return current_pose + def _solve_joint_target(self, target_pose: ArmPose) -> list[float]: + if self._last_valid_joint_target is None: + raise RuntimeError("valid joint feedback has not been initialized") try: - self._read_current_pose_for_control(now) + result = self._ik_solver.solve(target_pose) except Exception as exc: self.get_logger().warn( - f"{self._arm_name} 低频读取 TCP 位姿失败，继续透传目标：{exc}", + f"{self._arm_name} QP 求解失败，保持上一组关节目标：{exc}", throttle_duration_sec=1.0, ) + return list(self._last_valid_joint_target) + self._last_valid_joint_target = list(result) + return list(result) def _safe_stop(self, reset_active: bool) -> None: if not self._stop_sent: @@ -818,16 +877,16 @@ class SingleArmVelocityTeleop(Node): return ArmPose(*self._last_sent_target, *rpy) return None - def _send_cartesian_target(self, pose: ArmPose) -> bool: + def _send_joint_target(self, joints: list[float]) -> bool: try: - self._adapter.send_cartesian_target(pose, self._follow) + self._adapter.send_joint_target(joints, self._follow) except Exception as exc: self.get_logger().error( - f"{self._arm_name} 发送位姿透传命令失败：{exc}", + f"{self._arm_name} 发送关节透传命令失败：{exc}", throttle_duration_sec=1.0, ) - self._active = False self._send_stop_once() + self._safe_stop(reset_active=True) return False return True @@ -917,8 +976,6 @@ class SingleArmVelocityTeleop(Node): raise ValueError("target_filter_fast_threshold_m must be >= 0") if self._max_linear_speed <= 0.0: raise ValueError("max_linear_speed must be > 0") - if self._current_pose_poll_hz < 0.0: - raise ValueError("current_pose_poll_hz must be >= 0") if self._orientation_deadband_rad < 0.0: raise ValueError("orientation_deadband_rad must be >= 0") if not 0.0 <= self._orientation_filter_alpha <= 1.0: