| 15 | |
| 16 | |
| 17 | class MotionWatcher(Node): |
| 18 | |
| 19 | def __init__(self, need_save=True): |
| 20 | super().__init__('motion_watcher') |
| 21 | |
| 22 | # Visualization control |
| 23 | self.need_save = need_save |
| 24 | self.is_visualizing = True |
| 25 | self.dt = 1.0 / 50.0 # 50Hz |
| 26 | self.current_frame_index = 0 |
| 27 | self.toggle_callback() |
| 28 | |
| 29 | # Mujoco visualization |
| 30 | self.mjc = self.load_mujoco() |
| 31 | |
| 32 | # Subscribers |
| 33 | self.motion_sub = self.create_subscription(MotionBlock, '/dar/motion', |
| 34 | self.motion_block_callback, |
| 35 | 10) |
| 36 | |
| 37 | self.toggle_sub = self.create_subscription(Time, '/dar/toggle', |
| 38 | self.toggle_callback, 10) |
| 39 | |
| 40 | # Visualization timer (50Hz) |
| 41 | self.viz_timer = self.create_timer(self.dt, self.visualization_loop) |
| 42 | |
| 43 | self.get_logger().info("Motion Watcher initialized") |
| 44 | |
| 45 | def load_mujoco(self): |
| 46 | """Initialize MuJoCo visualization""" |
| 47 | try: |
| 48 | import mujoco |
| 49 | import mujoco.viewer |
| 50 | |
| 51 | # Use relative path from current working directory |
| 52 | print(os.getcwd()) |
| 53 | humanoid_xml = "./src/unitree_mujoco/unitree_robots/g1/scene_29dof.xml" |
| 54 | if not os.path.exists(humanoid_xml): |
| 55 | self.get_logger().error( |
| 56 | f"Could not find g1_29dof.xml in any expected location") |
| 57 | return None |
| 58 | |
| 59 | mj_model = mujoco.MjModel.from_xml_path(humanoid_xml) |
| 60 | mj_data = mujoco.MjData(mj_model) |
| 61 | mj_model.opt.timestep = 1 / 50 # 50Hz visualization |
| 62 | |
| 63 | viewer = mujoco.viewer.launch_passive(mj_model, |
| 64 | mj_data, |
| 65 | show_left_ui=False, |
| 66 | show_right_ui=False) |
| 67 | viewer.cam.lookat[:] = np.array([0, 0, 0.7]) |
| 68 | viewer.cam.distance = 3.0 |
| 69 | viewer.cam.azimuth = -130 |
| 70 | viewer.cam.elevation = -20 |
| 71 | |
| 72 | self.get_logger().info(f"MuJoCo loaded from: {humanoid_xml}") |
| 73 | return (mujoco, mj_model, mj_data, viewer) |
| 74 | |