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

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