From d1ec24f6ffa5f08ef4c07ff72c1c62c070052231 Mon Sep 17 00:00:00 2001 From: wty-yy <993660140@qq.com> Date: Wed, 4 Feb 2026 11:29:23 +0800 Subject: [PATCH] v1.1.2 prev1; add joystick goal --- UPDATE.md | 3 + robogauge/tasks/gauge/base_gauge.py | 3 + robogauge/tasks/gauge/base_gauge_config.py | 5 ++ robogauge/tasks/gauge/goals/__init__.py | 1 + robogauge/tasks/gauge/goals/base_goal.py | 4 ++ robogauge/tasks/gauge/goals/joystick_goal.py | 72 ++++++++++++++++++++ setup.py | 3 +- 7 files changed, 90 insertions(+), 1 deletion(-) create mode 100644 robogauge/tasks/gauge/goals/joystick_goal.py diff --git a/UPDATE.md b/UPDATE.md index 97956bf..8eb5618 100644 --- a/UPDATE.md +++ b/UPDATE.md @@ -1,4 +1,7 @@ # UPDATE +## 20260204 +### v1.1.2 +1. 加入手柄JoystickGoal功能 ## 20260126 ### v1.1.1-rc2 1. 加入可视化当前速度（蓝），目标速度（绿）箭头 diff --git a/robogauge/tasks/gauge/base_gauge.py b/robogauge/tasks/gauge/base_gauge.py index 7173f1e..a9ccf3c 100644 --- a/robogauge/tasks/gauge/base_gauge.py +++ b/robogauge/tasks/gauge/base_gauge.py @@ -55,6 +55,9 @@ class BaseGauge: elif name == 'target_pos_velocity': self.goals.append(TargetPosVelocityGoal(robot_cfg.control.control_dt, cfg.backward, **kwargs)) log_str += f" - Target Position Velocity Goal: {kwargs}\n" + elif name == 'joystick': + self.goals.append(JoystickGoal(robot_cfg.commands, **kwargs)) + log_str += f" - Joystick Goal: {kwargs}\n" else: raise NotImplementedError(f"Goal '{name}' is not implemented in BaseGauge.") self.info['goal'].append(name) diff --git a/robogauge/tasks/gauge/base_gauge_config.py b/robogauge/tasks/gauge/base_gauge_config.py index 1af60d1..6738437 100644 --- a/robogauge/tasks/gauge/base_gauge_config.py +++ b/robogauge/tasks/gauge/base_gauge_config.py @@ -48,6 +48,11 @@ class BaseGaugeConfig(Config): max_cmd_duration = 10.0 # [s] maximum duration to reach the target position reach_threshold = 0.1 + class joystick: # goal controlled by joystick + enabled = False + goal_type = 'velocity' # 'velocity' + dead_zone = 0.1 # joystick dead zone + class metrics: metric_dt = 0.1 # [s] frequency to compute metrics class dof_limits: diff --git a/robogauge/tasks/gauge/goals/__init__.py b/robogauge/tasks/gauge/goals/__init__.py index 32b6f0b..df3b9df 100644 --- a/robogauge/tasks/gauge/goals/__init__.py +++ b/robogauge/tasks/gauge/goals/__init__.py @@ -1,2 +1,3 @@ from robogauge.tasks.gauge.goals.base_goal import BaseGoal +from robogauge.tasks.gauge.goals.joystick_goal import JoystickGoal from robogauge.tasks.gauge.goals.velocity_goals import MaxVelocityGoal, DiagonalVelocityGoal, TargetPosVelocityGoal diff --git a/robogauge/tasks/gauge/goals/base_goal.py b/robogauge/tasks/gauge/goals/base_goal.py index 4dda693..63405a1 100644 --- a/robogauge/tasks/gauge/goals/base_goal.py +++ b/robogauge/tasks/gauge/goals/base_goal.py @@ -26,6 +26,10 @@ class BaseGoal: self.goal_quality_scores = [] def pre_get_goal(self) -> bool: + """ Run before getting the goal + Returns: + bool: whether the goal sequence is done + """ raise NotImplementedError def reset_goal(self): diff --git a/robogauge/tasks/gauge/goals/joystick_goal.py b/robogauge/tasks/gauge/goals/joystick_goal.py new file mode 100644 index 0000000..b584055 --- /dev/null +++ b/robogauge/tasks/gauge/goals/joystick_goal.py @@ -0,0 +1,72 @@ +# -*- coding: utf-8 -*- +''' +@File : joystick_goal.py +@Time : 2026/02/04 10:47:47 +@Author : wty-yy +@Version : 1.0 +@Blog : https://wty-yy.github.io/ +@Desc : Joystick Goals Implementation +''' +import pygame +from typing import Literal +from robogauge.utils.helpers import class_to_dict +from robogauge.tasks.robots import RobotConfig +from robogauge.tasks.gauge.goals import BaseGoal +from robogauge.tasks.gauge.goal_data import GoalData, VelocityGoal +from robogauge.tasks.simulator.sim_data import SimData + +class JoystickGoal(BaseGoal): + name = "joystick_goal" + + def __init__(self, + max_velocity: RobotConfig.commands, + goal_type: Literal['velocity', 'position'], + dead_zone=0.1, + **kwargs + ): + super().__init__() + self.goal_type = goal_type + self.dead_zone = dead_zone + self.max_velocity = class_to_dict(max_velocity) + if self.goal_type != 'velocity': + raise NotImplementedError("Only 'velocity' goal type is implemented for JoystickGoal.") + + pygame.init() + while not pygame.joystick.get_count(): + print("Waiting for joystick connection...") + pygame.time.wait(1000) + self.joystick = pygame.joystick.Joystick(0) + self.joystick.init() + print(f"Joystick '{self.joystick.get_name()}' initialized.") + + def is_reset(self, sim_data: SimData) -> bool: + return False # never reset + + def pre_get_goal(self, sim_data: SimData) -> bool: + return False # loop forever + + def joystick2cmd(self, joystick_value, key_name): + assert key_name in self.max_velocity, f"Key '{key_name}' not in max_velocity config." + mn, mx = self.max_velocity[key_name] + return (joystick_value + 1) / 2 * (mx - mn) + mn + + def get_goal(self, sim_data: SimData) -> GoalData: + pygame.event.pump() + lx = -self.joystick.get_axis(0) # Left stick X-axis + ly = -self.joystick.get_axis(1) # Left stick Y-axis + rx = -self.joystick.get_axis(3) # Right stick X-axis + if abs(lx) < self.dead_zone: lx = 0 + if abs(ly) < self.dead_zone: ly = 0 + if abs(rx) < self.dead_zone: rx = 0 + cmd_x = self.joystick2cmd(ly, 'lin_vel_x') + cmd_y = self.joystick2cmd(lx, 'lin_vel_y') + cmd_yaw = self.joystick2cmd(rx, 'ang_vel_yaw') + print(f"RAW CMD: {lx:.2f}, {ly:.2f}, {rx:.2f} => CMD: {cmd_x:.2f}, {cmd_y:.2f}, {cmd_yaw:.2f}", end='\r') + return GoalData( + goal_type='velocity', + velocity_goal=VelocityGoal( + lin_vel_x=cmd_x, + lin_vel_y=cmd_y, + ang_vel_yaw=cmd_yaw, + ) + ) diff --git a/setup.py b/setup.py index ab370d6..8ff0ea2 100644 --- a/setup.py +++ b/setup.py @@ -2,7 +2,7 @@ from setuptools import setup, find_packages setup( name="robogauge", # 包名 - version="1.1.1", # 版本号 + version="1.1.2", # 版本号 author="Wu Tianyang", # 你的名字 author_email="993660140@qq.com", description="A generic robot RL model evaluation library based on MuJoCo", @@ -22,6 +22,7 @@ setup( "PyYAML", "fastapi", "uvicorn", + "pygame", ], python_requires=">=3.8",