Add configuration files and launch scripts for left and right arm RM75 teleoperation

xr_rm_teleop/config/single_arm.yaml

@@ -15,8 +15,11 @@ single_arm_velocity_teleop:
     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]
+    workspace_min: [-0.70, -0.60, 0.10]
+    workspace_max: [0.50, 0.40, 0.70]
+    cyl_radius_limit: [0.20, 0.60]
+    low_z_threshold: 0.20
+    low_z_min_radius: 0.21
 
     # 右臂 PICO/OpenXR 相对位移到 RM75 base 坐标的初始映射：
     # 机器人位移增量 = [手柄y, -手柄z, -手柄x]。
@@ -25,10 +28,22 @@ single_arm_velocity_teleop:
                          -1.0, 0.0, 0.0]
 
     use_mock: true
-    mock_initial_pose: [0.1924, -0.3025, 0.2923, 3.0560, 0.1310, 0.9070]
+    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
+    configure_safety_limits: true
+    max_line_speed: 1.0
+    max_angular_speed: 1.5
+    max_line_acc: 1.0
+    max_angular_acc: 2.0
+    joint_max_speed: 180.0
+    joint_max_acc: 180.0
+    move_to_initial_pose_on_connect: false
+    initial_joint_pose: [-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67]
+    initial_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
+    init_move_speed: 20
+    debug_topic_prefix: /xr_rm

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -60,12 +60,36 @@ class RealManAdapter:
         dt: float,
         avoid_singularity: int,
         frame_type: int,
+        logger: Any | None = None,
+        configure_safety_limits: bool = True,
+        max_line_speed: float = 1.0,
+        max_angular_speed: float = 1.5,
+        max_line_acc: float = 1.0,
+        max_angular_acc: float = 2.0,
+        joint_max_speed: float = 180.0,
+        joint_max_acc: float = 180.0,
+        move_to_initial_pose_on_connect: bool = False,
+        initial_joint_pose: list[float] | None = None,
+        initial_tcp_pose: list[float] | None = None,
+        init_move_speed: int = 20,
     ) -> 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._logger = logger
+        self._configure_safety_limits = configure_safety_limits
+        self._max_line_speed = max_line_speed
+        self._max_angular_speed = max_angular_speed
+        self._max_line_acc = max_line_acc
+        self._max_angular_acc = max_angular_acc
+        self._joint_max_speed = joint_max_speed
+        self._joint_max_acc = joint_max_acc
+        self._move_to_initial_pose_on_connect = move_to_initial_pose_on_connect
+        self._initial_joint_pose = initial_joint_pose
+        self._initial_tcp_pose = initial_tcp_pose
+        self._init_move_speed = init_move_speed
         self._arm: Any | None = None
 
     def connect(self) -> None:
@@ -77,8 +101,12 @@ class RealManAdapter:
             ) 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")
