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

v0.1.6

Browse Source
This commit is contained in:
wty-yy
2025-12-03 09:43:16 +08:00
parent ad907089a8
commit 9f7db42f9f
12 changed files with 179 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
robogauge/tasks/custom/go2_flat_task.py
View File

@@ -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

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

@@ -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}")

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

@@ -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

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

@@ -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

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

@@ -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:

2
robogauge/tasks/gauge/goals/__init__.py
View File

@@ -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

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

@@ -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

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

@@ -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
)