cyy/RoboGauge
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

v0.1.5

Browse Source
This commit is contained in:
wty-yy
2025-12-01 18:01:12 +08:00
parent a91ea5b62e
commit ad907089a8
24 changed files with 324 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files

67
robogauge/tasks/gauge/base_gauge.py
View File

@@ -7,7 +7,9 @@
@Blog : https://wty-yy.github.io/
@Desc : Base Gauge for Robogauge
'''
import yaml
from typing import List
from pathlib import Path
from functools import partial
from robogauge.utils.logger import logger
@@ -19,11 +21,12 @@ 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.metrics import dof_limits_metric
from robogauge.tasks.gauge.metrics import *
class BaseGauge:
def __init__(self, cfg: BaseGaugeConfig, robot_cfg: RobotConfig):
self.cfg = cfg
self.robot_cfg = robot_cfg
self.goals_cfg = class_to_dict(self.cfg.goals)
self.metrics_cfg = class_to_dict(self.cfg.metrics)
@@ -31,12 +34,10 @@ class BaseGauge:
self.goal_idx = 0
self.goals: List[BaseGoal] = []
self.metrics: List[function] = []
self.info = {
'goal': [],
'metric': [],
}
self.info = {'goal': [], 'metric': []}
self.results = {} # {'goal/sub_goal': {'metric': result}}
log_str = "Initialized Gauge with Goals:\n"
log_str = "Initialized Gauge with Goals and Metrics:\n"
for name, kwargs in self.goals_cfg.items():
if not kwargs['enabled']: continue
if name == 'max_velocity':
@@ -47,11 +48,17 @@ class BaseGauge:
self.info['goal'].append(name)
for name, enabled in self.metrics_cfg.items():
if not enabled: continue
if name in ['metric_dt']: 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())
if len(self.goals) == 0:
logger.warning("No goals have been configured for the Gauge. Exiting.")
else:
self.create_new_goal_logger()
def is_reset(self, sim_data: SimData) -> bool:
if self.goal_idx >= len(self.goals):
@@ -60,8 +67,18 @@ class BaseGauge:
def is_done(self) -> bool:
if self.goal_idx >= len(self.goals):
self.save_results()
return True
return False
def create_new_goal_logger(self):
""" Create a new logger for new goal to metrics. """
if self.goal_idx >= len(self.goals): return
logger.create_tensorboard(
self.robot_cfg.robot_name,
Path(self.robot_cfg.control.model_path).stem,
self.goals[self.goal_idx].name
)
def get_goal(self, sim_data: SimData) -> GoalData:
# goal = GoalData(
@@ -73,23 +90,37 @@ class BaseGauge:
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:
goal_obj = self.goals[self.goal_idx]
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()
return None
now_goal_str = str(goal_instance)
if now_goal_str != self.goal_str:
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_instance.count+1}/{goal_instance.total}]: {self.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
def update_metrics(self, sim_data: SimData):
if sim_data.n_step % int(0.1 / sim_data.sim_dt) != 0:
if sim_data.n_step % int(self.cfg.metrics.metric_dt / sim_data.sim_dt) != 0:
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)
metrics_results = {}
for metric_name, metric_func in zip(self.info['metric'], self.metrics):
val = metric_func(sim_data)
if metric_name not in ['visualization']:
metrics_results[metric_name] = val
self.goals[self.goal_idx].update_metrics(metrics_results)
def save_results(self):
""" Save the results to a yaml file. """
save_path = Path(logger.log_dir) / "results.yaml"
with open(save_path, 'w') as file:
yaml.dump(self.results, file)
logger.info(f"Saved metric results to {save_path}")

17
robogauge/tasks/gauge/base_gauge_config.py
View File

@@ -14,20 +14,21 @@ class BaseGaugeConfig(Config):
class assets:
terrain_xml = '{ROBOGAUGE_ROOT_DIR}/resources/terrains/flat.xml'
terrain_spawn_xy = [0, 0] # x y [m]
terrain_spawn_pos = [0, 0, 0] # x y z [m], robot freejoint spawn position on the terrain
class goals:
class max_velocity: # goal with maximum velocity
enabled = True
cmd_duration = 3.0 # duration for each velocity command [s]
cmd_duration = 5.0 # duration for each velocity command [s]
class metrics:
metric_dt = 0.1 # [s], frequency to compute metrics
class dof_limits:
enabled = True
soft_dof_limit_ratio = 0.9
class commands:
stance = True
max_lin_vel = True
diagonal_lin_vel = True
dof_names = None # List of DOF names to monitor, None for all
class visualization:
enabled = True
dof_force = True
dof_pos = True

26
robogauge/tasks/gauge/gauge_configs/flat_gauge_config.py
View File

@@ -14,17 +14,21 @@ class FlatGaugeConfig(BaseGaugeConfig):
class assets:
terrain_xml = '{ROBOGAUGE_ROOT_DIR}/resources/terrains/flat.xml'
terrain_spawn_xy = [0, 0] # x y [m]
terrain_spawn_pos = [0, 0, 0] # x y z [m], robot freejoint spawn position on the terrain
class goals:
max_velocity = True # goal with maximum velocity
class max_velocity: # goal with maximum velocity
enabled = True
cmd_duration = 5.0 # duration for each velocity command [s]
class metrics:
dof_limits = True
class commands:
stance = True
max_lin_vel = True
diagonal_lin_vel = True
metric_dt = 0.1 # [s], frequency to compute metrics
class dof_limits:
enabled = True
soft_dof_limit_ratio = 0.9
dof_names = None # List of DOF names to monitor, None for all
class visualization:
enabled = True
dof_force = True
dof_pos = True

4
robogauge/tasks/gauge/goal_data.py
View File

@@ -15,8 +15,8 @@ class VelocityGoal:
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"
s += f"{field}={getattr(self, field):.1f}_"
return s[:-1] if s else "stance"
@dataclass
class PositionGoal:

42
robogauge/tasks/gauge/goals/base_goal.py
View File

@@ -8,12 +8,22 @@
@Desc : Base Goal Class
'''
from robogauge.tasks.simulator.sim_data import SimData
from collections import defaultdict
from robogauge.utils.measure import Average
from robogauge.tasks.gauge.goal_data import GoalData
from robogauge.tasks.simulator.sim_data import SimData
class BaseGoal:
count = 0
total = 0
name = 'base_goal'
def __init__(self):
self.count = 0
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)
def is_done(self) -> bool:
raise NotImplementedError
@@ -23,3 +33,29 @@ class BaseGoal:
def get_goal(self, sim_data: SimData) -> GoalData:
raise NotImplementedError
def __repr__(self):
if self.sub_name is None:
return f"{self.name}"
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)
for metric_name, value in metrics.items():
self._goal_mean_metrics[metric_name].update(value)
self._sub_goal_mean_metrics[metric_name].update(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()}

