From 4086357ad506a70f48e38a9324533da710808831 Mon Sep 17 00:00:00 2001 From: wty-yy <993660140@qq.com> Date: Wed, 4 Feb 2026 18:40:13 +0800 Subject: [PATCH] v1.1.2; fix freejoint pos err causes qvel calc err --- UPDATE.md | 2 ++ ...k_0.6745.pt => go2_moe_cts_137k_0.6819.pt} | Bin robogauge/tasks/robots/go2/go2_moe_config.py | 2 +- robogauge/tasks/simulator/mujoco_simulator.py | 31 +++++++++--------- 4 files changed, 18 insertions(+), 17 deletions(-) rename resources/models/go2/{go2_moe_cts_137k_0.6745.pt => go2_moe_cts_137k_0.6819.pt} (100%) diff --git a/UPDATE.md b/UPDATE.md index 8eb5618..d1e2a66 100644 --- a/UPDATE.md +++ b/UPDATE.md @@ -2,6 +2,8 @@ ## 20260204 ### v1.1.2 1. 加入手柄JoystickGoal功能 +2. 修复mujoco_simulator中qvel速度问题，错误的使用freejoint将qvel投影到地面平面上，导致速度计算波动极大，重新计算全部的评估结果 +3. 将moe默认模型重命名为`go2_moe_cts_137k_0.6819.pt` ## 20260126 ### v1.1.1-rc2 1. 加入可视化当前速度（蓝），目标速度（绿）箭头 diff --git a/resources/models/go2/go2_moe_cts_137k_0.6745.pt b/resources/models/go2/go2_moe_cts_137k_0.6819.pt similarity index 100% rename from resources/models/go2/go2_moe_cts_137k_0.6745.pt rename to resources/models/go2/go2_moe_cts_137k_0.6819.pt diff --git a/robogauge/tasks/robots/go2/go2_moe_config.py b/robogauge/tasks/robots/go2/go2_moe_config.py index e30812f..7080f2a 100644 --- a/robogauge/tasks/robots/go2/go2_moe_config.py +++ b/robogauge/tasks/robots/go2/go2_moe_config.py @@ -13,7 +13,7 @@ class Go2MoEConfig(Go2Config): robot_class = 'Go2MoE' class control(Go2Config.control): - model_path = "{ROBOGAUGE_ROOT_DIR}/resources/models/go2/go2_moe_cts_137k_0.6745.pt" + model_path = "{ROBOGAUGE_ROOT_DIR}/resources/models/go2/go2_moe_cts_137k_0.6819.pt" save_additional_output = False class Go2MoETerrainConfig(Go2MoEConfig): diff --git a/robogauge/tasks/simulator/mujoco_simulator.py b/robogauge/tasks/simulator/mujoco_simulator.py index 7aad1cd..1b73113 100644 --- a/robogauge/tasks/simulator/mujoco_simulator.py +++ b/robogauge/tasks/simulator/mujoco_simulator.py @@ -48,10 +48,6 @@ class MujocoSimulator: self.target_pos = None self.target_velocity: Optional[VelocityGoal] = None self.penetration_reset_count = 0 - self.vis_smooth_factor = 0.01 - self.vis_cur_vel = np.zeros(3) - self.ren_smooth_factor = self.vis_smooth_factor / self.cfg.render.video_fps / self.cfg.physics.simulation_dt - self.ren_cur_vel = np.zeros(3) def load( self, @@ -95,8 +91,16 @@ class MujocoSimulator: for j in robot_mjcf.find_all('joint'): if j.tag == 'freejoint': j.remove() + robot_base = robot_mjcf.find('body', 'base_link') + if robot_base is not None: + origin_robot_height = robot_base.pos.copy() if robot_base.pos is not None else None + robot_base.pos = [0, 0, 0] # move base_link translation to terrain_spawn_pos + else: + raise ValueError("Robot base_link body not found in the robot MJCF model.") attachment_frame = terrain_mjcf.attach(robot_mjcf) - attachment_frame.add('freejoint') + attachment_frame.add('freejoint', name='root') + if origin_robot_height is not None: + terrain_spawn_pos = np.array(terrain_spawn_pos) + origin_robot_height attachment_frame.pos = terrain_spawn_pos self.close_viewer() @@ -112,7 +116,7 @@ class MujocoSimulator: self.mj_data.qpos[7:] = default_dof_pos # Domain randomization: base mass - base_body_name = f'{Path(self.robot_xml).stem}/base_link' + base_body_name = f'{robot_mjcf.model}/base_link' body_id = mujoco.mj_name2id(self.mj_model, mujoco.mjtObj.mjOBJ_BODY, base_body_name) assert body_id != -1, f"Body '{base_body_name}' not found in the model." if self.cfg.domain_rand.base_mass != 0.0: @@ -331,7 +335,7 @@ class MujocoSimulator: base_quat = self.mj_data.qpos[3:7] # rendering arrows start position - offset_body = np.array([0.0, 0.0, 0.6]) + offset_body = np.array([0.0, 0.0, 0.2]) offset_world = np.zeros(3) mujoco.mju_rotVecQuat(offset_world, offset_body, base_quat) start_pos = base_pos_world + offset_world @@ -341,20 +345,13 @@ class MujocoSimulator: raw_cur_vel = self.proprio.base.lin_vel cur_vel_body = np.array([raw_cur_vel[0], raw_cur_vel[1], 0.0]) - # EMA: v_smooth = alpha * v_new + (1 - alpha) * v_old - alpha = self.vis_smooth_factor if ctype == 'viewer' else self.ren_smooth_factor - if ctype == 'viewer': - self.vis_cur_vel = alpha * cur_vel_body + (1 - alpha) * self.vis_cur_vel - else: - self.ren_cur_vel = alpha * cur_vel_body + (1 - alpha) * self.ren_cur_vel - tgt_vel_world = np.zeros(3) cur_vel_world = np.zeros(3) mujoco.mju_rotVecQuat(tgt_vel_world, tgt_vel_body, base_quat) if ctype == 'viewer': - mujoco.mju_rotVecQuat(cur_vel_world, self.vis_cur_vel, base_quat) + mujoco.mju_rotVecQuat(cur_vel_world, cur_vel_body, base_quat) else: - mujoco.mju_rotVecQuat(cur_vel_world, self.ren_cur_vel, base_quat) + mujoco.mju_rotVecQuat(cur_vel_world, cur_vel_body, base_quat) COLOR_CMD = [0, 1, 0, 1] # Green 0x00ff00 COLOR_REAL = [0, 0, 1, 1] # Blue 0x0000ff @@ -533,6 +530,8 @@ class MujocoSimulator: name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_SENSOR, i) if pattern is None or name and pattern.search(name): found.append(name) + if len(found) == 0: + logger.warning(f"No sensors found for pattern='{pattern}' tag_name='{tag_name}'") return found def get_sensor_data(self, cache_key: str) -> np.ndarray: