From d2ceb72eecd55bdfb0a201f4f9de311137e42792 Mon Sep 17 00:00:00 2001 From: wty-yy Date: Sat, 6 Dec 2025 04:21:52 +0800 Subject: [PATCH] v0.1.6.1 --- README.md | 1 + UPDATE.md | 3 + robogauge/tasks/gauge/base_gauge.py | 13 ++-- robogauge/tasks/gauge/base_gauge_config.py | 6 ++ .../gauge/gauge_configs/flat_gauge_config.py | 9 ++- robogauge/tasks/gauge/metrics/__init__.py | 74 +------------------ robogauge/tasks/gauge/metrics/base_metric.py | 15 ++++ robogauge/tasks/gauge/metrics/dof_metrics.py | 50 +++++++++++++ robogauge/tasks/gauge/metrics/vel_metrics.py | 64 ++++++++++++++++ .../tasks/gauge/metrics/visualization.py | 30 ++++++++ robogauge/tasks/robots/base_robot_config.py | 5 ++ robogauge/utils/helpers.py | 5 ++ 12 files changed, 198 insertions(+), 77 deletions(-) create mode 100644 robogauge/tasks/gauge/metrics/base_metric.py create mode 100644 robogauge/tasks/gauge/metrics/dof_metrics.py create mode 100644 robogauge/tasks/gauge/metrics/vel_metrics.py create mode 100644 robogauge/tasks/gauge/metrics/visualization.py diff --git a/README.md b/README.md index 2c9b396..aa0d957 100644 --- a/README.md +++ b/README.md @@ -37,6 +37,7 @@ 目前在每个`env.step`后可度量的指标, 所有指标均要求**越大越好**, 目前支持: | # | 指标名称 Metrics | 描述 | 包含的超参数 | 归一化系数 | 变化 | +| - | - | - | - | - | - | | 1 | `dof_limits` | 关节超出软关节范围的大小 | 软关节范围阈值 | 总关节变化范围 | `1-x` | | 2 | `lin_vel_err` | 线速度L2误差 | NA | 总线速度指令范围 | `1-x` | | 3 | `ang_vel_err` | 角速度L2误差 | NA | 总角速度指令范围 | `1-x` | diff --git a/UPDATE.md b/UPDATE.md index d9c427f..4601d1d 100644 --- a/UPDATE.md +++ b/UPDATE.md @@ -1,4 +1,7 @@ # UPDATE +## 20251203 +### v0.1.6.1 +1. 加入lin_vel, ang_vel err指标 ## 20251202 ### v0.1.6 1. 加入新目标`diagonal_velocity`, 记录的信息中仅保留总goal的metrics信息, metrics加入@25, @50两个后25%和50%的平均值 diff --git a/robogauge/tasks/gauge/base_gauge.py b/robogauge/tasks/gauge/base_gauge.py index 19591c4..e2de0ad 100644 --- a/robogauge/tasks/gauge/base_gauge.py +++ b/robogauge/tasks/gauge/base_gauge.py @@ -13,7 +13,7 @@ from pathlib import Path from functools import partial from robogauge.utils.logger import logger -from robogauge.utils.helpers import class_to_dict +from robogauge.utils.helpers import class_to_dict, snake_to_pascal from robogauge.tasks.robots import RobotConfig from robogauge.tasks.gauge.base_gauge_config import BaseGaugeConfig @@ -52,10 +52,11 @@ class BaseGauge: for name, enabled in self.metrics_cfg.items(): if not enabled: continue if name in ['metric_dt']: continue - metric_func = eval(f"{name}_metric") - self.metrics.append(partial(metric_func, robot_cfg=robot_cfg, **self.metrics_cfg[name])) + metric_class_name = f"{snake_to_pascal(name)}Metric" + metric_class = eval(metric_class_name) + self.metrics.append(metric_class(robot_cfg=robot_cfg, **self.metrics_cfg[name])) log_str += f" - Metric: {name}\n" - self.info['metric'].append(name) + self.info['metric'].append(metric_class_name) logger.info(log_str.strip()) if len(self.goals) == 0: @@ -112,8 +113,8 @@ class BaseGauge: if sim_data.n_step % int(self.cfg.metrics.metric_dt / sim_data.sim_dt) != 0: return metrics_results = {} - for metric_name, metric_func in zip(self.info['metric'], self.metrics): - val = metric_func(sim_data) + for metric_name, metric_obj in zip(self.info['metric'], self.metrics): + val = metric_obj(sim_data) if metric_name not in ['visualization']: metrics_results[metric_name] = val self.goals[self.goal_idx].update_metrics(metrics_results) diff --git a/robogauge/tasks/gauge/base_gauge_config.py b/robogauge/tasks/gauge/base_gauge_config.py index 06664a4..1bf338b 100644 --- a/robogauge/tasks/gauge/base_gauge_config.py +++ b/robogauge/tasks/gauge/base_gauge_config.py @@ -36,3 +36,9 @@ class BaseGaugeConfig(Config): enabled = True dof_force = True dof_pos = True + + class lin_vel_err: + enabled = True + + class ang_vel_err: + enabled = True diff --git a/robogauge/tasks/gauge/gauge_configs/flat_gauge_config.py b/robogauge/tasks/gauge/gauge_configs/flat_gauge_config.py index 3774c1b..b6f74f5 100644 --- a/robogauge/tasks/gauge/gauge_configs/flat_gauge_config.py +++ b/robogauge/tasks/gauge/gauge_configs/flat_gauge_config.py @@ -24,7 +24,8 @@ class FlatGaugeConfig(BaseGaugeConfig): class diagonal_velocity: # goal with diagonal velocity changes enabled = True cmd_duration = 6.0 # [s] duration for a pair of diagonal velocity commands - class metrics: + + class metrics(BaseGaugeConfig.metrics): metric_dt = 0.1 # [s] frequency to compute metrics class dof_limits: enabled = True @@ -35,3 +36,9 @@ class FlatGaugeConfig(BaseGaugeConfig): enabled = True dof_force = True dof_pos = True + + class lin_vel_err: + enabled = True + + class ang_vel_err: + enabled = True diff --git a/robogauge/tasks/gauge/metrics/__init__.py b/robogauge/tasks/gauge/metrics/__init__.py index 16db731..469cb4d 100644 --- a/robogauge/tasks/gauge/metrics/__init__.py +++ b/robogauge/tasks/gauge/metrics/__init__.py @@ -1,70 +1,4 @@ -import numpy as np - -from robogauge.tasks.robots import RobotConfig -from robogauge.tasks.simulator.sim_data import SimData - -from robogauge.utils.logger import logger - -def example_metric( - sim_data: SimData, - robot_cfg: RobotConfig, - **kwargs -) -> float: - """ An example metric function. """ - value = 0.0 - # Compute some metric value based on sim_data and robot_cfg - logger.log(value, 'example_metric', step=sim_data.n_step) - return value - -def dof_limits_metric( - sim_data: SimData, - robot_cfg: RobotConfig, - soft_dof_limit_ratio: float = 0.9, - dof_names: list = None, - **kwargs -) -> float: - """ Metric to log DOF limit violations. """ - values = [] - for i in range(len(sim_data.proprio.joint.limits)): - lower_limit = sim_data.proprio.joint.limits[i, 0] - upper_limit = sim_data.proprio.joint.limits[i, 1] - dof_range = upper_limit - lower_limit - soft_lower_limit = lower_limit + (1 - soft_dof_limit_ratio) * dof_range - soft_upper_limit = upper_limit - (1 - soft_dof_limit_ratio) * dof_range - - pos = sim_data.proprio.joint.pos[i] - dof_name = sim_data.proprio.joint.names[i] - value = 0 - if pos < soft_lower_limit: - value = soft_lower_limit - pos - elif pos > soft_upper_limit: - value = pos - soft_upper_limit - value /= dof_range # Normalize by DOF range - logger.log(value, f'dof_limits/{dof_name}', step=sim_data.n_step) - if dof_names is not None: - for use_name in dof_names: - if use_name in dof_name: - values.append(value) - else: - values.append(value) - rms_value = 1 - np.sqrt(np.mean(np.square(values))) - logger.log(1 - rms_value, f'dof_limits/rms', step=sim_data.n_step) - return rms_value - -def visualization_metric( - sim_data: SimData, - robot_cfg: RobotConfig, - dof_force: bool = False, - dof_pos: bool = False, - **kwargs, -): - """ Metric to visualize various robot states in the simulator. """ - for i in range(len(sim_data.proprio.joint.force)): - name = sim_data.proprio.joint.names[i] - if dof_force: - force = sim_data.proprio.joint.force[i] - logger.log(force, f'dof_force/{name}', step=sim_data.n_step) - if dof_pos: - pos = sim_data.proprio.joint.pos[i] - logger.log(pos, f'dof_pos/{name}', step=sim_data.n_step) - return 0.0 +from .base_metric import BaseMetric +from .dof_metrics import DofLimitsMetric +from .visualization import VisualizationMetric +from .vel_metrics import LinVelErrMetric, AngVelErrMetric diff --git a/robogauge/tasks/gauge/metrics/base_metric.py b/robogauge/tasks/gauge/metrics/base_metric.py new file mode 100644 index 0000000..5258204 --- /dev/null +++ b/robogauge/tasks/gauge/metrics/base_metric.py @@ -0,0 +1,15 @@ +from robogauge.utils.logger import logger +from robogauge.tasks.robots import RobotConfig +from robogauge.tasks.simulator.sim_data import SimData + +class BaseMetric: + """ Base class for all metric functions. """ + name = 'base_metric' + + def __init__(self, robot_cfg: RobotConfig, **kwargs): + self.robot_cfg = robot_cfg + + def __call__(self, sim_data: SimData) -> float: + value = 0.0 + logger.log(value, self.name, step=sim_data.n_step) + return value diff --git a/robogauge/tasks/gauge/metrics/dof_metrics.py b/robogauge/tasks/gauge/metrics/dof_metrics.py new file mode 100644 index 0000000..47752b7 --- /dev/null +++ b/robogauge/tasks/gauge/metrics/dof_metrics.py @@ -0,0 +1,50 @@ +import numpy as np + +from robogauge.tasks.robots import RobotConfig +from robogauge.tasks.simulator.sim_data import SimData +from robogauge.tasks.gauge.metrics.base_metric import BaseMetric + +from robogauge.utils.logger import logger + + +class DofLimitsMetric(BaseMetric): + """ Metric to log DOF limit violations. """ + name = 'dof_limits_metric' + + def __init__(self, + robot_cfg: RobotConfig, + soft_dof_limit_ratio: float = 0.9, + dof_names: list = None, + **kwargs + ): + super().__init__(robot_cfg) + self.soft_dof_limit_ratio = soft_dof_limit_ratio + self.calc_dof_names = dof_names + + def __call__(self, sim_data: SimData) -> float: + values = [] + for i in range(len(sim_data.proprio.joint.limits)): + lower_limit = sim_data.proprio.joint.limits[i, 0] + upper_limit = sim_data.proprio.joint.limits[i, 1] + dof_range = upper_limit - lower_limit + soft_lower_limit = lower_limit + (1 - self.soft_dof_limit_ratio) * dof_range + soft_upper_limit = upper_limit - (1 - self.soft_dof_limit_ratio) * dof_range + + pos = sim_data.proprio.joint.pos[i] + dof_name = sim_data.proprio.joint.names[i] + value = 0 + if pos < soft_lower_limit: + value = soft_lower_limit - pos + elif pos > soft_upper_limit: + value = pos - soft_upper_limit + value /= dof_range # Normalize by DOF range + logger.log(value, f'dof_limits/{dof_name}', step=sim_data.n_step) + if self.calc_dof_names is not None: + for use_name in self.calc_dof_names: + if use_name in dof_name: + values.append(value) + else: + values.append(value) + rms_value = 1 - np.sqrt(np.mean(np.square(values))) + logger.log(1 - rms_value, f'dof_limits/rms', step=sim_data.n_step) + return rms_value diff --git a/robogauge/tasks/gauge/metrics/vel_metrics.py b/robogauge/tasks/gauge/metrics/vel_metrics.py new file mode 100644 index 0000000..2d0f431 --- /dev/null +++ b/robogauge/tasks/gauge/metrics/vel_metrics.py @@ -0,0 +1,64 @@ +import numpy as np + +from robogauge.tasks.robots import RobotConfig +from robogauge.tasks.simulator.sim_data import SimData +from robogauge.tasks.gauge.metrics.base_metric import BaseMetric +from robogauge.tasks.gauge.goals.base_goal import GoalData + +from robogauge.utils.logger import logger + + +class LinVelErrMetric(BaseMetric): + """ Metric to log linear velocity error. """ + name = 'lin_vel_err_metric' + + def __init__(self, robot_cfg: RobotConfig, **kwargs): + super().__init__(robot_cfg) + max_ranges = [] + for name in ['lin_vel_x', 'lin_vel_y', 'lin_vel_z']: + max_ranges.append( + max( + abs(getattr(robot_cfg.commands, name)[0]), + abs(getattr(robot_cfg.commands, name)[1]) + ) + ) + self.norm_vel = np.linalg.norm(max_ranges) + + def __call__(self, sim_data: SimData, goal_data: GoalData) -> float: + if goal_data.goal_type != 'velocity': + logger.warning("LinVelErrMetric can only be used with VelocityGoal.") + return 0.0 + lin_vel = sim_data.proprio.base.lin_vel + target_lin_vel = [goal_data.velocity_goal.lin_vel_x, goal_data.velocity_goal.lin_vel_y, goal_data.velocity_goal.lin_vel_z] + vel_err = np.linalg.norm(lin_vel - target_lin_vel) / self.norm_vel + logger.log(vel_err, f'vel_metrics/lin_vel_err', step=sim_data.n_step) + return 1 - vel_err + +class AngVelErrMetric(BaseMetric): + """ Metric to log angular velocity error. """ + name = 'ang_vel_err_metric' + + def __init__(self, robot_cfg: RobotConfig, **kwargs): + super().__init__(robot_cfg) + max_ranges = [] + for name in ['ang_vel_roll', 'ang_vel_pitch', 'ang_vel_yaw']: + cmd_range = getattr(robot_cfg.commands, name) + if cmd_range is None: + cmd_range = [0, 0] + max_ranges.append( + max( + abs(cmd_range[0]), + abs(cmd_range[1]) + ) + ) + self.norm_vel = np.linalg.norm(max_ranges) + + def __call__(self, sim_data: SimData, goal_data: GoalData) -> float: + if goal_data.goal_type != 'velocity': + logger.warning("AngVelErrMetric can only be used with VelocityGoal.") + return 0.0 + ang_vel = sim_data.proprio.base.ang_vel + target_ang_vel = [goal_data.velocity_goal.ang_vel_roll, goal_data.velocity_goal.ang_vel_pitch, goal_data.velocity_goal.ang_vel_yaw] + vel_err = np.linalg.norm(ang_vel - target_ang_vel) / self.norm_vel + logger.log(vel_err, f'vel_metrics/ang_vel_err', step=sim_data.n_step) + return 1 - vel_err diff --git a/robogauge/tasks/gauge/metrics/visualization.py b/robogauge/tasks/gauge/metrics/visualization.py new file mode 100644 index 0000000..bfe9584 --- /dev/null +++ b/robogauge/tasks/gauge/metrics/visualization.py @@ -0,0 +1,30 @@ +from robogauge.tasks.robots import RobotConfig +from robogauge.tasks.simulator.sim_data import SimData +from robogauge.tasks.gauge.metrics.base_metric import BaseMetric + +from robogauge.utils.logger import logger + +class VisualizationMetric(BaseMetric): + """ Metric to visualize various robot states in the simulator. """ + name = 'visualization_metric' + + def __init__(self, + robot_cfg: RobotConfig, + dof_force: bool = False, + dof_pos: bool = False, + **kwargs + ): + super().__init__(robot_cfg) + self.dof_force = dof_force + self.dof_pos = dof_pos + + def __call__(self, sim_data: SimData) -> float: + for i in range(len(sim_data.proprio.joint.force)): + name = sim_data.proprio.joint.names[i] + if self.dof_force: + force = sim_data.proprio.joint.force[i] + logger.log(force, f'dof_force/{name}', step=sim_data.n_step) + if self.dof_pos: + pos = sim_data.proprio.joint.pos[i] + logger.log(pos, f'dof_pos/{name}', step=sim_data.n_step) + return 0.0 diff --git a/robogauge/tasks/robots/base_robot_config.py b/robogauge/tasks/robots/base_robot_config.py index ccf4901..53a35a3 100644 --- a/robogauge/tasks/robots/base_robot_config.py +++ b/robogauge/tasks/robots/base_robot_config.py @@ -52,3 +52,8 @@ class RobotConfig(Config): ang_vel_roll = None # min max [rad/s] ang_vel_pitch = None # min max [rad/s] ang_vel_yaw = [-1, 1] # min max [rad/s] + +if __name__ == '__main__': + cfg = RobotConfig() + print(getattr(cfg.commands, 'lin_vel_x')) + print(cfg.commands.lin_vel_x) diff --git a/robogauge/utils/helpers.py b/robogauge/utils/helpers.py index e0f528b..b2342fd 100644 --- a/robogauge/utils/helpers.py +++ b/robogauge/utils/helpers.py @@ -63,3 +63,8 @@ def parse_args(): else: args.experiment_name = args.task_name return args + +def snake_to_pascal(s: str) -> str: + """ 'snake_case' to 'PascalCase' conversion """ + parts = [p for p in s.split('_') if p] + return ''.join(p[0].upper() + p[1:] if p else '' for p in parts)