17
robogauge/tasks/gauge/goals/velocity_goals.py
View File

@@ -17,8 +17,11 @@ from robogauge.utils.helpers import class_to_dict
from robogauge.utils.logger import logger
class MaxVelocityGoal(BaseGoal):
name = "max_velocity"
""" Goal class for maximizing velocity commands. """
def __init__(self, max_velocity: RobotConfig.commands, cmd_duration: float = 5, **kwargs):
super().__init__()
kwargs.pop('enabled', None)
if kwargs:
logger.warning(f"Unused kwargs in MaxVelocityGoal: {kwargs}")
@@ -28,14 +31,18 @@ class MaxVelocityGoal(BaseGoal):
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:
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:
@@ -48,10 +55,8 @@ class MaxVelocityGoal(BaseGoal):
if self.count >= self.total:
return None
self.current_goal = self.goals[self.count]
self.sub_name = str(self.current_goal)
return GoalData(
goal_type='velocity',
velocity_goal=self.current_goal
)
def __repr__(self):
return f"{self.current_goal}"

43
robogauge/tasks/gauge/metrics/__init__.py
View File

@@ -1,3 +1,5 @@
import numpy as np
from robogauge.tasks.robots import RobotConfig
from robogauge.tasks.simulator.sim_data import SimData
@@ -18,26 +20,51 @@ 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. """
mean_value = 0.0
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 + soft_dof_limit_ratio * dof_range
soft_upper_limit = upper_limit - soft_dof_limit_ratio * dof_range
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/{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
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