diff --git a/UPDATE.md b/UPDATE.md index f770da6..1f26c3f 100644 --- a/UPDATE.md +++ b/UPDATE.md @@ -1,4 +1,8 @@ # UPDATE +## 20260113 +### v1.1.1 +1. 修复base lin vel计算错误，错误将世界坐标系下的速度作为了body坐标系下的速度，导致关键指标计算错误，重新评估 +2. 修复TorqueSmoothnessMetric中last_torque未在episode重置时重置的问题 ## 20260110 ### v1.1.0 1. 修复三个Metric计算错误 diff --git a/resources/robots/go2/go2.xml b/resources/robots/go2/go2.xml index 77b209a..7dffde5 100644 --- a/resources/robots/go2/go2.xml +++ b/resources/robots/go2/go2.xml @@ -64,6 +64,7 @@ + float: @@ -52,6 +54,8 @@ class AngVelErrMetric(BaseMetric): cmds = cfg_commands.get(name) if cmds is not None: max_ranges.append(max(abs(cmds[0]), abs(cmds[1]))) + if not max_ranges: + raise ValueError("[AngVelErrMetric] No angular velocity commands found in robot configuration.") self.norm_vel = np.linalg.norm(max_ranges) def __call__(self, sim_data: SimData, goal_data: GoalData) -> float: diff --git a/robogauge/tasks/pipeline/base_pipeline.py b/robogauge/tasks/pipeline/base_pipeline.py index dc6ddbe..74f51ab 100644 --- a/robogauge/tasks/pipeline/base_pipeline.py +++ b/robogauge/tasks/pipeline/base_pipeline.py @@ -137,6 +137,7 @@ class BasePipeline: self.first_reset = True sim_data = self.sim.step() self.robot.reset() + self.gauge.reset_metrics() return sim_data def add_noise(self, sim_data: SimData): diff --git a/robogauge/tasks/simulator/mujoco_simulator.py b/robogauge/tasks/simulator/mujoco_simulator.py index b562a48..afdc251 100644 --- a/robogauge/tasks/simulator/mujoco_simulator.py +++ b/robogauge/tasks/simulator/mujoco_simulator.py @@ -20,7 +20,7 @@ from typing import Literal, List from robogauge.utils.logger import logger from robogauge.utils.helpers import parse_path -from robogauge.utils.math_utils import get_projected_gravity +from robogauge.utils.math_utils import get_projected_gravity, quat_rotate_inverse from robogauge.tasks.simulator.mujoco_config import MujocoConfig from robogauge.tasks.simulator.sim_data import ( SimData, @@ -248,20 +248,24 @@ class MujocoSimulator: pos=self.get_sensor_data('imu_pos'), quat=self.get_sensor_data('imu_quat'), acc=self.get_sensor_data('imu_acc'), - lin_vel=self.get_sensor_data('imu_lin_vel'), - ang_vel=self.get_sensor_data('imu_ang_vel'), + lin_vel=self.get_sensor_data('imu_lin_vel'), # body frame, check direction, go2 is inverted + ang_vel=self.get_sensor_data('imu_ang_vel'), # body frame, check direction, go2 is inverted ), base=BaseState( pos=self.mj_data.qpos[:3], # world frame quat=self.mj_data.qpos[3:7], # world frame - lin_vel=self.mj_data.qvel[:3], # body frame - ang_vel=self.mj_data.qvel[3:6], # body frame + lin_vel=quat_rotate_inverse(self.mj_data.qpos[3:7], self.mj_data.qvel[:3]), # body frame + ang_vel=quat_rotate_inverse(self.mj_data.qpos[3:7], self.mj_data.qvel[3:6]), # body frame ) ) if self.n_step % int(0.1 / self.sim_dt) == 0: logger.log(value=np.mean(proprio.imu.quat - proprio.base.quat), tag="sim/delta_quat", step=self.n_step) logger.log(value=np.mean(proprio.imu.ang_vel - proprio.base.ang_vel), tag="sim/delta_ang_vel", step=self.n_step) logger.log(value=np.mean(proprio.imu.lin_vel - proprio.base.lin_vel), tag="sim/delta_lin_vel", step=self.n_step) + logger.log(value=proprio.imu.lin_vel[0], tag="sim/imu_lin_vel_x", step=self.n_step) + logger.log(value=proprio.imu.lin_vel[1], tag="sim/imu_lin_vel_y", step=self.n_step) + logger.log(value=proprio.base.lin_vel[0], tag="sim/base_lin_vel_x", step=self.n_step) + logger.log(value=proprio.base.lin_vel[1], tag="sim/base_lin_vel_y", step=self.n_step) if self.n_step == 0: self.debug_print_proprio_shapes() diff --git a/setup.py b/setup.py index fca4209..ab370d6 100644 --- a/setup.py +++ b/setup.py @@ -2,7 +2,7 @@ from setuptools import setup, find_packages setup( name="robogauge", # 包名 - version="1.1.0", # 版本号 + version="1.1.1", # 版本号 author="Wu Tianyang", # 你的名字 author_email="993660140@qq.com", description="A generic robot RL model evaluation library based on MuJoCo",