From 9f7db42f9fceaf1e6c5a4edb50b008534bf7e9f3 Mon Sep 17 00:00:00 2001 From: wty-yy <993660140@qq.com> Date: Wed, 3 Dec 2025 09:43:16 +0800 Subject: [PATCH] v0.1.6 --- README.md | 34 ++++-- UPDATE.md | 3 + robogauge/tasks/custom/go2_flat_task.py | 4 + robogauge/tasks/gauge/base_gauge.py | 14 ++- robogauge/tasks/gauge/base_gauge_config.py | 8 +- .../gauge/gauge_configs/flat_gauge_config.py | 9 +- robogauge/tasks/gauge/goal_data.py | 10 ++ robogauge/tasks/gauge/goals/__init__.py | 2 +- robogauge/tasks/gauge/goals/base_goal.py | 35 +++--- robogauge/tasks/gauge/goals/velocity_goals.py | 107 ++++++++++++++---- robogauge/utils/helpers.py | 6 +- robogauge/utils/logger.py | 7 +- 12 files changed, 179 insertions(+), 60 deletions(-) diff --git a/README.md b/README.md index 76ad117..2c9b396 100644 --- a/README.md +++ b/README.md @@ -17,7 +17,7 @@ - `robogauge/tasks`: 定义测试任务 - `robogauge/utils`: 常用工具 -## 指标 +## 指标/目标/地形 指标的计算方法是通过在环境中发送固定指令及持续时长, 通过Mujoco获取所需参数并计算. ### 环境参数 @@ -29,21 +29,41 @@ | 电机动作执行随机延迟 | `action delay` | `<= RL控制间隔` | | base负重 | `base mass` | `(-1, 5) kg` | -#### 地面 +#### 地形 1. 支持legged_gym中的部分地形, 包括: `wave, slope, rough_slope, stairs up, stairs down, obstacles, flat`, 除`flat`地形外其他地形可进行难度系数提升 2. 地面类型 (影响接触摩擦系数, 弹性摩擦系数), 包括: 橡胶地, 木地板, 瓷砖地 -### 速度追踪 +### 指标 +目前在每个`env.step`后可度量的指标, 所有指标均要求**越大越好**, 目前支持: + +| # | 指标名称 Metrics | 描述 | 包含的超参数 | 归一化系数 | 变化 | +| 1 | `dof_limits` | 关节超出软关节范围的大小 | 软关节范围阈值 | 总关节变化范围 | `1-x` | +| 2 | `lin_vel_err` | 线速度L2误差 | NA | 总线速度指令范围 | `1-x` | +| 3 | `ang_vel_err` | 角速度L2误差 | NA | 总角速度指令范围 | `1-x` | +| 4 | `base_height_std` | base高度变化方差 | NA | NA | `1-x` | +| 5 | `dof_power` | 电机耗能 | NA | 10 | `1-x` | + +### 速度追踪目标 针对在虚实迁移中发现的问题, 整理指标内容如下: -| # | 指标 | 标准化范围 | 对应真机问题 | 地形 | +| # | 描述 | 标准化范围 | 对应真机问题 | 地形 | | - | - | - | - | - | | 1 | 关节出现极端值的比例 | 关节范围 | 移动时发生危险的高抬腿行为 | Any | | 2 | 线速度与指令速度的L2误差 | 最大线速度指令 | 移动时可能无法达到指定速度 | Any | | 3 | 角速度与指令速度的L2误差 | 最大角速度指令 | 移动时可能无法达到指定速度 | Any | -| 4 | base高度变化 | 固定高度 | 高速移动时机身存在趴低问题 | 平地 | -| 5 | 速度对角突变base高度变化 | 固定高度 | 速度发生对角突变时无法平衡 | 平地 | -| 6 | 高速移动急停稳定性 | 固定用时 | 楼梯上静止时, 关节不稳定 | Any | +| 4 | 高速移动/速度对角突变base高度变化 | 固定高度 | 高速移动时机身存在趴低问题, 速度发生对角突变时无法平衡 | 平地 | +| 5 | 高速移动急停稳定性 | 固定用时 | 楼梯上静止时, 关节不稳定 | Any | + +总结速度最总目标如下: + +| # | 目标名称 Goals | 描述 | reset条件 | 最大reset次数 | +| 1 | `max_velocity` | 单一维度的最大线/角速度 | 每次执行一个维度的指令 | 6 | +| 2 | `diagonal_velocity` | 对角线速度变化 | 每次执行一对对角指令 | 6 | +| 3 | `move_stance` | 全线速度移动急停 | 每次执行一个方向的指令, 再急停 | 6 | + +1. `max_velocity`: 单一维度的最大线/角速度, 每次reset只执行单一指令 +2. `digonal_velocity`: 对角线速度变化, 每次reset执行一对指令, 总reset +3. `move_stance`: ## 创建新任务 评测任务注册在[`tasks/__init__.py`](./robogauge/tasks/__init__.py)中完成, 包含四个部分: diff --git a/UPDATE.md b/UPDATE.md index 4e0f625..d9c427f 100644 --- a/UPDATE.md +++ b/UPDATE.md @@ -1,4 +1,7 @@ # UPDATE +## 20251202 +### v0.1.6 +1. 加入新目标`diagonal_velocity`, 记录的信息中仅保留总goal的metrics信息, metrics加入@25, @50两个后25%和50%的平均值 ## 20251201 ### v0.1.5 1. 加入goals, metrics结果存储 diff --git a/robogauge/tasks/custom/go2_flat_task.py b/robogauge/tasks/custom/go2_flat_task.py index 1bfd07e..345e9ef 100644 --- a/robogauge/tasks/custom/go2_flat_task.py +++ b/robogauge/tasks/custom/go2_flat_task.py @@ -12,3 +12,7 @@ class Go2FlatGaugeConfig(FlatGaugeConfig): class max_velocity(FlatGaugeConfig.goals.max_velocity): enabled = True cmd_duration = 5.0 + + class diagonal_velocity(FlatGaugeConfig.goals.diagonal_velocity): + enabled = True + cmd_duration = 6.0 diff --git a/robogauge/tasks/gauge/base_gauge.py b/robogauge/tasks/gauge/base_gauge.py index 7cf6ed6..19591c4 100644 --- a/robogauge/tasks/gauge/base_gauge.py +++ b/robogauge/tasks/gauge/base_gauge.py @@ -20,7 +20,7 @@ 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.tasks.gauge.goals import BaseGoal, MaxVelocityGoal +from robogauge.tasks.gauge.goals import BaseGoal, MaxVelocityGoal, DiagonalVelocityGoal from robogauge.tasks.gauge.metrics import * class BaseGauge: @@ -43,6 +43,9 @@ class BaseGauge: if name == 'max_velocity': self.goals.append(MaxVelocityGoal(robot_cfg.commands, **kwargs)) log_str += f" - Max Velocity Goal: {kwargs}\n" + elif name == 'diagonal_velocity': + self.goals.append(DiagonalVelocityGoal(robot_cfg.commands, **kwargs)) + log_str += f" - Diagonal Velocity Goal: {kwargs}\n" else: raise NotImplementedError(f"Goal '{name}' is not implemented in BaseGauge.") self.info['goal'].append(name) @@ -94,7 +97,6 @@ class BaseGauge: goal = goal_obj.get_goal(sim_data) if goal is None: # goal obj finished - self.results[str(goal_obj)] = goal_obj.sub_goal_mean_metrics self.results[goal_obj.name] = goal_obj.goal_mean_metrics self.goal_idx += 1 self.create_new_goal_logger() @@ -102,8 +104,6 @@ class BaseGauge: now_goal_str = str(goal_obj) if now_goal_str != self.goal_str: # sub goal changed - if self.goal_str != "": - self.results[self.goal_str] = goal_obj.sub_goal_mean_metrics self.goal_str = now_goal_str logger.info(f"New Goal [{self.goal_idx+1}/{len(self.goals)}] [{goal_obj.count+1}/{goal_obj.total}]: {self.goal_str}") return goal @@ -123,4 +123,10 @@ class BaseGauge: save_path = Path(logger.log_dir) / "results.yaml" with open(save_path, 'w') as file: yaml.dump(self.results, file) + yaml_str = yaml.dump(self.results) + logger.info( + f"""\n{'='*20} Goals and Metrics results {'='*20}\n""" + f"""{yaml_str}""" + f"""{'='*68}""" + ) logger.info(f"Saved metric results to {save_path}") diff --git a/robogauge/tasks/gauge/base_gauge_config.py b/robogauge/tasks/gauge/base_gauge_config.py index b3172eb..06664a4 100644 --- a/robogauge/tasks/gauge/base_gauge_config.py +++ b/robogauge/tasks/gauge/base_gauge_config.py @@ -19,10 +19,14 @@ class BaseGaugeConfig(Config): class goals: class max_velocity: # goal with maximum velocity enabled = True - cmd_duration = 5.0 # duration for each velocity command [s] + cmd_duration = 5.0 # [s] duration for each velocity command + + 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: - metric_dt = 0.1 # [s], frequency to compute metrics + metric_dt = 0.1 # [s] frequency to compute metrics class dof_limits: enabled = True soft_dof_limit_ratio = 0.9 diff --git a/robogauge/tasks/gauge/gauge_configs/flat_gauge_config.py b/robogauge/tasks/gauge/gauge_configs/flat_gauge_config.py index fdc11d5..3774c1b 100644 --- a/robogauge/tasks/gauge/gauge_configs/flat_gauge_config.py +++ b/robogauge/tasks/gauge/gauge_configs/flat_gauge_config.py @@ -19,10 +19,13 @@ class FlatGaugeConfig(BaseGaugeConfig): class goals: class max_velocity: # goal with maximum velocity enabled = True - cmd_duration = 5.0 # duration for each velocity command [s] - + cmd_duration = 5.0 # [s] duration for each velocity command + + 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: - metric_dt = 0.1 # [s], frequency to compute metrics + metric_dt = 0.1 # [s] frequency to compute metrics class dof_limits: enabled = True soft_dof_limit_ratio = 0.9 diff --git a/robogauge/tasks/gauge/goal_data.py b/robogauge/tasks/gauge/goal_data.py index b06ea67..713296c 100644 --- a/robogauge/tasks/gauge/goal_data.py +++ b/robogauge/tasks/gauge/goal_data.py @@ -17,6 +17,16 @@ class VelocityGoal: if getattr(self, field) != 0.0: s += f"{field}={getattr(self, field):.1f}_" return s[:-1] if s else "stance" + + def invert(self): + return VelocityGoal( + lin_vel_x = -self.lin_vel_x, + lin_vel_y = -self.lin_vel_y, + lin_vel_z = -self.lin_vel_z, + ang_vel_roll = -self.ang_vel_roll, + ang_vel_pitch = -self.ang_vel_pitch, + ang_vel_yaw = -self.ang_vel_yaw, + ) @dataclass class PositionGoal: diff --git a/robogauge/tasks/gauge/goals/__init__.py b/robogauge/tasks/gauge/goals/__init__.py index 1bef328..2e6d1c3 100644 --- a/robogauge/tasks/gauge/goals/__init__.py +++ b/robogauge/tasks/gauge/goals/__init__.py @@ -1,2 +1,2 @@ from robogauge.tasks.gauge.goals.base_goal import BaseGoal -from robogauge.tasks.gauge.goals.velocity_goals import MaxVelocityGoal +from robogauge.tasks.gauge.goals.velocity_goals import MaxVelocityGoal, DiagonalVelocityGoal diff --git a/robogauge/tasks/gauge/goals/base_goal.py b/robogauge/tasks/gauge/goals/base_goal.py index 89e525b..7a9a940 100644 --- a/robogauge/tasks/gauge/goals/base_goal.py +++ b/robogauge/tasks/gauge/goals/base_goal.py @@ -21,9 +21,7 @@ class BaseGoal: self.total = 0 self.sub_name = None - self._goal_mean_metrics = defaultdict(Average) - self.last_sub_name = None - self._sub_goal_mean_metrics = defaultdict(Average) + self._goal_mean_metrics = defaultdict(list) def is_done(self) -> bool: raise NotImplementedError @@ -40,22 +38,25 @@ class BaseGoal: return f"{self.name}/{self.sub_name}" def update_metrics(self, metrics: dict): - """ Update step metrics for the current goal and sub-goal.""" - if self.last_sub_name is None or self.last_sub_name != self.sub_name: - self.last_sub_name = self.sub_name - self._sub_goal_mean_metrics = defaultdict(Average) + """ Update step metrics for the current goal.""" for metric_name, value in metrics.items(): - self._goal_mean_metrics[metric_name].update(value) - self._sub_goal_mean_metrics[metric_name].update(value) + self._goal_mean_metrics[metric_name].append(value) @property def goal_mean_metrics(self): """ Get the mean metrics for the current goal. """ - return {k: float(v.mean) for k, v in self._goal_mean_metrics.items()} - - @property - def sub_goal_mean_metrics(self): - """ Get the mean metrics for the current sub-goal. """ - if len(self._sub_goal_mean_metrics) == 0: - return {} - return {k: float(v.mean) for k, v in self._sub_goal_mean_metrics.items()} + return {k: self._analysis_metrics(v) for k, v in self._goal_mean_metrics.items()} + + @staticmethod + def _analysis_metrics(metrics: list): + result = {'mean': 0} + for i in [25, 50]: + result[f'mean@{i}'] = 0 + if len(metrics) == 0: + return result + metrics.sort() + result['mean'] = float(sum(metrics) / len(metrics)) + for i in [25, 50]: + count = max(1, int(len(metrics) * i / 100)) + result[f'mean@{i}'] = float(sum(metrics[:count]) / count) + return result diff --git a/robogauge/tasks/gauge/goals/velocity_goals.py b/robogauge/tasks/gauge/goals/velocity_goals.py index 96e17e9..5e5b1db 100644 --- a/robogauge/tasks/gauge/goals/velocity_goals.py +++ b/robogauge/tasks/gauge/goals/velocity_goals.py @@ -7,6 +7,7 @@ @Blog : https://wty-yy.github.io/ @Desc : Velocity Goals Implementation ''' +from copy import deepcopy from typing import Optional from robogauge.tasks.gauge.goals import BaseGoal @@ -16,33 +17,15 @@ 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): - name = "max_velocity" +class BaseVelocityGoal(BaseGoal): + name = "base_velocity_goal" - """ Goal class for maximizing velocity commands. """ - def __init__(self, max_velocity: RobotConfig.commands, cmd_duration: float = 5, **kwargs): + def __init__(self, cmd_duration: float = 5, **kwargs): super().__init__() - 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.goal_start_time = None self.last_reset_time = 0.0 - self.goals = [] - for key in ['lin_vel_x', 'lin_vel_y', 'lin_vel_z', 'ang_vel_roll', 'ang_vel_pitch', 'ang_vel_yaw']: - if self.max_velocity.get(key) is None: continue - for value in self.max_velocity[key]: - if value != 0: - self.goals.append(VelocityGoal(**{key: value})) - - self.count = 0 - self.total = len(self.goals) - if len(self.goals) == 0: - logger.warning("MaxVelocityGoal initialized with no valid velocity commands.") - return - self.sub_name = str(self.goals[0]) def is_reset(self, sim_data: SimData) -> bool: if sim_data.sim_time - self.last_reset_time >= self.cmd_duration: @@ -51,7 +34,9 @@ class MaxVelocityGoal(BaseGoal): return False def get_goal(self, sim_data: SimData) -> Optional[GoalData]: - self.count = int(sim_data.sim_time / self.cmd_duration) + if self.goal_start_time is None: + self.goal_start_time = sim_data.sim_time + self.count = int((sim_data.sim_time - self.goal_start_time) / self.cmd_duration) if self.count >= self.total: return None self.current_goal = self.goals[self.count] @@ -60,3 +45,79 @@ class MaxVelocityGoal(BaseGoal): goal_type='velocity', velocity_goal=self.current_goal ) + +class MaxVelocityGoal(BaseVelocityGoal): + name = "max_velocity" + + """ Goal class for maximizing velocity commands. """ + def __init__(self, max_velocity: RobotConfig.commands, cmd_duration: float = 5, **kwargs): + super().__init__(cmd_duration=cmd_duration) + kwargs.pop('enabled', None) + if kwargs: + logger.warning(f"Unused kwargs in MaxVelocityGoal: {kwargs}") + self.max_velocity = class_to_dict(max_velocity) + + self.goals = [] + for key in ['lin_vel_x', 'lin_vel_y', 'lin_vel_z', 'ang_vel_roll', 'ang_vel_pitch', 'ang_vel_yaw']: + if self.max_velocity.get(key) is None: continue + for value in self.max_velocity[key]: + if value != 0: + self.goals.append(VelocityGoal(**{key: value})) + + # self.goals = self.goals[:2] + self.count = 0 + self.total = len(self.goals) + if len(self.goals) == 0: + logger.warning("MaxVelocityGoal initialized with no valid velocity commands.") + return + self.sub_name = str(self.goals[0]) + +class DiagonalVelocityGoal(BaseVelocityGoal): + name = "diagonal_velocity" + + def __init__(self, max_velocity: RobotConfig.commands, cmd_duration: float = 6, **kwargs): + """ Goal class for diagonal velocity changes. + Args: + max_velocity (RobotConfig.commands): Maximum velocity commands. + cmd_duration (float, optional): Duration for a pair of diagonal commands. + """ + super().__init__(cmd_duration=cmd_duration) + kwargs.pop('enabled', None) + if kwargs: + logger.warning(f"Unused kwargs in DiagonalVelocityGoal: {kwargs}") + + self.cmd_duration = cmd_duration + self.last_reset_time = 0.0 + + self.goals = [] + lin_vel_x_vals = max_velocity.lin_vel_x + [0] + lin_vel_y_vals = max_velocity.lin_vel_y + [0] + + for lx in lin_vel_x_vals: + for ly in lin_vel_y_vals: + if lx == 0 and ly == 0: + continue + self.goals.append(VelocityGoal(lin_vel_x=lx, lin_vel_y=ly)) + + # self.goals = self.goals[:2] + self.count = 0 + self.total = len(self.goals) + if len(self.goals) == 0: + logger.warning("DiagonalVelocityGoal initialized with no valid diagonal velocity commands.") + return + self.sub_name = str(self.goals[0]) + + def get_goal(self, sim_data: SimData) -> Optional[GoalData]: + if self.goal_start_time is None: + self.goal_start_time = sim_data.sim_time + self.count = int((sim_data.sim_time - self.goal_start_time) / self.cmd_duration) + if self.count >= self.total: + return None + self.current_goal = self.goals[self.count] + if sim_data.sim_time - self.count * self.cmd_duration >= self.cmd_duration / 2: + self.current_goal = self.current_goal.invert() + self.sub_name = str(self.current_goal) + return GoalData( + goal_type='velocity', + velocity_goal=self.current_goal + ) diff --git a/robogauge/utils/helpers.py b/robogauge/utils/helpers.py index dacb78b..e0f528b 100644 --- a/robogauge/utils/helpers.py +++ b/robogauge/utils/helpers.py @@ -58,6 +58,8 @@ def parse_args(): for param in parameters: parser.add_argument(param['name'], **{k: v for k, v in param.items() if k != 'name'}) args = parser.parse_args() - if args.experiment_name is None: - args.experiment_name = f"exp" + if args.experiment_name is not None: + args.experiment_name = f"{args.task_name}_{args.experiment_name}" + else: + args.experiment_name = args.task_name return args diff --git a/robogauge/utils/logger.py b/robogauge/utils/logger.py index 07a561c..b22ed78 100644 --- a/robogauge/utils/logger.py +++ b/robogauge/utils/logger.py @@ -104,10 +104,15 @@ class Logger: self.logger.addHandler(fh) self.info(f"Logs saved at: {path_log_file}") + def get_data_path(self, robot_name: str, model_name: str, goal_name: str) -> Path: + data_path = Path(ROBOGAUGE_LOGS_DIR) / self.experiment_name / 'data' / robot_name / model_name / goal_name / self.tag + data_path.mkdir(parents=True, exist_ok=True) + return data_path + def create_tensorboard(self, robot_name: str, model_name: str, goal_name: str): if self.writer is not None: self.writer.close() - data_path = Path(ROBOGAUGE_LOGS_DIR) / self.experiment_name / 'data' / robot_name / model_name / goal_name / self.tag + data_path = self.get_data_path(robot_name, model_name, goal_name) self.writer = SummaryWriter(str(data_path)) self.info(f"Tensorboard writer created at: {data_path}")