Max acceleration branch

utama_core/config/formulas.py

@@ -0,0 +1,3 @@
+# utama_core/config/formulas.py
+def max_acceleration(gravity, robot_radius, height_com) -> float:
+    return gravity * robot_radius / height_com

utama_core/config/settings.py

@@ -2,6 +2,8 @@
 
 from numpy import pi
 
+from .formulas import max_acceleration
+
 MAX_ROBOTS = 6
 
 # interval between frames
@@ -21,6 +23,8 @@
 
 ROBOT_RADIUS = 0.09  # TODO: probably not the best place to put this
 
+HEIGHT_COM = 0.07  # height of the robot's center of mass
+
 BLACKBOARD_NAMESPACE_MAP = {True: "Opp", False: "My"}
 
 # sim kick speed
@@ -74,3 +78,7 @@
 
 REPLAY_BASE_PATH = Path.cwd() / "replays"
 RENDER_BASE_PATH = Path.cwd() / "renders"
+
+GRAVITY = 9.81
+
+MAX_ACCELERATION = max_acceleration(gravity=GRAVITY, robot_radius=ROBOT_RADIUS, height_com=HEIGHT_COM)

utama_core/motion_planning/src/dwa/config.py

@@ -1,14 +1,14 @@
 from dataclasses import dataclass
 
-from utama_core.config.settings import MAX_VEL, ROBOT_RADIUS
+from utama_core.config.settings import MAX_ACCELERATION, MAX_VEL, ROBOT_RADIUS
 
 
 @dataclass(slots=True)
 class DynamicWindowConfig:
     """Configuration shared by the Dynamic Window planner and controller."""
 
     simulate_frames: float = 3.0
-    max_acceleration: float = 50
+    max_acceleration: float = MAX_ACCELERATION
     max_safety_radius: float = ROBOT_RADIUS * 2.5
     safety_penalty_distance_sq: float = 0.3
     max_speed_for_full_bubble: float = 1.0

utama_core/motion_planning/src/pid/pid.py

@@ -3,6 +3,7 @@
 from typing import Optional, Tuple
 
 from utama_core.config.settings import (
+    MAX_ACCELERATION,
     MAX_ANGULAR_VEL,
     MAX_VEL,
     REAL_MAX_ANGULAR_VEL,
@@ -362,7 +363,7 @@ class PIDAccelerationLimiterWrapper:
     Maintains separate state for each robot to prevent interference.
     """
 
-    def __init__(self, internal_pid: AbstractPID, max_acceleration: float, dt: float = TIMESTEP):
+    def __init__(self, internal_pid: AbstractPID, max_acceleration: float = MAX_ACCELERATION, dt: float = TIMESTEP):
         self._internal_pid = internal_pid
         self._last_results = {}  # Key: robot_id
         self._max_acceleration = max_acceleration