v0.1.4

.gitignore

@@ -8,3 +8,4 @@ __pycache__/
 
 # Logger
 logs/
+.aim/

UPDATE.md

@@ -1,4 +1,8 @@
 # UPDATE
+## 20251130
+### v0.1.4
+1. 加入velocity_goals中的MaxVelocityGoal, 依次执行每种维度上的极值
+2. 加入metrics中的dof_limits_metric, 计算关节扭矩超过soft_dof_limit的比例
 ## 20251128
 ### v0.1.3
 1. 完成go2模型预测

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 .gauge_configs.flat_gauge_config import FlatGaugeConfig
-from .flat.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:
+    def is_reset(self, sim_data: SimData) -> bool:
-        return False
+        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:
+    def get_goal(self, sim_data: SimData) -> GoalData:
-        goal = GoalData(
+        # goal = GoalData(
-            goal_type='velocity',
+        #     goal_type='velocity',
-            velocity_goal=VelocityGoal(
+        #     velocity_goal=VelocityGoal(
-                lin_vel=[5.0, 0.0, 0.0],
+        #         ang_vel_yaw=-5.0,
-                ang_vel=[0.0, 0.0, 0.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
+    lin_vel_x: float = 0.0  # [m/s]
-    ang_vel: List[float]  # roll, pitch, yaw [rad/s], roll and pitch are ignored for ground robots
+    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

robogauge/tasks/pipeline/base_pipeline.py

@@ -27,7 +27,7 @@ class BasePipeline:
 
         self.sim: MujocoSimulator = eval(simulator_cfg.simulator_class)(simulator_cfg)
         self.robot: BaseRobot = eval(robot_cfg.robot_class)(robot_cfg)
-        self.gauge: BaseGauge = eval(gauge_cfg.gauge_class)(gauge_cfg)
+        self.gauge: BaseGauge = eval(gauge_cfg.gauge_class)(gauge_cfg, robot_cfg)
     
     def load(self):
         logger.create_tensorboard(self.run_name)
@@ -47,14 +47,16 @@ class BasePipeline:
             logger.info(f"Sim FPS: {1.0 / self.simulator_cfg.physics.simulation_dt:.2f}, Control FPS: {1.0 / self.robot_cfg.control.control_dt:.2f}, Frame Skip: {frame_skip:d}")
             logger.info("Running pipeline...")
             while not self.gauge.is_done():
-                goal = self.gauge.get_goal()
+                goal = self.gauge.get_goal(sim_data)
+                if goal is None:
+                    continue
                 obs = self.robot.build_observation(sim_data, goal)
                 action, p_gains, d_gains, control_type = self.robot.get_action(obs)
                 self.sim.setup_action(action, p_gains, d_gains, control_type)
                 for _ in range(frame_skip):
                     sim_data = self.sim.step()
                     self.gauge.update_metrics(sim_data)
-                if self.gauge.is_reset():
+                if self.gauge.is_reset(sim_data):
                     self.sim.reset()
                     sim_data = self.sim.step()
         finally:

robogauge/tasks/robots/base_robot_config.py

@@ -7,7 +7,8 @@
 @Blog    : https://wty-yy.github.io/
 @Desc    : Base Robot Configuration
 '''
-from typing_extensions import Literal
+from dataclasses import dataclass
+from typing import Optional, List
 from robogauge.utils.config import Config
 
 class RobotConfig(Config):
@@ -30,7 +31,6 @@ class RobotConfig(Config):
         num_observations = 45
         num_actions = 12
 
-        max_velocity_cmd = [1.5, 1.0, 2.0]
         default_dof_pos = [0.1,  0.8,  -1.5,  -0.1, 0.8, -1.5, 
                            0.1,  1.0,  -1.5,  -0.1, 1.0, -1.5] 
 
@@ -47,5 +47,7 @@ class RobotConfig(Config):
     class commands:
         lin_vel_x = [-1, 1]  # min max [m/s]
         lin_vel_y = [-1, 1]  # min max [m/s]
+        lin_vel_z = None  # min max [m/s]
+        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]
-    

robogauge/tasks/robots/go2/go2.py

@@ -19,7 +19,7 @@ from robogauge.utils.logger import logger
 class Go2(BaseRobot):
     def __init__(self, cfg: Go2Config):
         super().__init__(cfg)
-        self.max_velocity_cmd = np.array(cfg.control.max_velocity_cmd, dtype=np.float32)
+        self.cmd_range = cfg.commands
         self.default_dof_pos = np.array(cfg.control.default_dof_pos, dtype=np.float32)
         self.last_action = np.zeros(self.num_action, dtype=np.float32)
         self.action_scale = cfg.control.scales.action
@@ -35,8 +35,11 @@ class Go2(BaseRobot):
             dof_pos = (sim_proprio.joint.pos - self.default_dof_pos) * self.cfg.control.scales.dof_pos
             dof_vel = sim_proprio.joint.vel * self.cfg.control.scales.dof_vel
 
-            cmd = np.array(goal_data.velocity_goal.lin_vel[:2] + goal_data.velocity_goal.ang_vel[2:3], np.float32)
+            goal = goal_data.velocity_goal
-            cmd = np.minimum(np.maximum(cmd, -self.max_velocity_cmd), self.max_velocity_cmd)
+            cmd = np.array([goal.lin_vel_x, goal.lin_vel_y, goal.ang_vel_yaw], dtype=np.float32)
+            cmd = np.minimum(np.maximum(cmd, np.array([self.cmd_range.lin_vel_x[0], self.cmd_range.lin_vel_y[0], self.cmd_range.ang_vel_yaw[0]], dtype=np.float32)), 
+                             np.array([self.cmd_range.lin_vel_x[1], self.cmd_range.lin_vel_y[1], self.cmd_range.ang_vel_yaw[1]], dtype=np.float32))
+            
             cmd *= self.cfg.control.scales.cmd
 
             obs[:3] = ang_vel

robogauge/tasks/robots/go2/go2_config.py

@@ -20,6 +20,7 @@ class Go2Config(RobotConfig):
     class control(RobotConfig.control):
         device = 'cpu'
         torch_script_model_path = "{ROBOGAUGE_ROOT_DIR}/resources/models/go2/go2_cts_83501.pt"
+        # torch_script_model_path = "{ROBOGAUGE_ROOT_DIR}/resources/models/go2/go2_cts_cmd-1,1_38k.pt"
         control_dt = 0.02  # 50 Hz
         control_type = 'P'  # Position control
 
@@ -30,7 +31,6 @@ class Go2Config(RobotConfig):
         num_observations = 45
         num_actions = 12
 
-        max_velocity_cmd = [1.5, 1.0, 2.0]
         default_dof_pos = [0.1,  0.8,  -1.5,  -0.1, 0.8, -1.5, 
                            0.1,  1.0,  -1.5,  -0.1, 1.0, -1.5] 
         
@@ -45,6 +45,10 @@ class Go2Config(RobotConfig):
             cmd = [2.0, 2.0, 0.25]
 
     class commands(RobotConfig.commands):
-        lin_vel_x = [-1, 1]  # min max [m/s]
+        lin_vel_x = [-1.5, 1.5]  # min max [m/s]
         lin_vel_y = [-1, 1]  # min max [m/s]
-        ang_vel_yaw = [-1, 1]  # min max [rad/s]
+        lin_vel_z = None  # min max [m/s]
+        ang_vel_roll = None  # min max [rad/s]
+        ang_vel_pitch = None  # min max [rad/s]
+        ang_vel_yaw = [-2, 2]  # min max [rad/s]
+

robogauge/tasks/simulator/mujoco_simulator.py

@@ -117,6 +117,7 @@ class MujocoSimulator:
         self._pause = False
         self.n_step = 0
         self.sim_time = 0.0
+        self.load_dof_limits()
         self.preload_sensors()
 
 
@@ -159,6 +160,7 @@ class MujocoSimulator:
                 pos=self.get_sensor_data('joint_pos'),
                 vel=self.get_sensor_data('joint_vel'),
                 force=self.get_sensor_data('joint_eff'),
+                limits=self.dof_limits,
             ),
             imu=IMUState(
                 pos=self.get_sensor_data('imu_pos'),
@@ -184,6 +186,7 @@ class MujocoSimulator:
         sim_data = SimData(
             n_step=self.n_step,
             sim_dt=self.sim_dt,
+            sim_time=self.sim_time,
             proprio=proprio
         )
 
@@ -248,7 +251,9 @@ class MujocoSimulator:
         self.imu_lin_vel = self.find_sensors(tag_name="framelinvel")
         actuator_names = [mujoco.mj_id2name(self.mj_model, mujoco.mjtObj.mjOBJ_ACTUATOR, i) for i in range(self.mj_model.nu)]
         logger.info(
-            f"\n{'='*20} XML SENSOR NAMES {'='*20}\n"
+            f"""\nRobot XML: {self.robot_xml}\n"""
+            f"""Robot joint names: {[x.rsplit('/')[-1] for x in self.dof_names]}\n"""
+            f"""{'='*20} XML SENSOR NAMES {'='*20}\n"""
             f"""Joint Position Sensors [{len(self.joint_pos_sensor_names)}]: {[x.rsplit('/')[-1] for x in self.joint_pos_sensor_names]}\n"""
             f"""Joint Velocity Sensors [{len(self.joint_vel_sensor_names)}]: {[x.rsplit('/')[-1] for x in self.joint_vel_sensor_names]}\n"""
             f"""Joint Effort Sensors [{len(self.joint_eff_sensor_names)}]: {[x.rsplit('/')[-1] for x in self.joint_eff_sensor_names]}\n"""
@@ -351,3 +356,16 @@ class MujocoSimulator:
         logger.info(f"  imu.acc: { _shape(imu.acc) }")
         logger.info(f"  imu.pos: { _shape(imu.pos) }")
         logger.info(f"  imu.lin_vel: { _shape(imu.lin_vel) }")
+
+    def load_dof_limits(self):
+        self.dof_limits = []
+        self.dof_names = []
+        for i in range(self.mj_model.njnt):
+            name = mujoco.mj_id2name(self.mj_model, mujoco.mjtObj.mjOBJ_JOINT, i)
+            jnt_type = self.mj_model.jnt_type[i]
+            if jnt_type == mujoco.mjtJoint.mjJNT_FREE:
+                continue
+            limits = self.mj_model.jnt_range[i]
+            self.dof_limits.append(limits)
+            self.dof_names.append(name)
+        self.dof_limits = np.array(self.dof_limits, np.float32)

robogauge/tasks/simulator/sim_data.py

@@ -3,9 +3,10 @@ from dataclasses import dataclass
 
 @dataclass
 class JointState:
-    pos: np.ndarray
+    pos: np.ndarray     # [rad] shape (n_dof,)
-    vel: np.ndarray
+    vel: np.ndarray     # [rad/s] shape (n_dof,)
-    force: np.ndarray
+    force: np.ndarray   # [N*m] shape (n_dof,)
+    limits: np.ndarray  # [rad] shape (n_dof, 2), lower and upper limits
 
 @dataclass
 class BaseState:
@@ -32,4 +33,5 @@ class RobotProprioception:
 class SimData:
     n_step: int
     sim_dt: float
+    sim_time: float
     proprio: RobotProprioception

robogauge/utils/helpers.py

@@ -20,6 +20,7 @@ def parse_path(path):
     return path
 
 def class_to_dict(obj) -> dict:
+    # From https://github.com/leggedrobotics/legged_gym/blob/master/legged_gym/utils/helpers.py
     if not hasattr(obj, "__dict__"):
         return obj
     result = {}