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

v0.1.6.1

Browse Source
This commit is contained in:
wty-yy
2025-12-06 04:21:52 +08:00
parent 9f7db42f9f
commit d2ceb72eec
12 changed files with 198 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -13,7 +13,7 @@ from pathlib import Path
from functools import partial
from robogauge.utils.logger import logger
from robogauge.utils.helpers import class_to_dict
from robogauge.utils.helpers import class_to_dict, snake_to_pascal
from robogauge.tasks.robots import RobotConfig
from robogauge.tasks.gauge.base_gauge_config import BaseGaugeConfig
@@ -52,10 +52,11 @@ class BaseGauge:
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]))
metric_class_name = f"{snake_to_pascal(name)}Metric"
metric_class = eval(metric_class_name)
self.metrics.append(metric_class(robot_cfg=robot_cfg, **self.metrics_cfg[name]))
log_str += f" - Metric: {name}\n"
self.info['metric'].append(name)
self.info['metric'].append(metric_class_name)
logger.info(log_str.strip())
if len(self.goals) == 0:
@@ -112,8 +113,8 @@ class BaseGauge:
if sim_data.n_step % int(self.cfg.metrics.metric_dt / sim_data.sim_dt) != 0:
return
metrics_results = {}
for metric_name, metric_func in zip(self.info['metric'], self.metrics):
val = metric_func(sim_data)
for metric_name, metric_obj in zip(self.info['metric'], self.metrics):
val = metric_obj(sim_data)
if metric_name not in ['visualization']:
metrics_results[metric_name] = val
self.goals[self.goal_idx].update_metrics(metrics_results)

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

@@ -36,3 +36,9 @@ class BaseGaugeConfig(Config):
enabled = True
dof_force = True
dof_pos = True
class lin_vel_err:
enabled = True
class ang_vel_err:
enabled = True

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

@@ -24,7 +24,8 @@ class FlatGaugeConfig(BaseGaugeConfig):
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:
class metrics(BaseGaugeConfig.metrics):
metric_dt = 0.1 # [s] frequency to compute metrics
class dof_limits:
enabled = True
@@ -35,3 +36,9 @@ class FlatGaugeConfig(BaseGaugeConfig):
enabled = True
dof_force = True
dof_pos = True
class lin_vel_err:
enabled = True
class ang_vel_err:
enabled = True

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

