diff --git a/utama_core/config/formulas.py b/utama_core/config/formulas.py new file mode 100644 index 00000000..3e178b3b --- /dev/null +++ b/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 diff --git a/utama_core/config/settings.py b/utama_core/config/settings.py index 445ce295..02d5cb58 100644 --- a/utama_core/config/settings.py +++ b/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) 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 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