v0.1.4

robogauge/tasks/gauge/__init__.py

@@ -1,4 +1,3 @@
 from .base_gauge import BaseGauge
 from .base_gauge_config import BaseGaugeConfig
-from .flat.flat_gauge import FlatGauge
-from .flat.flat_gauge_config import FlatGaugeConfig
+from .gauge_configs.flat_gauge_config import FlatGaugeConfig

robogauge/tasks/gauge/base_gauge.py

@@ -7,30 +7,82 @@
 @Blog    : https://wty-yy.github.io/
 @Desc    : Base Gauge for Robogauge
 '''
-from robogauge.tasks.robots.base_robot_config import RobotConfig
+from typing import List
+from functools import partial
+
+from robogauge.utils.logger import logger
+from robogauge.utils.helpers import class_to_dict
+
+from robogauge.tasks.robots import RobotConfig
 from robogauge.tasks.gauge.base_gauge_config import BaseGaugeConfig
 from robogauge.tasks.gauge.goal_data import GoalData, VelocityGoal, PositionGoal
 from robogauge.tasks.simulator.sim_data import SimData
-from robogauge.utils.logger import logger
+
+from robogauge.tasks.gauge.goals import BaseGoal, MaxVelocityGoal
+from robogauge.tasks.gauge.metrics import dof_limits_metric
 
 class BaseGauge:
-    def __init__(self, cfg: BaseGaugeConfig):
+    def __init__(self, cfg: BaseGaugeConfig, robot_cfg: RobotConfig):
         self.cfg = cfg
+        self.goals_cfg = class_to_dict(self.cfg.goals)
+        self.metrics_cfg = class_to_dict(self.cfg.metrics)
+
+        self.goal_str = ""
+        self.goal_idx = 0
+        self.goals: List[BaseGoal] = []
+        self.metrics: List[function] = []
+        self.info = {
+            'goal': [],
+            'metric': [],
+        }
+
+        log_str = "Initialized Gauge with Goals:\n"
+        for name, kwargs in self.goals_cfg.items():
+            if not kwargs['enabled']: continue
+            if name == 'max_velocity':
+                self.goals.append(MaxVelocityGoal(robot_cfg.commands, **kwargs))
+                log_str += f"  - Max Velocity Goal: {kwargs}\n"
+            else:
+                raise NotImplementedError(f"Goal '{name}' is not implemented in BaseGauge.")
+            self.info['goal'].append(name)
+        for name, enabled in self.metrics_cfg.items():
+            if not enabled: continue
+            metric_func = eval(f"{name}_metric")
+            self.metrics.append(partial(metric_func, robot_cfg=robot_cfg, **self.metrics_cfg[name]))
+            log_str += f"  - Metric: {name}\n"
+            self.info['metric'].append(name)
+        logger.info(log_str.strip())
     
-    def is_reset(self) -> bool:
-        return False
+    def is_reset(self, sim_data: SimData) -> bool:
+        if self.goal_idx >= len(self.goals):
+            return False
+        return self.goals[self.goal_idx].is_reset(sim_data)
     
     def is_done(self) -> bool:
+        if self.goal_idx >= len(self.goals):
+            return True
         return False
     
-    def get_goal(self) -> GoalData:
-        goal = GoalData(
-            goal_type='velocity',
-            velocity_goal=VelocityGoal(
-                lin_vel=[5.0, 0.0, 0.0],
-                ang_vel=[0.0, 0.0, 0.0]
-            )
-        )
+    def get_goal(self, sim_data: SimData) -> GoalData:
+        # goal = GoalData(
+        #     goal_type='velocity',
+        #     velocity_goal=VelocityGoal(
+        #         ang_vel_yaw=-5.0,
+        #     )
+        # )
+        if self.goal_idx >= len(self.goals):
+            logger.error("All goals have been exhausted.")
+            return None
+        goal_instance = self.goals[self.goal_idx]
+        goal = goal_instance.get_goal(sim_data)
+        if goal is None:
+            self.goal_idx += 1
+            return None
+
+        now_goal_str = str(goal_instance)
+        if now_goal_str != self.goal_str:
+            self.goal_str = now_goal_str
+            logger.info(f"New Goal [{self.goal_idx+1}/{len(self.goals)}] [{goal_instance.count+1}/{goal_instance.total}]: {self.goal_str}")
         return goal
     
     def update_metrics(self, sim_data: SimData):
@@ -38,4 +90,6 @@ class BaseGauge:
             return
         for i in range(len(sim_data.proprio.joint.force)):
             logger.log(sim_data.proprio.joint.force[i], f'dof/force_{i}', step=sim_data.n_step)
+        for metric_func in self.metrics:
+            metric_func(sim_data)
     

robogauge/tasks/gauge/base_gauge_config.py

@@ -15,9 +15,16 @@ class BaseGaugeConfig(Config):
     class assets:
         terrain_xml = '{ROBOGAUGE_ROOT_DIR}/resources/terrains/flat.xml'
         terrain_spawn_xy = [0, 0]  # x y [m]
+    
+    class goals:
+        class max_velocity:  # goal with maximum velocity
+            enabled = True
+            cmd_duration = 3.0  # duration for each velocity command [s]
 
     class metrics:
-        dof_limits = True
+        class dof_limits:
+            enabled = True
+            soft_dof_limit_ratio = 0.9
     
     class commands:
         stance = True

robogauge/tasks/gauge/flat/flat_gauge.py

@@ -1,13 +0,0 @@
-# -*- coding: utf-8 -*-
-'''
-@File    : flat_gauge.py
-@Time    : 2025/11/27 16:03:11
-@Author  : wty-yy
-@Version : 1.0
-@Blog    : https://wty-yy.github.io/
-@Desc    : Flat Gauge Implementation
-'''
-from robogauge.tasks.gauge.base_gauge import BaseGauge
-
-class FlatGauge(BaseGauge):
-    ...

robogauge/tasks/gauge/flat/flat_gauge_config.py

@@ -1,13 +0,0 @@
-# -*- coding: utf-8 -*-
-'''
-@File    : flat_gauge_config.py
-@Time    : 2025/11/27 16:03:02
-@Author  : wty-yy
-@Version : 1.0
-@Blog    : https://wty-yy.github.io/
-@Desc    : Flat Gauge Configuration
-'''
-from robogauge.tasks.gauge.base_gauge_config import BaseGaugeConfig
-
-class FlatGaugeConfig(BaseGaugeConfig):
-    ...

robogauge/tasks/gauge/gauge_configs/flat_gauge_config.py

@@ -0,0 +1,30 @@
+# -*- coding: utf-8 -*-
+'''
+@File    : flat_gauge_config.py
+@Time    : 2025/11/27 16:03:02
+@Author  : wty-yy
+@Version : 1.0
+@Blog    : https://wty-yy.github.io/
+@Desc    : Flat Gauge Configuration
+'''
+from robogauge.tasks.gauge.base_gauge_config import BaseGaugeConfig
+
+class FlatGaugeConfig(BaseGaugeConfig):
+    gauge_class = 'BaseGauge'
+
+    class assets:
+        terrain_xml = '{ROBOGAUGE_ROOT_DIR}/resources/terrains/flat.xml'
+        terrain_spawn_xy = [0, 0]  # x y [m]
+
+    class goals:
+        max_velocity = True  # goal with maximum velocity
+
+    class metrics:
+        dof_limits = True
+    
+    class commands:
+        stance = True
+        max_lin_vel = True
+        diagonal_lin_vel = True
+    
+

robogauge/tasks/gauge/goal_data.py

@@ -4,8 +4,19 @@ from typing import List, Optional, Literal
 
 @dataclass
 class VelocityGoal:
-    lin_vel: List[float]  # x, y, z [m/s], z is ignored for ground robots
-    ang_vel: List[float]  # roll, pitch, yaw [rad/s], roll and pitch are ignored for ground robots
+    lin_vel_x: float = 0.0  # [m/s]
+    lin_vel_y: float = 0.0  # [m/s]
+    lin_vel_z: float = 0.0  # [m/s]
+    ang_vel_roll: float = 0.0  # [rad/s]
+    ang_vel_pitch: float = 0.0  # [rad/s]
+    ang_vel_yaw: float = 0.0  # [rad/s]
+    
+    def __repr__(self):
+        s = ""
+        for field in self.__dataclass_fields__:
+            if getattr(self, field) != 0.0:
+                s += f"{field}={getattr(self, field):.1f}, "
+        return s[:-2] if s else "stance"
 
 @dataclass
 class PositionGoal:

robogauge/tasks/gauge/goals/__init__.py

@@ -0,0 +1,2 @@
+from robogauge.tasks.gauge.goals.base_goal import BaseGoal
+from robogauge.tasks.gauge.goals.velocity_goals import MaxVelocityGoal

robogauge/tasks/gauge/goals/base_goal.py

@@ -0,0 +1,25 @@
+# -*- coding: utf-8 -*-
+'''
+@File    : base_goals.py
+@Time    : 2025/11/30 21:52:59
+@Author  : wty-yy
+@Version : 1.0
+@Blog    : https://wty-yy.github.io/
+@Desc    : Base Goal Class
+'''
+
+from robogauge.tasks.simulator.sim_data import SimData
+from robogauge.tasks.gauge.goal_data import GoalData
+
+class BaseGoal:
+    count = 0
+    total = 0
+
+    def is_done(self) -> bool:
+        raise NotImplementedError
+
+    def is_reset(self, sim_data: SimData) -> bool:
+        raise NotImplementedError
+
+    def get_goal(self, sim_data: SimData) -> GoalData:
+        raise NotImplementedError

robogauge/tasks/gauge/goals/velocity_goals.py

@@ -0,0 +1,57 @@
+# -*- coding: utf-8 -*-
+'''
+@File    : velocity_goals.py
+@Time    : 2025/11/30 21:44:13
+@Author  : wty-yy
+@Version : 1.0
+@Blog    : https://wty-yy.github.io/
+@Desc    : Velocity Goals Implementation
+'''
+from typing import Optional
+
+from robogauge.tasks.gauge.goals import BaseGoal
+from robogauge.tasks.robots import RobotConfig
+from robogauge.tasks.simulator.sim_data import SimData
+from robogauge.tasks.gauge.goal_data import GoalData, VelocityGoal
+from robogauge.utils.helpers import class_to_dict
+from robogauge.utils.logger import logger
+
+class MaxVelocityGoal(BaseGoal):
+    """ Goal class for maximizing velocity commands. """
+    def __init__(self, max_velocity: RobotConfig.commands, cmd_duration: float = 5, **kwargs):
+        kwargs.pop('enabled', None)
+        if kwargs:
+            logger.warning(f"Unused kwargs in MaxVelocityGoal: {kwargs}")
+
+        self.max_velocity = class_to_dict(max_velocity)
+        self.cmd_duration = cmd_duration
+        self.last_reset_time = 0.0
+
+        self.goals = []
+        for key, min_max in self.max_velocity.items():
+            if min_max is None: continue
+            for value in min_max:
+                if value != 0:
+                    self.goals.append(VelocityGoal(**{key: value}))
+        
+        self.count = 0
+        self.total = len(self.goals)
+
+    def is_reset(self, sim_data: SimData) -> bool:
+        if sim_data.sim_time - self.last_reset_time >= self.cmd_duration:
+            self.last_reset_time = sim_data.sim_time
+            return True
+        return False
+
+    def get_goal(self, sim_data: SimData) -> Optional[GoalData]:
+        self.count = int(sim_data.sim_time / self.cmd_duration)
+        if self.count >= self.total:
+            return None
+        self.current_goal = self.goals[self.count]
+        return GoalData(
+            goal_type='velocity',
+            velocity_goal=self.current_goal
+        )
+    
+    def __repr__(self):
+        return f"{self.current_goal}"

robogauge/tasks/gauge/metrics/__init__.py

@@ -0,0 +1,43 @@
+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,
+    **kwargs
+) -> float:
+    """ Metric to log DOF limit violations. """
+    mean_value = 0.0
+    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 + soft_dof_limit_ratio * dof_range
+        soft_upper_limit = upper_limit - soft_dof_limit_ratio * dof_range
+
+        pos = sim_data.proprio.joint.pos[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/{i}', step=sim_data.n_step)
+        mean_value += value
+    mean_value /= len(sim_data.proprio.joint.limits)
+    logger.log(mean_value, f'dof_limits/mean', step=sim_data.n_step)
+    return mean_value