58 lines
1.9 KiB
Python
58 lines
1.9 KiB
Python
# -*- coding: utf-8 -*-
|
|
'''
|
|
@File : velocity_goals.py
|
|
@Time : 2025/11/30 21:44:13
|
|
@Author : wty-yy
|
|
@Version : 1.0
|
|
@Blog : https://wty-yy.github.io/
|
|
@Desc : Velocity Goals Implementation
|
|
'''
|
|
from typing import Optional
|
|
|
|
from robogauge.tasks.gauge.goals import BaseGoal
|
|
from robogauge.tasks.robots import RobotConfig
|
|
from robogauge.tasks.simulator.sim_data import SimData
|
|
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):
|
|
""" Goal class for maximizing velocity commands. """
|
|
def __init__(self, max_velocity: RobotConfig.commands, cmd_duration: float = 5, **kwargs):
|
|
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.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:
|
|
if value != 0:
|
|
self.goals.append(VelocityGoal(**{key: value}))
|
|
|
|
self.count = 0
|
|
self.total = len(self.goals)
|
|
|
|
def is_reset(self, sim_data: SimData) -> bool:
|
|
if sim_data.sim_time - self.last_reset_time >= self.cmd_duration:
|
|
self.last_reset_time = sim_data.sim_time
|
|
return True
|
|
return False
|
|
|
|
def get_goal(self, sim_data: SimData) -> Optional[GoalData]:
|
|
self.count = int(sim_data.sim_time / self.cmd_duration)
|
|
if self.count >= self.total:
|
|
return None
|
|
self.current_goal = self.goals[self.count]
|
|
return GoalData(
|
|
goal_type='velocity',
|
|
velocity_goal=self.current_goal
|
|
)
|
|
|
|
def __repr__(self):
|
|
return f"{self.current_goal}"
|