From 826b929d975de87f8a573ec00a6923c2b73839cc Mon Sep 17 00:00:00 2001 From: YikaiFu-cart Date: Tue, 4 Aug 2026 13:36:39 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E6=B7=BB=E5=8A=A0=E5=8F=8C=E8=87=82=20?= =?UTF-8?q?MuJoCo=20=E6=98=BE=E7=A4=BA=E8=8A=82=E7=82=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- xr_rm_bringup/config/dual_arm_mujoco.yaml | 4 + xr_rm_mujoco/setup.py | 5 + xr_rm_mujoco/test/test_dual_arm_simulator.py | 10 ++ .../xr_rm_mujoco/dual_arm_simulator.py | 114 ++++++++++++++++++ 4 files changed, 133 insertions(+) create mode 100644 xr_rm_bringup/config/dual_arm_mujoco.yaml diff --git a/xr_rm_bringup/config/dual_arm_mujoco.yaml b/xr_rm_bringup/config/dual_arm_mujoco.yaml new file mode 100644 index 0000000..40da7ff --- /dev/null +++ b/xr_rm_bringup/config/dual_arm_mujoco.yaml @@ -0,0 +1,4 @@ +# 双 RM75 MuJoCo 运动学显示参数。初始姿态和控制限制仍由 dual_arm_rm75.yaml 管理。 +dual_arm_simulator: + ros__parameters: + render_rate_hz: 60.0 diff --git a/xr_rm_mujoco/setup.py b/xr_rm_mujoco/setup.py index ecfb22a..8ffb730 100644 --- a/xr_rm_mujoco/setup.py +++ b/xr_rm_mujoco/setup.py @@ -20,4 +20,9 @@ setup( description="MuJoCo kinematic visualization for the dual RM75 platform.", license="Apache-2.0", tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "dual_arm_simulator = xr_rm_mujoco.dual_arm_simulator:main", + ], + }, ) diff --git a/xr_rm_mujoco/test/test_dual_arm_simulator.py b/xr_rm_mujoco/test/test_dual_arm_simulator.py index 10799a8..79999d0 100644 --- a/xr_rm_mujoco/test/test_dual_arm_simulator.py +++ b/xr_rm_mujoco/test/test_dual_arm_simulator.py @@ -15,6 +15,9 @@ URDF_PATH = ( SRC_DIR / "xr_rm_teleop" / "models" / "dual_rm75" / "Dual_arm.urdf" ) DUAL_CONFIG_PATH = SRC_DIR / "xr_rm_bringup" / "config" / "dual_arm_rm75.yaml" +MUJOCO_CONFIG_PATH = ( + SRC_DIR / "xr_rm_bringup" / "config" / "dual_arm_mujoco.yaml" +) def test_dual_urdf_loads_with_expected_joint_mapping() -> None: @@ -60,6 +63,13 @@ def test_yaml_initial_poses_populate_both_arms() -> None: assert simulation.ready +def test_mujoco_config_contains_only_render_parameters() -> None: + with MUJOCO_CONFIG_PATH.open(encoding="utf-8") as stream: + parameters = yaml.safe_load(stream)["dual_arm_simulator"]["ros__parameters"] + + assert parameters == {"render_rate_hz": 60.0} + + @pytest.mark.parametrize( ("names", "positions", "match"), [ diff --git a/xr_rm_mujoco/xr_rm_mujoco/dual_arm_simulator.py b/xr_rm_mujoco/xr_rm_mujoco/dual_arm_simulator.py index 57a78ff..cc28e59 100644 --- a/xr_rm_mujoco/xr_rm_mujoco/dual_arm_simulator.py +++ b/xr_rm_mujoco/xr_rm_mujoco/dual_arm_simulator.py @@ -4,14 +4,23 @@ from __future__ import annotations import math from pathlib import Path +from typing import Callable import mujoco +import mujoco.viewer +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import JointState ARM_JOINT_NAMES = { "left": tuple(f"scissor_joint_{index}" for index in range(1, 8)), "right": tuple(f"omnipic_joint_{index}" for index in range(1, 8)), } +STATE_TOPICS = { + "left": "/xr_rm/left_rm75/joint_states", + "right": "/xr_rm/right_rm75/joint_states", +} class DualArmKinematicModel: @@ -84,3 +93,108 @@ class DualArmKinematicModel: float(self.data.qpos[self._qpos_addresses[arm][name]]) for name in ARM_JOINT_NAMES[arm] ] + + +class DualArmSimulator(Node): + """订阅左右关节反馈并刷新一个 MuJoCo 双臂 viewer。""" + + def __init__( + self, + viewer_factory: Callable = mujoco.viewer.launch_passive, + ) -> None: + super().__init__("dual_arm_simulator") + self.declare_parameter("robot_urdf_path", "") + self.declare_parameter("render_rate_hz", 60.0) + + render_rate_hz = float(self.get_parameter("render_rate_hz").value) + if not math.isfinite(render_rate_hz) or render_rate_hz <= 0.0: + raise ValueError("render_rate_hz must be finite and > 0") + + self._kinematics = DualArmKinematicModel( + str(self.get_parameter("robot_urdf_path").value) + ) + self._viewer_factory = viewer_factory + self._viewer = None + self._subscriptions = [ + self.create_subscription( + JointState, + topic, + lambda message, selected_arm=arm: self._on_joint_state( + selected_arm, + message, + ), + 10, + ) + for arm, topic in STATE_TOPICS.items() + ] + self.create_timer(1.0 / render_rate_hz, self._render) + self.get_logger().info( + "MuJoCo 双臂节点已启动，等待左右关节状态，" + f"render_rate_hz={render_rate_hz:.1f}" + ) + + def _on_joint_state(self, arm: str, message: JointState) -> None: + try: + if self._viewer is None: + self._kinematics.apply_arm_state( + arm, + list(message.name), + list(message.position), + ) + else: + with self._viewer.lock(): + self._kinematics.apply_arm_state( + arm, + list(message.name), + list(message.position), + ) + except (RuntimeError, ValueError) as exc: + self.get_logger().warn( + f"拒绝 {arm} 关节状态：{exc}", + throttle_duration_sec=1.0, + ) + + def _render(self) -> None: + for topic in STATE_TOPICS.values(): + publisher_count = self.count_publishers(topic) + if publisher_count > 1: + self.get_logger().warn( + f"关节状态话题存在多个发布者：{topic}, count={publisher_count}", + throttle_duration_sec=5.0, + ) + + if not self._kinematics.ready: + return + if self._viewer is None: + self._viewer = self._viewer_factory( + self._kinematics.model, + self._kinematics.data, + ) + if not self._viewer.is_running(): + self.get_logger().info("MuJoCo viewer 已关闭。") + rclpy.shutdown() + return + self._viewer.sync() + + def close_viewer(self) -> None: + if self._viewer is not None: + self._viewer.close() + self._viewer = None + + +def main(args=None) -> None: + rclpy.init(args=args) + node = None + try: + node = DualArmSimulator() + rclpy.spin(node) + finally: + if node is not None: + node.close_viewer() + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main()