cyy/RoboGauge
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

v1.1.1; fix base vel calc bug, fix last_torque reset bug

Browse Source
This commit is contained in:
wty-yy
2026-01-13 12:12:43 +08:00
parent a82f3d8930
commit a96ce5e350
7 changed files with 26 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
robogauge/tasks/gauge/base_gauge.py
View File

@@ -39,7 +39,7 @@ class BaseGauge:
self.goal_str = "Init"
self.goal_idx = 0
self.goals: List[BaseGoal] = []
self.metrics: List[function] = []
self.metrics: List[BaseMetric] = []
self.info = {'goal': [], 'metric': []}
self.results = {} # {'goal/sub_goal': {'metric': result}}
@@ -134,7 +134,7 @@ class BaseGauge:
self.create_new_goal_logger()
def update_metrics(self, sim_data: SimData, goal_data: GoalData):
if sim_data.n_step % int(self.cfg.metrics.metric_dt / sim_data.sim_dt) != 0:
if sim_data.n_step % int(self.cfg.metrics.metric_dt / sim_data.sim_dt + 1e-9) != 0:
return
metrics_results = {}
for metric_name, metric_obj in zip(self.info['metric'], self.metrics):
@@ -142,6 +142,10 @@ class BaseGauge:
if metric_name not in ['visualization']:
metrics_results[metric_name] = val
self.goals[self.goal_idx].update_metrics(metrics_results)
def reset_metrics(self):
for metric in self.metrics:
metric.reset()
def save_results(self):
""" Save the results to a yaml file. """

4
robogauge/tasks/gauge/metrics/vel_metrics.py
View File

@@ -28,6 +28,8 @@ class LinVelErrMetric(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("[LinVelErrMetric] No linear velocity commands found in robot configuration.")
self.norm_vel = np.linalg.norm(max_ranges)
def __call__(self, sim_data: SimData, goal_data: GoalData) -> 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:

1
robogauge/tasks/pipeline/base_pipeline.py
View File

@@ -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):

14
robogauge/tasks/simulator/mujoco_simulator.py
View File

@@ -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()