v0.1.9

robogauge/tasks/gauge/base_gauge.py

@@ -43,10 +43,10 @@ class BaseGauge:
         for name, kwargs in self.goals_cfg.items():
             if not kwargs['enabled']: continue
             if name == 'max_velocity':
-                self.goals.append(MaxVelocityGoal(robot_cfg.commands, **kwargs))
+                self.goals.append(MaxVelocityGoal(robot_cfg.control.control_dt, robot_cfg.commands, **kwargs))
                 log_str += f"  - Max Velocity Goal: {kwargs}\n"
             elif name == 'diagonal_velocity':
-                self.goals.append(DiagonalVelocityGoal(robot_cfg.commands, **kwargs))
+                self.goals.append(DiagonalVelocityGoal(robot_cfg.control.control_dt, robot_cfg.commands, **kwargs))
                 log_str += f"  - Diagonal Velocity Goal: {kwargs}\n"
             else:
                 raise NotImplementedError(f"Goal '{name}' is not implemented in BaseGauge.")

robogauge/tasks/gauge/gauge_configs/flat_gauge_config.py

@@ -19,7 +19,9 @@ class FlatGaugeConfig(BaseGaugeConfig):
     class goals:
         class max_velocity:  # goal with maximum velocity
             enabled = True
-            cmd_duration = 5.0  # [s] duration for each velocity command
+            move_duration = 5.0  # [s] duration for each velocity command
+            end_stance = True  # whether to end with zero velocity command
+            standce_duration = 2.0  # [s] duration for the ending stance command
         
         class diagonal_velocity:  # goal with diagonal velocity changes
             enabled = True

robogauge/tasks/gauge/goal_data.py

@@ -1,6 +1,6 @@
 from enum import Enum
 from dataclasses import dataclass
-from typing import List, Optional, Literal
+from typing import List, Optional, Literal, Tuple
 
 @dataclass
 class VelocityGoal:
@@ -31,10 +31,10 @@ class VelocityGoal:
 @dataclass
 class PositionGoal:
     # relative to robot's current position
-    target_pos: List[float]  # x, y, z [m], z is ignored for ground robots
+    target_pos: Tuple[float, float, float] = (0.0, 0.0, 0.0)  # x, y, z [m], z is ignored for ground robots
     # reach target orientation
-    target_quat: List[float]  # x, y, z, w quaternion
-    tolerance: float  # [m] position tolerance to consider goal reached
+    target_quat: Tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0)  # x, y, z, w quaternion
+    tolerance: float = 0.01  # [m] position tolerance to consider goal reached
 
 @dataclass
 class GoalData:

robogauge/tasks/gauge/goals/velocity_goals.py

@@ -20,38 +20,47 @@ from robogauge.utils.logger import logger
 class BaseVelocityGoal(BaseGoal):
     name = "base_velocity_goal"
 
-    def __init__(self, cmd_duration: float = 5, **kwargs):
+    def __init__(self, control_dt: float, cmd_duration: float = 5, **kwargs):
         super().__init__()
+        self.control_dt = control_dt
         self.cmd_duration = cmd_duration
-        self.goal_start_time = None
+        self.goal_runtime = 0.0
+        self.first_goal_after_reset = True
         self.last_reset_time = 0.0
         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
+            self.first_goal_after_reset = True
             return True
         return False
 
-    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]
-        self.sub_name = str(self.current_goal)
-        return GoalData(
-            goal_type='velocity',
-            velocity_goal=self.current_goal
-        )
+    def update_runtime_count(self, sim_data: SimData):
+        if self.first_goal_after_reset:
+            self.last_reset_time = sim_data.sim_time  # update last reset time (stance after reset)
+            self.first_goal_after_reset = False
+        self.goal_runtime += self.control_dt
+        self.count = int(self.goal_runtime / self.cmd_duration)
 
 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)
+    def __init__(self,
+        control_dt: float,
+        max_velocity: RobotConfig.commands,
+        move_duration: float = 5,
+        end_stance: bool = True,
+        stance_duration: float = 2.0,
+        **kwargs
+    ):
+        cmd_duration = move_duration + (stance_duration if end_stance else 0)
+        super().__init__(control_dt=control_dt, cmd_duration=cmd_duration)
+        self.move_duration = move_duration
+        self.end_stance = end_stance
+        self.stance_duration = stance_duration
+
         kwargs.pop('enabled', None)
         if kwargs:
             logger.warning(f"Unused kwargs in MaxVelocityGoal: {kwargs}")
@@ -72,16 +81,35 @@ class MaxVelocityGoal(BaseVelocityGoal):
             return
         self.sub_name = str(self.goals[0])
 
+    def get_goal(self, sim_data: SimData) -> Optional[GoalData]:
+        self.update_runtime_count(sim_data)
+        if self.count >= self.total:
+            return None
+        self.current_goal = self.goals[self.count]
+        if self.end_stance and self.goal_runtime - self.count * self.cmd_duration >= self.move_duration:
+            self.current_goal = VelocityGoal()  # zero velocity
+        self.sub_name = str(self.current_goal)
+        return GoalData(
+            goal_type='velocity',
+            velocity_goal=self.current_goal
+        )
+
 class DiagonalVelocityGoal(BaseVelocityGoal):
     name = "diagonal_velocity"
 
-    def __init__(self, max_velocity: RobotConfig.commands, cmd_duration: float = 6, **kwargs):
+    def __init__(self,
+        control_dt: float,
+        max_velocity: RobotConfig.commands,
+        cmd_duration: float = 6,
+        **kwargs
+    ):
         """ Goal class for diagonal velocity changes.
         Args:
+            control_dt (float): Control timestep.
             max_velocity (RobotConfig.commands): Maximum velocity commands.
             cmd_duration (float, optional): Duration for a pair of diagonal commands.
         """
-        super().__init__(cmd_duration=cmd_duration)
+        super().__init__(control_dt=control_dt, cmd_duration=cmd_duration)
         kwargs.pop('enabled', None)
         if kwargs:
             logger.warning(f"Unused kwargs in DiagonalVelocityGoal: {kwargs}")
@@ -108,13 +136,11 @@ class DiagonalVelocityGoal(BaseVelocityGoal):
         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)
+        self.update_runtime_count(sim_data)
         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:
+        if self.goal_runtime - 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(