From a72409a0d33d7d1ba00bed0755149f23dff815ed Mon Sep 17 00:00:00 2001 From: valentinbruehl Date: Thu, 30 Oct 2025 10:16:26 +0000 Subject: [PATCH 1/3] update max acceleration computation --- utama_core/config/formulas.py | 5 +++++ utama_core/config/settings.py | 7 +++++++ utama_core/motion_planning/src/dwa/config.py | 4 ++-- 3 files changed, 14 insertions(+), 2 deletions(-) create mode 100644 utama_core/config/formulas.py diff --git a/utama_core/config/formulas.py b/utama_core/config/formulas.py new file mode 100644 index 00000000..a34fce54 --- /dev/null +++ b/utama_core/config/formulas.py @@ -0,0 +1,5 @@ +from settings import GRAVITY, HEIGHT_COM, ROBOT_RADIUS + + +def max_acceleration() -> float: + return GRAVITY * ROBOT_RADIUS / HEIGHT_COM diff --git a/utama_core/config/settings.py b/utama_core/config/settings.py index 445ce295..3a7ff680 100644 --- a/utama_core/config/settings.py +++ b/utama_core/config/settings.py @@ -1,5 +1,6 @@ from pathlib import Path +from formulas import max_acceleration from numpy import pi MAX_ROBOTS = 6 @@ -21,6 +22,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 +77,7 @@ REPLAY_BASE_PATH = Path.cwd() / "replays" RENDER_BASE_PATH = Path.cwd() / "renders" + +GRAVITY = 9.81 + +MAX_ACCELERATION = max_acceleration() diff --git a/utama_core/motion_planning/src/dwa/config.py b/utama_core/motion_planning/src/dwa/config.py index 5b07ed4b..a740b604 100644 --- a/utama_core/motion_planning/src/dwa/config.py +++ b/utama_core/motion_planning/src/dwa/config.py @@ -1,6 +1,6 @@ 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) @@ -8,7 +8,7 @@ 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 From 58121714d95ed6cc9440c77de61fd0cb270be0e4 Mon Sep 17 00:00:00 2001 From: valentinbruehl Date: Thu, 30 Oct 2025 10:36:33 +0000 Subject: [PATCH 2/3] update max_acceleration computation --- utama_core/config/formulas.py | 8 +++----- utama_core/config/settings.py | 5 +++-- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/utama_core/config/formulas.py b/utama_core/config/formulas.py index a34fce54..3e178b3b 100644 --- a/utama_core/config/formulas.py +++ b/utama_core/config/formulas.py @@ -1,5 +1,3 @@ -from settings import GRAVITY, HEIGHT_COM, ROBOT_RADIUS - - -def max_acceleration() -> float: - return GRAVITY * ROBOT_RADIUS / HEIGHT_COM +# utama_core/config/formulas.py +def max_acceleration(gravity, robot_radius, height_com) -> float: + return gravity * robot_radius / height_com diff --git a/utama_core/config/settings.py b/utama_core/config/settings.py index 3a7ff680..02d5cb58 100644 --- a/utama_core/config/settings.py +++ b/utama_core/config/settings.py @@ -1,8 +1,9 @@ from pathlib import Path -from formulas import max_acceleration from numpy import pi +from .formulas import max_acceleration + MAX_ROBOTS = 6 # interval between frames @@ -80,4 +81,4 @@ GRAVITY = 9.81 -MAX_ACCELERATION = max_acceleration() +MAX_ACCELERATION = max_acceleration(gravity=GRAVITY, robot_radius=ROBOT_RADIUS, height_com=HEIGHT_COM) From 522728d8274ca0ebafa408183563a10d8461c2f0 Mon Sep 17 00:00:00 2001 From: valentinbruehl Date: Thu, 30 Oct 2025 11:05:57 +0000 Subject: [PATCH 3/3] Use default max_acceleration for PID --- utama_core/motion_planning/src/pid/pid.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/utama_core/motion_planning/src/pid/pid.py b/utama_core/motion_planning/src/pid/pid.py index a51fc578..0dd1165e 100644 --- a/utama_core/motion_planning/src/pid/pid.py +++ b/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