+        handle = self._arm.rm_create_robot_arm(self._robot_ip, self._robot_port)
+        self._check_robot_handle(handle)
+        if self._configure_safety_limits:
+            self._apply_safety_limits()
+        if self._move_to_initial_pose_on_connect:
+            self._move_to_initial_pose()
         # 速度透传初始化必须和控制循环周期一致，避免真实机械臂出现周期不稳定。
         ret = self._arm.rm_set_movev_canfd_init(
             self._avoid_singularity,
@@ -122,6 +150,47 @@ class RealManAdapter:
         if self._arm is None:
             raise RuntimeError("睿尔曼机械臂尚未连接")
 
+    def _apply_safety_limits(self) -> None:
+        self._try_call("rm_set_avoid_singularity_mode", True)
+        self._try_call("rm_set_arm_max_line_speed", self._max_line_speed)
+        self._try_call("rm_set_arm_max_angular_speed", self._max_angular_speed)
+        self._try_call("rm_set_arm_max_line_acc", self._max_line_acc)
+        self._try_call("rm_set_arm_max_angular_acc", self._max_angular_acc)
+        for joint_index in range(1, 8):
+            self._try_call("rm_set_joint_max_speed", joint_index, self._joint_max_speed)
+            self._try_call("rm_set_joint_max_acc", joint_index, self._joint_max_acc)
+
+    def _move_to_initial_pose(self) -> None:
+        if self._initial_joint_pose is None or self._initial_tcp_pose is None:
+            raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose 和 initial_tcp_pose")
+
+        ret = self._arm.rm_movej(self._initial_joint_pose, self._init_move_speed, 0, 0, 1)
+        self._check_return(ret, "rm_movej(initial_joint_pose)")
+        ret = self._arm.rm_movel(self._initial_tcp_pose, self._init_move_speed, 0, 0, 1)
+        self._check_return(ret, "rm_movel(initial_tcp_pose)")
+
+    def _try_call(self, name: str, *args: Any) -> None:
+        func = getattr(self._arm, name, None)
+        if func is None:
+            self._log_warn(f"当前睿尔曼 SDK 不支持 {name}，跳过该安全配置。")
+            return
+
+        try:
+            ret = func(*args)
+            self._check_return(ret, name)
+        except Exception as exc:
+            self._log_warn(f"{name} 安全配置失败，继续使用软件侧限幅：{exc}")
+
+    def _log_warn(self, message: str) -> None:
+        if self._logger is not None:
+            self._logger.warn(message)
+
+    @staticmethod
+    def _check_robot_handle(handle: Any) -> None:
+        handle_id = getattr(handle, "id", None)
+        if handle_id == -1:
+            raise RuntimeError("rm_create_robot_arm failed: socket error or robot unreachable")
+
     @staticmethod
     def _check_return(ret: Any, name: str) -> None:
         code = ret[0] if isinstance(ret, tuple) and ret else ret
@@ -148,6 +217,19 @@ class RealManAdapter:
                 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
@@ -155,4 +237,33 @@ class RealManAdapter:
         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

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -4,11 +4,12 @@ import math
 from typing import Iterable
 
 import rclpy
+from geometry_msgs.msg import PoseStamped, TwistStamped
 from rclpy.node import Node
 
 from xr_rm_interfaces.msg import XrController
 
-from .realman_adapter import MockRealManAdapter, RealManAdapter
+from .realman_adapter import ArmPose, MockRealManAdapter, RealManAdapter
 
 
 def _norm(values: Iterable[float]) -> float:
@@ -37,6 +38,9 @@ class SingleArmVelocityTeleop(Node):
         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("cyl_radius_limit", [0.20, 0.60])
+        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("use_mock", True)
         self.declare_parameter("mock_initial_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0])
@@ -45,10 +49,24 @@ class SingleArmVelocityTeleop(Node):
         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)
+        self.declare_parameter("max_angular_speed", 1.5)
+        self.declare_parameter("max_line_acc", 1.0)
+        self.declare_parameter("max_angular_acc", 2.0)
+        self.declare_parameter("joint_max_speed", 180.0)
+        self.declare_parameter("joint_max_acc", 180.0)
+        self.declare_parameter("move_to_initial_pose_on_connect", False)
+        self.declare_parameter("initial_joint_pose", [0.0] * 7)
+        self.declare_parameter("initial_tcp_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0])
+        self.declare_parameter("init_move_speed", 20)
+        self.declare_parameter("debug_topic_prefix", "/xr_rm")
 
         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)
+        if control_rate_hz <= 0.0:
+            raise ValueError("control_rate_hz must be > 0")
         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)
@@ -59,9 +77,15 @@ class SingleArmVelocityTeleop(Node):
         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._cyl_radius_limit = self._float_list_parameter("cyl_radius_limit", 2)
+        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._follow = bool(self.get_parameter("follow").value)
-        self._validate_workspace()
+        self._follow = self._bool_parameter("follow")
+        self._debug_topic_prefix = str(self.get_parameter("debug_topic_prefix").value).rstrip("/")
+        if not self._debug_topic_prefix:
+            self._debug_topic_prefix = "/xr_rm"
+        self._validate_parameters()
 
         self._last_msg: XrController | None = None
         self._last_msg_time = None
