From 7d5725cc3e799692030e0acc9371fcb33ddc3bfc Mon Sep 17 00:00:00 2001 From: Johnny Fernandes Date: Sat, 25 Apr 2026 00:18:01 +0100 Subject: [PATCH] Sheep training flock _ improver --- training/herding_env.py | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/training/herding_env.py b/training/herding_env.py index 9beb0c8..7c408a0 100644 --- a/training/herding_env.py +++ b/training/herding_env.py @@ -335,7 +335,8 @@ class HerdingEnv(gym.Env): def _step_sheep(self, i: int) -> np.ndarray: """Apply one timestep of boid dynamics to sheep i (mirrors sheep.py).""" - pos = self.sheep_pos[i].copy() + old_pos = self.sheep_pos[i].copy() # saved for pen wall collision check + pos = old_pos.copy() fx, fy = 0.0, 0.0 fleeing = False @@ -401,4 +402,21 @@ class HerdingEnv(gym.Env): pos = np.clip(pos + (force / mag) * speed * self.DT, -self.FIELD, self.FIELD) + # Pen solid wall collision — mirrors Webots geometry. + # The pen has THREE solid walls: west (x=PEN_X[0]), east (x=PEN_X[1]), + # south (y=PEN_Y[0]). The NORTH face (y=PEN_Y[1]=-8) is the open entrance. + # Sheep may only enter through the north face; crossing a solid wall is blocked. + px0, px1 = self.PEN_X[0], self.PEN_X[1] + py0, py1 = self.PEN_Y[0], self.PEN_Y[1] + entered_from_north = ( + old_pos[1] >= py1 and pos[1] < py1 and px0 < pos[0] < px1 + ) + if not entered_from_north: + # Block crossing through west wall from outside + if old_pos[0] < px0 <= pos[0] and py0 < pos[1] < py1: + pos = np.array([px0 - 1e-3, pos[1]], dtype=np.float32) + # Block crossing through east wall from outside + if old_pos[0] > px1 >= pos[0] and py0 < pos[1] < py1: + pos = np.array([px1 + 1e-3, pos[1]], dtype=np.float32) + return pos.astype(np.float32)