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

2026-05-21 17:25:59 +08:00
parent 5a48619599
commit 4dea1ae530
16 changed files with 1576 additions and 122 deletions
@@ -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