jalf/TIR_PROJ
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Checkpoint 8

Browse Source
This commit is contained in:
Johnny Fernandes
2026-05-12 22:41:03 +01:00
parent a01a5c9cef
commit 5c2ee4bba5
31 changed files with 3189 additions and 380 deletions
Download Patch File Download Diff File Expand all files Collapse all files
herding/world/diffdrive.py
+130 -1
View File
@@ -1,4 +1,5 @@
"""Differential-drive kinematics, shared by the env and Webots controllers.
"""Differential-drive and mecanum kinematics, shared by the env and Webots
controllers.
First-order rigid-body model — no slip, wheel-accel limits, or contact
forces. Webots' ODE physics handles those at inference; the env stays
@@ -59,3 +60,131 @@ def heading_speed_to_wheels(heading, speed_motor, h, max_wheel_omega,
left = max(-max_wheel_omega, min(max_wheel_omega, fwd - turn))
right = max(-max_wheel_omega, min(max_wheel_omega, fwd + turn))
return left, right
# ---------------------------------------------------------------------------
# Mecanum (4-wheel omnidirectional) kinematics
# ---------------------------------------------------------------------------
def mecanum_kinematics_step(x, y, h, w_fl, w_fr, w_rl, w_rr,
wheel_radius, lx, ly, dt):
"""Integrate one step of mecanum forward kinematics.
Parameters
----------
x, y : robot position (m)
h : robot heading (rad), 0 = +x axis
w_fl, w_fr, w_rl, w_rr : wheel angular velocities (rad/s)
wheel_radius : wheel radius (m)
lx : half the front-to-back axle distance (m)
ly : half the left-to-right axle distance (m)
dt : timestep (s)
Returns (new_x, new_y, new_h).
"""
r = wheel_radius
vx_body = (w_fl + w_fr + w_rl + w_rr) * r / 4.0
vy_body = (-w_fl + w_fr + w_rl - w_rr) * r / 4.0
omega = (-w_fl + w_fr - w_rl + w_rr) * r / (4.0 * (lx + ly))
cos_h = math.cos(h)
sin_h = math.sin(h)
vx_world = vx_body * cos_h - vy_body * sin_h
vy_world = vx_body * sin_h + vy_body * cos_h
new_x = x + vx_world * dt
new_y = y + vy_world * dt
new_h = math.atan2(math.sin(h + omega * dt), math.cos(h + omega * dt))
return new_x, new_y, new_h
def mecanum_inverse(vx_body, vy_body, omega, wheel_radius, lx, ly,
max_wheel_omega):
"""Mecanum inverse kinematics: body-frame velocities to 4 wheel speeds.
Parameters
----------
vx_body, vy_body : desired body-frame linear velocities (m/s)
omega : desired yaw rate (rad/s)
wheel_radius : wheel radius (m)
lx : half front-to-back axle distance (m)
ly : half left-to-right axle distance (m)
max_wheel_omega : wheel angular velocity clamp (rad/s)
Returns (w_fl, w_fr, w_rl, w_rr).
"""
r = wheel_radius
k = lx + ly
w_fl = (vx_body - vy_body - k * omega) / r
w_fr = (vx_body + vy_body + k * omega) / r
w_rl = (vx_body + vy_body - k * omega) / r
w_rr = (vx_body - vy_body + k * omega) / r
scale = max(abs(w_fl), abs(w_fr), abs(w_rl), abs(w_rr), 1e-9)
if scale > max_wheel_omega:
ratio = max_wheel_omega / scale
w_fl *= ratio
w_fr *= ratio
w_rl *= ratio
w_rr *= ratio
return w_fl, w_fr, w_rl, w_rr
def velocity_to_mecanum_wheels(vx, vy, omega, h, max_linear, wheel_radius,
lx, ly, max_wheel_omega,
k_turn=4.0, wheel_base=0.28):
"""Convert world-frame (vx, vy, omega) action in [-1, 1]^3 to 4 wheel speeds.
Truly holonomic interpretation: (vx, vy) is the desired *world-frame*
velocity (magnitude up to ``max_linear`` m/s) and ``omega`` is the
desired yaw rate (independent of motion direction). The dog can
crab-walk and rotate at the same time.
This matches the universal teacher's signal: drive toward a standoff
point while facing the sheep / pen separately. With the older
non-holonomic version, ``omega`` from the teacher fought against the
forward-only kinematics and dropped success rates instead of helping.
Parameters
----------
vx, vy : desired world-frame velocity intent in [-1, 1] (clamped on
magnitude to ≤ 1)
omega : desired yaw rate intent in [-1, 1]
h : current heading (rad), 0 = +x
max_linear : max linear speed (m/s)
wheel_radius : wheel radius (m)
lx, ly : half axle distances (m)
max_wheel_omega : wheel angular velocity clamp (rad/s)
k_turn : unused (kept for signature compatibility)
wheel_base : unused (kept for signature compatibility)
Returns (w_fl, w_fr, w_rl, w_rr).
"""
# Clamp the action magnitude in the (vx, vy) unit disk.
norm = math.hypot(vx, vy)
if norm > 1.0:
vx /= norm
vy /= norm
# World-frame velocity → body-frame velocity (rotate by -h).
vx_world = vx * max_linear
vy_world = vy * max_linear
cos_h = math.cos(h)
sin_h = math.sin(h)
vx_body = cos_h * vx_world + sin_h * vy_world
vy_body = -sin_h * vx_world + cos_h * vy_world
# Yaw rate: omega ∈ [-1, 1] maps to ± max_linear / (lx + ly) — same
# peak yaw as the old "omega_extra" channel, but used directly
# rather than added to a heading-tracker.
yaw_max = max_linear / max(lx + ly, 1e-6)
omega_rad = omega * yaw_max
if abs(vx_body) < 1e-3 and abs(vy_body) < 1e-3 and abs(omega_rad) < 1e-3:
return 0.0, 0.0, 0.0, 0.0
return mecanum_inverse(
vx_body, vy_body, omega_rad,
wheel_radius, lx, ly, max_wheel_omega,
)
herding/world/flocking_sim.py
+40 -23
View File
@@ -27,9 +27,10 @@ import math
import random
from herding.world.geometry import (
FIELD_SHAPE, FIELD_ROUND_R,
FIELD_X, FIELD_Y,
PEN_X, PEN_Y,
GATE_X,
GATE_X, GATE_Y,
)
# Speeds are in wheel rad/s (motor units); m/s = speed * SHEEP_WHEEL_RADIUS.
@@ -131,33 +132,49 @@ def compute_heading_speed(x, y, penned, dog_xy, peers, wander_angle, rng=None):
fx -= (ddx / d) * push * 2.5
fy -= (ddy / d) * push * 2.5
# Wall soft repulsion (south wall absent inside the gate column).
if x < FIELD_X[0] + WALL_MARGIN:
fx += ((FIELD_X[0] + WALL_MARGIN - x) / WALL_MARGIN) * 6.0
if x > FIELD_X[1] - WALL_MARGIN:
fx -= ((x - (FIELD_X[1] - WALL_MARGIN)) / WALL_MARGIN) * 6.0
if y > FIELD_Y[1] - WALL_MARGIN:
fy -= ((y - (FIELD_Y[1] - WALL_MARGIN)) / WALL_MARGIN) * 6.0
if y < FIELD_Y[0] + WALL_MARGIN and not (GATE_X[0] <= x <= GATE_X[1]):
fy += ((FIELD_Y[0] + WALL_MARGIN - y) / WALL_MARGIN) * 6.0
# Wall soft repulsion.
if FIELD_SHAPE == "field_round":
r = math.hypot(x, y)
wall_d = FIELD_ROUND_R - r
in_gate_col = (GATE_X[0] <= x <= GATE_X[1]
and y < GATE_Y + WALL_MARGIN)
if wall_d < WALL_MARGIN and r > 1e-6 and not in_gate_col:
gain = ((WALL_MARGIN - wall_d) / WALL_MARGIN) * 6.0
fx -= (x / r) * gain
fy -= (y / r) * gain
# Hard escape band.
if wall_d < WALL_HARD_MARGIN and not in_gate_col:
hgain = WALL_HARD_GAIN * (1.0 - wall_d / WALL_HARD_MARGIN)
fx -= (x / r) * hgain
fy -= (y / r) * hgain
else:
# Rectangular: south wall absent inside the gate column.
if x < FIELD_X[0] + WALL_MARGIN:
fx += ((FIELD_X[0] + WALL_MARGIN - x) / WALL_MARGIN) * 6.0
if x > FIELD_X[1] - WALL_MARGIN:
fx -= ((x - (FIELD_X[1] - WALL_MARGIN)) / WALL_MARGIN) * 6.0
if y > FIELD_Y[1] - WALL_MARGIN:
fy -= ((y - (FIELD_Y[1] - WALL_MARGIN)) / WALL_MARGIN) * 6.0
if y < FIELD_Y[0] + WALL_MARGIN and not (GATE_X[0] <= x <= GATE_X[1]):
fy += ((FIELD_Y[0] + WALL_MARGIN - y) / WALL_MARGIN) * 6.0
# Hard escape band — overrides everything else near a wall.
m, g = WALL_HARD_MARGIN, WALL_HARD_GAIN
if x - FIELD_X[0] < m:
fx = max(fx, g * (1.0 - (x - FIELD_X[0]) / m))
if FIELD_X[1] - x < m:
fx = min(fx, -g * (1.0 - (FIELD_X[1] - x) / m))
if FIELD_Y[1] - y < m:
fy = min(fy, -g * (1.0 - (FIELD_Y[1] - y) / m))
if (y - FIELD_Y[0] < m) and not (GATE_X[0] <= x <= GATE_X[1]):
fy = max(fy, g * (1.0 - (y - FIELD_Y[0]) / m))
if not fleeing:
if random.random() < 0.02:
wander_angle += random.uniform(-0.6, 0.6)
if rnd.random() < 0.02:
wander_angle += rnd.uniform(-0.6, 0.6)
fx += math.cos(wander_angle) * 0.5
fy += math.sin(wander_angle) * 0.5
# Hard escape band — overrides everything else near a wall.
m, g = WALL_HARD_MARGIN, WALL_HARD_GAIN
if x - FIELD_X[0] < m:
fx = max(fx, g * (1.0 - (x - FIELD_X[0]) / m))
if FIELD_X[1] - x < m:
fx = min(fx, -g * (1.0 - (FIELD_X[1] - x) / m))
if FIELD_Y[1] - y < m:
fy = min(fy, -g * (1.0 - (FIELD_Y[1] - y) / m))
if (not penned) and (y - FIELD_Y[0] < m) and not (GATE_X[0] <= x <= GATE_X[1]):
fy = max(fy, g * (1.0 - (y - FIELD_Y[0]) / m))
heading = math.atan2(fy, fx)
mag = math.hypot(fx, fy)
speed = max(WANDER_SPEED, min(FLEE_SPEED, mag * 3.0))
herding/world/geometry.py
+114 -7
View File
@@ -4,20 +4,35 @@ Coordinates are metres; (0, 0) is the field centre, +x east, +y north.
These constants mirror ``worlds/field.wbt`` and the proto files — if
the world changes, this file is the single point of update.
field +y north
field (rectangular)
+-----------+
| |
| |
| ...... |
+---||||----+ y = -15 (south wall, 3 m gate at x ∈ [10, 13])
+---||||----+ y = -15 (south wall, 3 m gate at x in [10, 13])
||||
|pen| y ∈ [-22, -15]
|pen| y in [-22, -15]
+---+
field_round (circular, R = 15 m)
.---.
/ ... \\
| ..... | gate at south, x in [-1.83, 1.83]
\\ ... /
'-+-' pen y in [-22, -15]
"""
import os
import math
# Field (square, stone-walled)
# ---------------------------------------------------------------------------
# Field shape selection — controlled by HERDING_WORLD env var at runtime.
# Defaults to "field" (rectangular). The launcher writes it into the
# runtime cfg so the controller can pick it up too.
# ---------------------------------------------------------------------------
FIELD_SHAPE = (os.environ.get("HERDING_WORLD", "field")).lower()
# ==================== Rectangular field (field.wbt) ====================
FIELD_X = (-15.0, 15.0)
FIELD_Y = (-15.0, 15.0)
FIELD_INSIDE_MARGIN = 0.5
@@ -32,12 +47,67 @@ PEN_ENTRY = (0.5 * (PEN_X[0] + PEN_X[1]), -15.0)
GATE_X = PEN_X
GATE_Y = -15.0
# ==================== Round field (field_round.wbt) ====================
FIELD_ROUND_R = 15.0
FIELD_ROUND_PEN_X = (-1.5, 1.5)
FIELD_ROUND_PEN_Y = (-22.0, -15.0)
FIELD_ROUND_PEN_CENTER = (
0.5 * (FIELD_ROUND_PEN_X[0] + FIELD_ROUND_PEN_X[1]),
0.5 * (FIELD_ROUND_PEN_Y[0] + FIELD_ROUND_PEN_Y[1]),
)
FIELD_ROUND_PEN_ENTRY = (0.0, -15.0)
FIELD_ROUND_GATE_X = FIELD_ROUND_PEN_X
FIELD_ROUND_GATE_Y = -15.0
# ==================== Active geometry (resolved at import) ===============
# Rectangular defaults are already assigned above. Override for round.
if FIELD_SHAPE == "field_round":
PEN_X = FIELD_ROUND_PEN_X
PEN_Y = FIELD_ROUND_PEN_Y
PEN_CENTER = FIELD_ROUND_PEN_CENTER
PEN_ENTRY = FIELD_ROUND_PEN_ENTRY
GATE_X = FIELD_ROUND_GATE_X
GATE_Y = FIELD_ROUND_GATE_Y
def configure(shape: str) -> None:
"""Switch the active field geometry at runtime.
Call this **before** importing any other ``herding.*`` module that
depends on the constants below (flocking_sim, lidar_sim, obs, etc.).
The import-time env-var path (``HERDING_WORLD``) still works; this
function is for scripts that need to choose the world via a CLI flag.
"""
global FIELD_SHAPE, PEN_X, PEN_Y, PEN_CENTER, PEN_ENTRY, GATE_X, GATE_Y
shape = shape.lower()
FIELD_SHAPE = shape
if shape == "field_round":
PEN_X = FIELD_ROUND_PEN_X
PEN_Y = FIELD_ROUND_PEN_Y
PEN_CENTER = FIELD_ROUND_PEN_CENTER
PEN_ENTRY = FIELD_ROUND_PEN_ENTRY
GATE_X = FIELD_ROUND_GATE_X
GATE_Y = FIELD_ROUND_GATE_Y
else:
PEN_X = (10.0, 13.0)
PEN_Y = (-22.0, -15.0)
PEN_CENTER = (0.5 * (PEN_X[0] + PEN_X[1]), 0.5 * (PEN_Y[0] + PEN_Y[1]))
PEN_ENTRY = (0.5 * (PEN_X[0] + PEN_X[1]), -15.0)
GATE_X = PEN_X
GATE_Y = -15.0
# Dog spec — protos/ShepherdDog.proto
DOG_WHEEL_RADIUS = 0.038 # m
DOG_WHEEL_BASE = 0.28 # m, axle-to-axle
DOG_MAX_WHEEL_OMEGA = 70.0 # rad/s
DOG_MAX_LINEAR = DOG_WHEEL_RADIUS * DOG_MAX_WHEEL_OMEGA # ≈ 2.66 m/s
# Dog mecanum spec — 4-wheel omnidirectional layout
DOG_WHEEL_BASE_X = 0.28 # m, front-to-back axle distance
DOG_WHEEL_BASE_Y = 0.28 # m, left-to-right axle distance
# Sheep spec — protos/Sheep.proto
SHEEP_WHEEL_RADIUS = 0.031 # m
SHEEP_WHEEL_BASE = 0.20 # m
@@ -58,21 +128,58 @@ def in_pen(x: float, y: float) -> bool:
def in_field(x: float, y: float, margin: float = 0.0) -> bool:
if FIELD_SHAPE == "field_round":
r = FIELD_ROUND_R - margin
return x * x + y * y <= r * r
return (FIELD_X[0] + margin <= x <= FIELD_X[1] - margin
and FIELD_Y[0] + margin <= y <= FIELD_Y[1] - margin)
def in_gate_corridor(x: float, y: float, margin: float = 0.0) -> bool:
"""True if (x, y) lies in the column of the gate (between field and pen)."""
return (PEN_X[0] - margin <= x <= PEN_X[1] + margin
return (GATE_X[0] - margin <= x <= GATE_X[1] + margin
and PEN_Y[0] - margin <= y <= GATE_Y + margin)
def is_penned_position(x: float, y: float, latch_margin: float = 0.2) -> bool:
"""True iff (x, y) is in the gate column and south of the gate line."""
return (PEN_X[0] - latch_margin <= x <= PEN_X[1] + latch_margin
return (GATE_X[0] - latch_margin <= x <= GATE_X[1] + latch_margin
and y <= GATE_Y)
def distance_to_pen_entry(x: float, y: float) -> float:
return math.hypot(x - PEN_ENTRY[0], y - PEN_ENTRY[1])
def distance_to_wall(x: float, y: float) -> float:
"""Shortest distance from (x, y) to the nearest field wall.
For a rectangular field this is the minimum Manhattan distance to the
four bounding walls. For a round field it is ``R - sqrt(x²+y²)``.
Returns a negative value if the point is outside the field.
"""
if FIELD_SHAPE == "field_round":
return FIELD_ROUND_R - math.hypot(x, y)
return min(
x - FIELD_X[0], FIELD_X[1] - x,
y - FIELD_Y[0], FIELD_Y[1] - y,
)
def clip_to_field(x: float, y: float, margin: float = 0.2) -> tuple[float, float]:
"""Clip (x, y) inside the field boundary with a small margin.
For round fields the point is projected radially inward if it exceeds
the circular boundary.
"""
if FIELD_SHAPE == "field_round":
r = math.hypot(x, y)
limit = FIELD_ROUND_R - margin
if r > limit and r > 1e-6:
scale = limit / r
return x * scale, y * scale
return x, y
return (
max(FIELD_X[0] + margin, min(FIELD_X[1] - margin, x)),
max(FIELD_Y[0] + margin, min(FIELD_Y[1] - margin, y)),
)