@@ -73,12 +97,17 @@ class SingleArmVelocityTeleop(Node):
         self._adapter = self._make_adapter()
         self._adapter.connect()
 
+        debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
+        self._current_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/current_pose", 10)
+        self._target_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/target_pose", 10)
+        self._cmd_vel_pub = self.create_publisher(TwistStamped, f"{debug_ns}/cmd_vel", 10)
+
         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):
+        if self._bool_parameter("use_mock"):
             return MockRealManAdapter(
                 [float(v) for v in self.get_parameter("mock_initial_pose").value],
                 self._dt,
@@ -90,6 +119,18 @@ class SingleArmVelocityTeleop(Node):
             dt=self._dt,
             avoid_singularity=int(self.get_parameter("avoid_singularity").value),
             frame_type=int(self.get_parameter("frame_type").value),
+            logger=self.get_logger(),
+            configure_safety_limits=self._bool_parameter("configure_safety_limits"),
+            max_line_speed=float(self.get_parameter("max_line_speed").value),
+            max_angular_speed=float(self.get_parameter("max_angular_speed").value),
+            max_line_acc=float(self.get_parameter("max_line_acc").value),
+            max_angular_acc=float(self.get_parameter("max_angular_acc").value),
+            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_tcp_pose=self._float_list_parameter("initial_tcp_pose", 6),
+            init_move_speed=int(self.get_parameter("init_move_speed").value),
         )
 
     def _on_controller(self, msg: XrController) -> None:
@@ -119,7 +160,8 @@ class SingleArmVelocityTeleop(Node):
 
         controller_now = self._controller_xyz(self._last_msg)
         try:
-            robot_now = self._adapter.get_current_pose().xyz()
+            robot_pose = self._adapter.get_current_pose()
+            robot_now = robot_pose.xyz()
         except Exception as exc:
             self.get_logger().error(
                 f"{self._arm_name} 读取 TCP 位姿失败，停止输出：{exc}",
@@ -162,7 +204,9 @@ class SingleArmVelocityTeleop(Node):
             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])
+        cartesian_velocity = self._filtered_velocity + [0.0, 0.0, 0.0]
+        self._publish_debug(robot_pose, target, cartesian_velocity)
+        self._send_cartesian_velocity(cartesian_velocity)
 
     @staticmethod
     def _controller_xyz(msg: XrController) -> list[float]:
@@ -177,10 +221,32 @@ class SingleArmVelocityTeleop(Node):
         ]
 
     def _clamp_workspace(self, target: list[float]) -> list[float]:
-        return [
+        clamped = [
             _clamp(target[i], self._workspace_min[i], self._workspace_max[i])
             for i in range(3)
         ]
+        return self._clamp_cylinder_radius(clamped)
+
+    def _clamp_cylinder_radius(self, target: list[float]) -> list[float]:
+        min_radius, max_radius = self._cyl_radius_limit
+        if target[2] < self._low_z_threshold:
+            min_radius = max(min_radius, self._low_z_min_radius)
+
+        radius = math.hypot(target[0], target[1])
+        if max_radius > 0.0 and radius > max_radius:
+            scale = max_radius / radius
+            target[0] *= scale
+            target[1] *= scale
+        elif min_radius > 0.0 and radius < min_radius:
+            if radius > 1e-9:
+                scale = min_radius / radius
+                target[0] *= scale
+                target[1] *= scale
+            else:
+                target[0] = min_radius
+                target[1] = 0.0
+
+        return target
 
     @staticmethod
     def _clamp_vector_norm(vector: list[float], max_norm: float) -> list[float]:
@@ -205,6 +271,64 @@ class SingleArmVelocityTeleop(Node):
             )
             self._active = False
 
