v0.1.10

robogauge/tasks/gauge/metrics/__init__.py

@@ -1,4 +1,5 @@
 from .base_metric import BaseMetric
-from .dof_metrics import DofLimitsMetric
+from .dof_metrics import DofLimitsMetric, DofPowerMetric
 from .visualization import VisualizationMetric
 from .vel_metrics import LinVelErrMetric, AngVelErrMetric
+from .stable_metric import OrientationStabilityMetric, TorqueSmoothnessMetric

robogauge/tasks/gauge/metrics/base_metric.py

@@ -1,3 +1,12 @@
+# -*- coding: utf-8 -*-
+'''
+@File    : base_metric.py
+@Time    : 2025/12/18 20:18:45
+@Author  : wty-yy
+@Version : 1.0
+@Blog    : https://wty-yy.github.io/
+@Desc    : Base Metric Implementation
+'''
 from robogauge.utils.logger import logger
 from robogauge.tasks.robots import RobotConfig
 from robogauge.tasks.simulator.sim_data import SimData
@@ -9,6 +18,9 @@ class BaseMetric:
 
     def __init__(self, robot_cfg: RobotConfig, **kwargs):
         self.robot_cfg = robot_cfg
+    
+    def reset(self):
+        pass
 
     def __call__(self, sim_data: SimData, goal_data: GoalData) -> float:
         value = 0.0

robogauge/tasks/gauge/metrics/dof_metrics.py

@@ -1,3 +1,12 @@
+# -*- coding: utf-8 -*-
+'''
+@File    : dof_metrics.py
+@Time    : 2025/12/18 20:18:24
+@Author  : wty-yy
+@Version : 1.0
+@Blog    : https://wty-yy.github.io/
+@Desc    : DOF Limits Metric Implementation
+'''
 import numpy as np
 
 from robogauge.tasks.robots import RobotConfig
@@ -47,3 +56,29 @@ class DofLimitsMetric(BaseMetric):
         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
+
+class DofPowerMetric(BaseMetric):
+    """ Metric to log DOF power efficiency. """
+    name = 'dof_power_metric'
+
+    def __init__(self,
+        robot_cfg: RobotConfig,
+        scaling_factor: float = 100.0,
+        **kwargs
+    ):
+        super().__init__(robot_cfg)
+        self.scaling_factor = scaling_factor
+    
+    def __call__(self, sim_data: SimData, goal_data: GoalData) -> float:
+        values = []
+        for i in range(len(sim_data.proprio.joint.torque)):
+            torque = sim_data.proprio.joint.torque[i]
+            velocity = sim_data.proprio.joint.vel[i]
+            power = abs(torque * velocity)
+            values.append(power)
+            dof_name = sim_data.proprio.joint.names[i]
+            logger.log(power, f'dof_power/{dof_name}', step=sim_data.n_step)
+        rms_power = np.sqrt(np.mean(np.square(values)))
+        metric_power = 1 - rms_power / self.scaling_factor
+        logger.log(rms_power, f'dof_power/rms', step=sim_data.n_step)
+        return metric_power

robogauge/tasks/gauge/metrics/stable_metric.py

@@ -0,0 +1,61 @@
+# -*- coding: utf-8 -*-
+'''
+@File    : height_metric.py
+@Time    : 2025/12/18 20:18:33
+@Author  : wty-yy
+@Version : 1.0
+@Blog    : https://wty-yy.github.io/
+@Desc    : Orientation Stability, Torque Smoothness Metric Implementation
+'''
+import numpy as np
+
+from robogauge.tasks.robots import RobotConfig
+from robogauge.tasks.gauge.metrics.base_metric import BaseMetric, GoalData, SimData
+from robogauge.utils.math_utils import get_projected_gravity
+
+from robogauge.utils.logger import logger
+
+
+class OrientationStabilityMetric(BaseMetric):
+    """ Metric to log height stability. """
+    name = 'height_std_metric'
+
+    def __init__(self,
+        robot_cfg: RobotConfig,
+        **kwargs
+    ):
+        super().__init__(robot_cfg)
+    
+    def __call__(self, sim_data: SimData, goal_data: GoalData) -> float:
+        projected_gravity = get_projected_gravity(sim_data.proprio.base.quat)
+        projected_x = projected_gravity[0]
+        metric_value = 1 - abs(projected_x)  # consider roll only
+        logger.log(abs(projected_x), f'stable_metric/projected_x_abs', step=sim_data.n_step)
+        return metric_value
+
+class TorqueSmoothnessMetric(BaseMetric):
+    """ Metric to log torque smoothness. """
+    name = 'torque_smoothness_metric'
+
+    def __init__(self,
+        robot_cfg: RobotConfig,
+        scaling_factor: float = 30.0,
+        **kwargs
+    ):
+        super().__init__(robot_cfg)
+        self.last_torque = None
+        self.scaling_factor = scaling_factor
+    
+    def reset(self):
+        self.last_torque = None
+    
+    def __call__(self, sim_data: SimData, goal_data: GoalData) -> float:
+        current_torque = np.array(sim_data.proprio.joint.torque, np.float32)
+        if self.last_torque is None:
+            self.last_torque = current_torque
+            return 1.0  # No change at first step
+        torque_diff = current_torque - self.last_torque
+        rms_value = np.sqrt(np.mean(np.square(torque_diff)))
+        metric_value = 1.0 - rms_value / self.scaling_factor
+        logger.log(rms_value, f'stable_metric/torque_rms_diff', step=sim_data.n_step)
+        return metric_value

robogauge/tasks/gauge/metrics/vel_metrics.py

@@ -1,3 +1,12 @@
+# -*- coding: utf-8 -*-
+'''
+@File    : vel_metrics.py
+@Time    : 2025/12/18 20:18:56
+@Author  : wty-yy
+@Version : 1.0
+@Blog    : https://wty-yy.github.io/
+@Desc    : Velocity Metrics Implementation
+'''
 import numpy as np
 
 from robogauge.tasks.robots import RobotConfig

robogauge/tasks/gauge/metrics/visualization.py

@@ -1,3 +1,12 @@
+# -*- coding: utf-8 -*-
+'''
+@File    : visualization.py
+@Time    : 2025/12/18 20:19:09
+@Author  : wty-yy
+@Version : 1.0
+@Blog    : https://wty-yy.github.io/
+@Desc    : Visualization Metric Implementation
+'''
 from robogauge.tasks.robots import RobotConfig
 from robogauge.tasks.gauge.metrics.base_metric import BaseMetric, SimData, GoalData
 
@@ -9,20 +18,20 @@ class VisualizationMetric(BaseMetric):
 
     def __init__(self,
         robot_cfg: RobotConfig,
-        dof_force: bool = False,
+        dof_torque: bool = False,
         dof_pos: bool = False,
         **kwargs
     ):
         super().__init__(robot_cfg)
-        self.dof_force = dof_force
+        self.dof_torque = dof_torque
         self.dof_pos = dof_pos
 
     def __call__(self, sim_data: SimData, goal_data: GoalData) -> float:
-        for i in range(len(sim_data.proprio.joint.force)):
+        for i in range(len(sim_data.proprio.joint.torque)):
             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_torque:
+                torque = sim_data.proprio.joint.torque[i]
+                logger.log(torque, f'dof_torque/{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)