v1.1.2 prev1; add joystick goal

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)

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:

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

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

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,
+            )
+        )