+    def _publish_debug(self, current_pose: ArmPose, target_xyz: list[float], velocity: list[float]) -> None:
+        stamp = self.get_clock().now().to_msg()
+        current_msg = self._pose_msg(stamp, current_pose)
+        target_msg = self._pose_msg(
+            stamp,
+            ArmPose(
+                x=target_xyz[0],
+                y=target_xyz[1],
+                z=target_xyz[2],
+                rx=current_pose.rx,
+                ry=current_pose.ry,
+                rz=current_pose.rz,
+            ),
+        )
+        velocity_msg = TwistStamped()
+        velocity_msg.header.stamp = stamp
+        velocity_msg.header.frame_id = "rm_base"
+        velocity_msg.twist.linear.x = float(velocity[0])
+        velocity_msg.twist.linear.y = float(velocity[1])
+        velocity_msg.twist.linear.z = float(velocity[2])
+        velocity_msg.twist.angular.x = float(velocity[3])
+        velocity_msg.twist.angular.y = float(velocity[4])
+        velocity_msg.twist.angular.z = float(velocity[5])
+
+        self._current_pose_pub.publish(current_msg)
+        self._target_pose_pub.publish(target_msg)
+        self._cmd_vel_pub.publish(velocity_msg)
+
+    @staticmethod
+    def _pose_msg(stamp, pose: ArmPose) -> PoseStamped:
+        qx, qy, qz, qw = SingleArmVelocityTeleop._euler_to_quaternion(pose.rx, pose.ry, pose.rz)
+        msg = PoseStamped()
+        msg.header.stamp = stamp
+        msg.header.frame_id = "rm_base"
+        msg.pose.position.x = float(pose.x)
+        msg.pose.position.y = float(pose.y)
+        msg.pose.position.z = float(pose.z)
+        msg.pose.orientation.x = qx
+        msg.pose.orientation.y = qy
+        msg.pose.orientation.z = qz
+        msg.pose.orientation.w = qw
+        return msg
+
+    @staticmethod
+    def _euler_to_quaternion(roll: float, pitch: float, yaw: float) -> tuple[float, float, float, float]:
+        cy = math.cos(yaw * 0.5)
+        sy = math.sin(yaw * 0.5)
+        cp = math.cos(pitch * 0.5)
+        sp = math.sin(pitch * 0.5)
+        cr = math.cos(roll * 0.5)
+        sr = math.sin(roll * 0.5)
+
+        qw = cr * cp * cy + sr * sp * sy
+        qx = sr * cp * cy - cr * sp * sy
+        qy = cr * sp * cy + sr * cp * sy
+        qz = cr * cp * sy - sr * sp * cy
+        return qx, qy, qz, qw
+
     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:
@@ -212,15 +336,41 @@ class SingleArmVelocityTeleop(Node):
         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]
+        values = [self._as_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:
+    def _bool_parameter(self, name: str) -> bool:
+        return self._as_bool(self.get_parameter(name).value)
+
+    @staticmethod
+    def _as_bool(value) -> bool:
+        if isinstance(value, bool):
+            return value
+        if isinstance(value, str):
+            return value.strip().lower() in ("1", "true", "yes", "on")
+        return bool(value)
+
+    def _validate_parameters(self) -> None:
+        if self._command_timeout_sec <= 0.0:
+            raise ValueError("command_timeout_sec must be > 0")
+        if self._deadband_m < 0.0:
+            raise ValueError("deadband_m must be >= 0")
+        if not 0.0 <= self._low_pass_alpha <= 1.0:
+            raise ValueError("low_pass_alpha must be in [0, 1]")
+        if self._max_linear_speed < 0.0:
+            raise ValueError("max_linear_speed must be >= 0")
         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}]")
+        min_radius, max_radius = self._cyl_radius_limit
+        if min_radius < 0.0:
+            raise ValueError("cyl_radius_limit[0] must be >= 0")
+        if max_radius <= min_radius:
+            raise ValueError("cyl_radius_limit[1] must be > cyl_radius_limit[0]")
+        if self._low_z_min_radius < 0.0:
+            raise ValueError("low_z_min_radius must be >= 0")
 
     def destroy_node(self) -> bool:
         self._adapter.close()