YikaiFu-cart/acRealman_xr
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add Placo IK solver and associated tests.

Browse Source
This commit is contained in:
YikaiFu-cart
2026-07-28 10:47:49 +08:00
parent bfd50e1035
commit fae5a560fb
24 changed files with 1351 additions and 242 deletions
Download Patch File Download Diff File Expand all files Collapse all files
xr_rm_bringup/config/dual_arm_rm75.yaml
+3 -11
View File
@@ -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
xr_rm_bringup/config/left_arm_rm75.yaml
+2 -6
View File
@@ -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
xr_rm_bringup/config/right_arm_rm75.yaml
+7 -11
View File
@@ -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
xr_rm_bringup/launch/arm_debug.launch.py
+26 -10
View File
@@ -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"),