@@ -1,70 +1,4 @@
import numpy as np
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,
dof_names: list = None,
**kwargs
) -> float:
""" Metric to log DOF limit violations. """
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 + (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/{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
from .base_metric import BaseMetric
from .dof_metrics import DofLimitsMetric
from .visualization import VisualizationMetric
from .vel_metrics import LinVelErrMetric, AngVelErrMetric

15
robogauge/tasks/gauge/metrics/base_metric.py Normal file
View File

@@ -0,0 +1,15 @@
from robogauge.utils.logger import logger
from robogauge.tasks.robots import RobotConfig
from robogauge.tasks.simulator.sim_data import SimData
class BaseMetric:
""" Base class for all metric functions. """
name = 'base_metric'
def __init__(self, robot_cfg: RobotConfig, **kwargs):
self.robot_cfg = robot_cfg
def __call__(self, sim_data: SimData) -> float:
value = 0.0
logger.log(value, self.name, step=sim_data.n_step)
return value

50
robogauge/tasks/gauge/metrics/dof_metrics.py Normal file
View File

@@ -0,0 +1,50 @@
import numpy as np
from robogauge.tasks.robots import RobotConfig
from robogauge.tasks.simulator.sim_data import SimData
from robogauge.tasks.gauge.metrics.base_metric import BaseMetric
from robogauge.utils.logger import logger
class DofLimitsMetric(BaseMetric):
""" Metric to log DOF limit violations. """
name = 'dof_limits_metric'
def __init__(self,
robot_cfg: RobotConfig,
soft_dof_limit_ratio: float = 0.9,
dof_names: list = None,
**kwargs
):
super().__init__(robot_cfg)
self.soft_dof_limit_ratio = soft_dof_limit_ratio
self.calc_dof_names = dof_names
def __call__(self, sim_data: SimData) -> float:
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 + (1 - self.soft_dof_limit_ratio) * dof_range
soft_upper_limit = upper_limit - (1 - self.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/{dof_name}', step=sim_data.n_step)
if self.calc_dof_names is not None:
for use_name in self.calc_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

64
robogauge/tasks/gauge/metrics/vel_metrics.py Normal file
View File

@@ -0,0 +1,64 @@
import numpy as np
from robogauge.tasks.robots import RobotConfig
from robogauge.tasks.simulator.sim_data import SimData
from robogauge.tasks.gauge.metrics.base_metric import BaseMetric
from robogauge.tasks.gauge.goals.base_goal import GoalData
from robogauge.utils.logger import logger
class LinVelErrMetric(BaseMetric):
""" Metric to log linear velocity error. """
name = 'lin_vel_err_metric'
def __init__(self, robot_cfg: RobotConfig, **kwargs):
super().__init__(robot_cfg)
max_ranges = []
for name in ['lin_vel_x', 'lin_vel_y', 'lin_vel_z']:
max_ranges.append(
max(
abs(getattr(robot_cfg.commands, name)[0]),
abs(getattr(robot_cfg.commands, name)[1])
)
)
self.norm_vel = np.linalg.norm(max_ranges)
def __call__(self, sim_data: SimData, goal_data: GoalData) -> float:
if goal_data.goal_type != 'velocity':
logger.warning("LinVelErrMetric can only be used with VelocityGoal.")
return 0.0
lin_vel = sim_data.proprio.base.lin_vel
target_lin_vel = [goal_data.velocity_goal.lin_vel_x, goal_data.velocity_goal.lin_vel_y, goal_data.velocity_goal.lin_vel_z]
vel_err = np.linalg.norm(lin_vel - target_lin_vel) / self.norm_vel
logger.log(vel_err, f'vel_metrics/lin_vel_err', step=sim_data.n_step)
return 1 - vel_err
class AngVelErrMetric(BaseMetric):
""" Metric to log angular velocity error. """
name = 'ang_vel_err_metric'
def __init__(self, robot_cfg: RobotConfig, **kwargs):
super().__init__(robot_cfg)
max_ranges = []
for name in ['ang_vel_roll', 'ang_vel_pitch', 'ang_vel_yaw']:
cmd_range = getattr(robot_cfg.commands, name)
if cmd_range is None:
cmd_range = [0, 0]
max_ranges.append(
max(
abs(cmd_range[0]),
abs(cmd_range[1])
)
)
self.norm_vel = np.linalg.norm(max_ranges)
def __call__(self, sim_data: SimData, goal_data: GoalData) -> float:
if goal_data.goal_type != 'velocity':
logger.warning("AngVelErrMetric can only be used with VelocityGoal.")
return 0.0
ang_vel = sim_data.proprio.base.ang_vel
target_ang_vel = [goal_data.velocity_goal.ang_vel_roll, goal_data.velocity_goal.ang_vel_pitch, goal_data.velocity_goal.ang_vel_yaw]
vel_err = np.linalg.norm(ang_vel - target_ang_vel) / self.norm_vel
logger.log(vel_err, f'vel_metrics/ang_vel_err', step=sim_data.n_step)
return 1 - vel_err

30
robogauge/tasks/gauge/metrics/visualization.py Normal file
View File

@@ -0,0 +1,30 @@
from robogauge.tasks.robots import RobotConfig
from robogauge.tasks.simulator.sim_data import SimData
from robogauge.tasks.gauge.metrics.base_metric import BaseMetric
from robogauge.utils.logger import logger
class VisualizationMetric(BaseMetric):
""" Metric to visualize various robot states in the simulator. """
name = 'visualization_metric'
def __init__(self,
robot_cfg: RobotConfig,
dof_force: bool = False,
dof_pos: bool = False,
**kwargs
):
super().__init__(robot_cfg)
self.dof_force = dof_force
self.dof_pos = dof_pos
def __call__(self, sim_data: SimData) -> float:
for i in range(len(sim_data.proprio.joint.force)):
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_pos:
pos = sim_data.proprio.joint.pos[i]
logger.log(pos, f'dof_pos/{name}', step=sim_data.n_step)
return 0.0