v0.1.6.1

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` |

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%的平均值

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")
+            metric_class_name = f"{snake_to_pascal(name)}Metric"
-            self.metrics.append(partial(metric_func, robot_cfg=robot_cfg, **self.metrics_cfg[name]))
+            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):
+        for metric_name, metric_obj in zip(self.info['metric'], self.metrics):
-            val = metric_func(sim_data)
+            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)

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

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

robogauge/tasks/gauge/metrics/__init__.py

@@ -1,70 +1,4 @@
-import numpy as np
+from .base_metric import BaseMetric
-
+from .dof_metrics import DofLimitsMetric
-from robogauge.tasks.robots import RobotConfig
+from .visualization import VisualizationMetric
-from robogauge.tasks.simulator.sim_data import SimData
+from .vel_metrics import LinVelErrMetric, AngVelErrMetric
-
-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

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

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

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

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

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)

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)