feat(simulation): add Worker AI polygon runtime and terrain navigation
This commit is contained in:
@@ -0,0 +1,36 @@
|
||||
"""Physics-clock command envelope; perception remains asynchronous.
|
||||
|
||||
Limit wheel-surface acceleration without delaying a safety reduction. Scaling
|
||||
both sides together preserves the planner's requested curvature. Stops bypass
|
||||
the ramp, as required by the already qualified collision/braking envelope.
|
||||
This is a simulation actuator, not a VESC controller or a motor calibration.
|
||||
"""
|
||||
|
||||
CONTROL_PROFILE = {
|
||||
"clock": "physics-pre-step",
|
||||
"rate_hz": 60,
|
||||
"wheel_surface_acceleration_mps2": 0.2,
|
||||
"safety_reductions": "immediate",
|
||||
"authority": "simulation-only",
|
||||
}
|
||||
|
||||
|
||||
class DriveEnvelope:
|
||||
def __init__(self, track_width=0.9, acceleration=0.2):
|
||||
self.half_track = track_width / 2
|
||||
self.acceleration = acceleration
|
||||
self.wheels = (0.0, 0.0)
|
||||
|
||||
def step(self, velocity, yaw_rate, dt, *, stop=False):
|
||||
requested = (velocity - yaw_rate * self.half_track, velocity + yaw_rate * self.half_track)
|
||||
if stop or max(map(abs, requested)) < 1e-8:
|
||||
self.wheels = (0.0, 0.0)
|
||||
return 0.0, 0.0
|
||||
scale = 1.0
|
||||
for old, new in zip(self.wheels, requested, strict=True):
|
||||
# Reversal starts from zero; never retain motion in the old direction.
|
||||
prior = abs(old) if old * new >= 0 else 0.0
|
||||
if abs(new) > prior:
|
||||
scale = min(scale, (prior + self.acceleration * max(0.0, dt)) / abs(new))
|
||||
self.wheels = tuple(value * scale for value in requested)
|
||||
return velocity * scale, yaw_rate * scale
|
||||
Reference in New Issue
Block a user