Extension Penalty [AARD-2011] (#1248)

BrandonPacewic · AlexD717 · web-flow · commit ef8121ac2e87 · 2025-08-06T10:08:42.000-07:00
Co-authored-by: Alexey Dmitriev &lt;157652245+AlexD717@users.noreply.github.com&gt;
diff --git a/fission/src/systems/match_mode/DefaultMatchModeConfigs.ts b/fission/src/systems/match_mode/DefaultMatchModeConfigs.ts
@@ -13,7 +13,9 @@ class DefaultMatchModeConfigs {
             endgameTime: 20,
             ignoreRotation: true,
             maxHeight: Infinity,
-            heightPenalty: 0,
+            heightLimitPenalty: 0,
+            sideMaxExtension: convertFeetToMeters(1.5),
+            sideExtensionPenalty: 0,
         }
     }
 
@@ -27,7 +29,9 @@ class DefaultMatchModeConfigs {
             endgameTime: 20,
             ignoreRotation: true,
             maxHeight: convertFeetToMeters(4),
-            heightPenalty: 2,
+            heightLimitPenalty: 2,
+            sideMaxExtension: convertFeetToMeters(1),
+            sideExtensionPenalty: 2,
         }
     }
 
@@ -41,7 +45,9 @@ class DefaultMatchModeConfigs {
             endgameTime: 30,
             ignoreRotation: true,
             maxHeight: convertFeetToMeters(6.5),
-            heightPenalty: 5,
+            heightLimitPenalty: 5,
+            sideMaxExtension: convertFeetToMeters(4),
+            sideExtensionPenalty: 5,
         }
     }
 
@@ -55,7 +61,9 @@ class DefaultMatchModeConfigs {
             endgameTime: 5,
             ignoreRotation: true,
             maxHeight: Infinity,
-            heightPenalty: 0,
+            heightLimitPenalty: 0,
+            sideMaxExtension: Infinity,
+            sideExtensionPenalty: 0,
         }
     }
 
diff --git a/fission/src/systems/match_mode/MatchMode.ts b/fission/src/systems/match_mode/MatchMode.ts
@@ -9,11 +9,13 @@ import SimulationSystem from "../simulation/SimulationSystem"
 import { SoundPlayer } from "../sound/SoundPlayer"
 import {
     DEFAULT_AUTONOMOUS_TIME,
+    DEFAULT_TELEOP_TIME,
     DEFAULT_ENDGAME_TIME,
-    DEFAULT_HEIGHT_PENALTY,
     DEFAULT_IGNORE_ROTATION,
     DEFAULT_MAX_HEIGHT,
-    DEFAULT_TELEOP_TIME,
+    DEFAULT_HEIGHT_LIMIT_PENALTY,
+    DEFAULT_SIDE_MAX_EXTENSION,
+    DEFAULT_SIDE_EXTENSION_PENALTY,
     MatchModeType,
 } from "./MatchModeTypes"
 import RobotDimensionTracker from "./RobotDimensionTracker"
@@ -42,7 +44,9 @@ class MatchMode {
         endgameTime: DEFAULT_ENDGAME_TIME,
         ignoreRotation: DEFAULT_IGNORE_ROTATION,
         maxHeight: DEFAULT_MAX_HEIGHT,
-        heightPenalty: DEFAULT_HEIGHT_PENALTY,
+        heightLimitPenalty: DEFAULT_HEIGHT_LIMIT_PENALTY,
+        sideExtensionPenalty: DEFAULT_SIDE_EXTENSION_PENALTY,
+        sideMaxExtension: DEFAULT_SIDE_MAX_EXTENSION,
     }
 
     private constructor() {}
@@ -54,7 +58,13 @@ class MatchMode {
 
     setMatchModeConfig(config: MatchModeConfig) {
         this._matchModeConfig = config
-        RobotDimensionTracker.setConfigValues(config.ignoreRotation, config.maxHeight, config.heightPenalty)
+        RobotDimensionTracker.setConfigValues(
+            config.ignoreRotation,
+            config.maxHeight,
+            config.heightLimitPenalty,
+            config.sideMaxExtension,
+            config.sideExtensionPenalty
+        )
     }
 
     startTimer(duration: number, functionCall: () => void, updateTimeLeft: boolean = true) {
@@ -109,6 +119,7 @@ class MatchMode {
     start() {
         this.autonomousModeStart()
         SimulationSystem.resetScores()
+        RobotDimensionTracker.matchStart()
     }
 
     matchEnded() {
diff --git a/fission/src/systems/match_mode/MatchModeTypes.ts b/fission/src/systems/match_mode/MatchModeTypes.ts
@@ -12,4 +12,6 @@ export const DEFAULT_TELEOP_TIME = 135
 export const DEFAULT_ENDGAME_TIME = 20
 export const DEFAULT_IGNORE_ROTATION = true
 export const DEFAULT_MAX_HEIGHT = Infinity
-export const DEFAULT_HEIGHT_PENALTY = 2
+export const DEFAULT_HEIGHT_LIMIT_PENALTY = 2
+export const DEFAULT_SIDE_MAX_EXTENSION = Infinity
+export const DEFAULT_SIDE_EXTENSION_PENALTY = 2
diff --git a/fission/src/systems/match_mode/RobotDimensionTracker.ts b/fission/src/systems/match_mode/RobotDimensionTracker.ts
@@ -1,21 +1,34 @@
 import { MiraType } from "@/mirabuf/MirabufLoader"
 import MirabufSceneObject from "@/mirabuf/MirabufSceneObject"
 import SimulationSystem from "@/systems/simulation/SimulationSystem"
+import World from "@/systems/World"
 import type SceneRenderer from "../scene/SceneRenderer"
 import MatchMode from "./MatchMode"
 
 const BUFFER_HEIGHT = 0.1
+const SIDE_BUFFER = 0.1
 
 class RobotDimensionTracker {
     private static _robotLastFramePenalty: Map<number, boolean> = new Map()
     private static _ignoreRotation: boolean = true
     private static _maxHeight: number = Infinity
-    private static _heightPenalty: number = 0
+    private static _heightLimitPenalty: number = 0
+    private static _sideExtensionPenalty: number = 0
+    private static _robotSize: Map<number, { width: number; depth: number }> = new Map()
+    private static _sideMaxExtension: number = 0
 
-    public static setConfigValues(ignoreRotation: boolean, maxHeight: number, heightPenalty: number) {
+    public static setConfigValues(
+        ignoreRotation: boolean,
+        maxHeight: number,
+        heightLimitPenalty: number,
+        sideMaxExtension: number,
+        sideExtensionPenalty: number
+    ) {
         this._ignoreRotation = ignoreRotation
         this._maxHeight = maxHeight
-        this._heightPenalty = heightPenalty
+        this._heightLimitPenalty = heightLimitPenalty
+        this._sideMaxExtension = sideMaxExtension
+        this._sideExtensionPenalty = sideExtensionPenalty
     }
 
     public static update(sceneRenderer: SceneRenderer): void {
@@ -30,12 +43,38 @@ class RobotDimensionTracker {
 
             if (dimensions.height > this._maxHeight + BUFFER_HEIGHT) {
                 if (!(this._robotLastFramePenalty.get(robot.id) ?? false)) {
-                    SimulationSystem.robotPenalty(robot, this._heightPenalty, "Height Expansion Limit")
+                    SimulationSystem.robotPenalty(robot, this._heightLimitPenalty, "Height Expansion Limit")
                 }
                 this._robotLastFramePenalty.set(robot.id, true)
-            } else {
-                this._robotLastFramePenalty.set(robot.id, false)
+                return
             }
+
+            const startingRobotSize = this._robotSize.get(robot.id) ?? { width: Infinity, depth: Infinity }
+            if (
+                dimensions.width > startingRobotSize.width + this._sideMaxExtension + SIDE_BUFFER ||
+                dimensions.depth > startingRobotSize.depth + this._sideMaxExtension + SIDE_BUFFER
+            ) {
+                if (!(this._robotLastFramePenalty.get(robot.id) ?? false)) {
+                    SimulationSystem.robotPenalty(robot, this._sideExtensionPenalty, "Side Expansion Limit")
+                }
+                this._robotLastFramePenalty.set(robot.id, true)
+                return
+            }
+
+            this._robotLastFramePenalty.set(robot.id, false)
+        })
+    }
+
+    public static matchStart(): void {
+        this._robotSize.clear()
+        this._robotLastFramePenalty.clear()
+
+        const robots = [...World.sceneRenderer.sceneObjects.values()].filter(
+            (obj): obj is MirabufSceneObject => obj instanceof MirabufSceneObject && obj.miraType === MiraType.ROBOT
+        )
+
+        robots.forEach(robot => {
+            this._robotSize.set(robot.id, robot.getDimensions())
         })
     }
 }
diff --git a/fission/src/test/RobotDimensionsTracker.test.ts b/fission/src/test/RobotDimensionsTracker.test.ts
@@ -3,6 +3,8 @@ import { MiraType } from "@/mirabuf/MirabufLoader"
 import MirabufSceneObject from "@/mirabuf/MirabufSceneObject"
 import RobotDimensionTracker from "@/systems/match_mode/RobotDimensionTracker"
 import SimulationSystem from "@/systems/simulation/SimulationSystem"
+import World from "@/systems/World"
+import SceneObject from "@/systems/scene/SceneObject"
 
 interface MockDimensions {
     width: number
@@ -11,20 +13,30 @@ interface MockDimensions {
 }
 
 interface MockRobotObject {
+    _id: number | null
+    id: number
     assemblyName: string
     miraType: MiraType
     getDimensions: () => MockDimensions
     getDimensionsWithoutRotation: () => MockDimensions
+    setup: () => void
+    update: () => void
+    dispose: () => void
 }
 
 interface MockNonRobotObject {
+    _id: number | null
+    id: number
     assemblyName: string
     miraType: MiraType
     getDimensions: () => MockDimensions
+    setup: () => void
+    update: () => void
+    dispose: () => void
 }
 
 interface MockSceneRenderer {
-    sceneObjects: Map<string, MockRobotObject | MockNonRobotObject>
+    sceneObjects: Map<number, MockRobotObject | MockNonRobotObject>
 }
 
 type TrackerUpdateParam = Parameters<typeof RobotDimensionTracker.update>[0]
@@ -48,7 +60,9 @@ vi.mock("@/systems/match_mode/MatchMode", () => ({
     DEFAULT_ENDGAME_TIME: 20,
     DEFAULT_IGNORE_ROTATION: true,
     DEFAULT_MAX_HEIGHT: Infinity,
-    DEFAULT_HEIGHT_PENALTY: 2,
+    DEFAULT_HEIGHT_LIMIT_PENALTY: 2,
+    DEFAULT_SIDE_MAX_EXTENSION: 1.5,
+    DEFAULT_SIDE_EXTENSION_PENALTY: 2,
 }))
 
 vi.mock("@/systems/simulation/SimulationSystem", () => ({
@@ -57,6 +71,14 @@ vi.mock("@/systems/simulation/SimulationSystem", () => ({
     },
 }))
 
+vi.mock("@/systems/World", () => ({
+    default: {
+        sceneRenderer: {
+            sceneObjects: new Map(),
+        },
+    },
+}))
+
 describe("RobotDimensionTracker", () => {
     let mockSceneRenderer: MockSceneRenderer
     let mockRobot1: MockRobotObject
@@ -66,8 +88,12 @@ describe("RobotDimensionTracker", () => {
     beforeEach(() => {
         vi.clearAllMocks()
 
-        const tracker = RobotDimensionTracker as unknown as { _robotLastFramePenalty?: Map<number, boolean> }
+        const tracker = RobotDimensionTracker as unknown as {
+            _robotLastFramePenalty?: Map<number, boolean>
+            _robotSize?: Map<number, { width: number; depth: number }>
+        }
         tracker._robotLastFramePenalty?.clear()
+        tracker._robotSize?.clear()
 
         const robot1Base = Object.create(MirabufSceneObject.prototype)
         const robot2Base = Object.create(MirabufSceneObject.prototype)
@@ -97,34 +123,43 @@ describe("RobotDimensionTracker", () => {
         })
 
         mockNonRobot = {
+            _id: 3,
+            id: 3,
             assemblyName: "Field",
             miraType: MiraType.FIELD,
             getDimensions: vi.fn(() => ({ height: 5.0, width: 10.0, depth: 10.0 })),
+            setup: vi.fn(),
+            update: vi.fn(),
+            dispose: vi.fn(),
         }
 
         mockSceneRenderer = {
             sceneObjects: new Map([
-                ["robot1", mockRobot1],
-                ["robot2", mockRobot2],
-                ["field", mockNonRobot],
+                [1, mockRobot1],
+                [2, mockRobot2],
+                [3, mockNonRobot],
             ]),
         }
+
+        World.sceneRenderer.sceneObjects.set(1, mockRobot1 as unknown as SceneObject)
+        World.sceneRenderer.sceneObjects.set(2, mockRobot2 as unknown as SceneObject)
+        World.sceneRenderer.sceneObjects.set(3, mockNonRobot as unknown as SceneObject)
     })
 
     afterEach(() => {
         vi.restoreAllMocks()
     })
 
     test("config values determine which dimension method is used", () => {
-        RobotDimensionTracker.setConfigValues(false, 2, 15)
+        RobotDimensionTracker.setConfigValues(false, 2, 15, 1.5, 15)
         RobotDimensionTracker.update(mockSceneRenderer as unknown as TrackerUpdateParam)
 
         expect(mockRobot1.getDimensions).toHaveBeenCalled()
         expect(mockRobot1.getDimensionsWithoutRotation).not.toHaveBeenCalled()
     })
 
     test("should penalize robot if it exceeds max height", () => {
-        RobotDimensionTracker.setConfigValues(true, 3, 5)
+        RobotDimensionTracker.setConfigValues(true, 3, 5, 1.5, 5)
 
         mockRobot1.getDimensionsWithoutRotation = vi.fn().mockReturnValue({ height: 2.0, width: 1.0, depth: 1.0 })
         mockRobot2.getDimensionsWithoutRotation = vi.fn().mockReturnValue({ height: 3.5, width: 1.0, depth: 1.0 })
@@ -139,8 +174,65 @@ describe("RobotDimensionTracker", () => {
         )
     })
 
+    test("should penalize robot if it exceeds side max extension (width)", () => {
+        RobotDimensionTracker.setConfigValues(true, 10, 5, 0.5, 2)
+
+        mockRobot1.getDimensions = vi.fn().mockReturnValue({ height: 2.0, width: 1.0, depth: 1.0 })
+        mockRobot1.getDimensionsWithoutRotation = vi.fn().mockReturnValue({ height: 2.0, width: 1.0, depth: 1.0 })
+
+        mockRobot2.getDimensions = vi.fn().mockReturnValue({ height: 2.0, width: 1.0, depth: 1.0 })
+        mockRobot2.getDimensionsWithoutRotation = vi.fn().mockReturnValue({ height: 2.0, width: 1.0, depth: 1.0 })
+
+        RobotDimensionTracker.matchStart()
+
+        mockRobot2.getDimensionsWithoutRotation = vi.fn().mockReturnValue({ height: 2.0, width: 1.7, depth: 1.0 })
+
+        RobotDimensionTracker.update(mockSceneRenderer as unknown as TrackerUpdateParam)
+
+        expect(SimulationSystem.robotPenalty).toHaveBeenCalledWith(mockRobot2, 2, expect.any(String))
+        expect(SimulationSystem.robotPenalty).not.toHaveBeenCalledWith(
+            mockRobot1,
+            expect.any(Number),
+            expect.any(String)
+        )
+    })
+
+    test("should penalize robot if it exceeds side max extension (depth)", () => {
+        RobotDimensionTracker.setConfigValues(true, 10, 5, 0.3, 3)
+
+        mockRobot1.getDimensions = vi.fn().mockReturnValue({ height: 2.0, width: 1.0, depth: 1.2 })
+        mockRobot1.getDimensionsWithoutRotation = vi.fn().mockReturnValue({ height: 2.0, width: 1.0, depth: 1.2 })
+
+        mockRobot2.getDimensions = vi.fn().mockReturnValue({ height: 2.0, width: 1.0, depth: 1.2 })
+        mockRobot2.getDimensionsWithoutRotation = vi.fn().mockReturnValue({ height: 2.0, width: 1.0, depth: 1.2 })
+
+        RobotDimensionTracker.matchStart()
+
+        mockRobot2.getDimensionsWithoutRotation = vi.fn().mockReturnValue({ height: 2.0, width: 1.0, depth: 1.7 })
+
+        RobotDimensionTracker.update(mockSceneRenderer as unknown as TrackerUpdateParam)
+
+        expect(SimulationSystem.robotPenalty).toHaveBeenCalledWith(mockRobot2, 3, expect.any(String))
+        expect(SimulationSystem.robotPenalty).not.toHaveBeenCalledWith(
+            mockRobot1,
+            expect.any(Number),
+            expect.any(String)
+        )
+    })
+
+    test("should not penalize a robot for side extension if initial dimensions were not recorded", () => {
+        RobotDimensionTracker.setConfigValues(false, 10, 5, 0.3, 3)
+
+        mockRobot1.getDimensions = vi.fn().mockReturnValue({ height: 2.0, width: 1.0, depth: 1.2 })
+        mockRobot2.getDimensions = vi.fn().mockReturnValue({ height: 2.0, width: 1.0, depth: 1.2 })
+
+        RobotDimensionTracker.update(mockSceneRenderer as unknown as TrackerUpdateParam)
+
+        expect(SimulationSystem.robotPenalty).not.toHaveBeenCalled()
+    })
+
     test("should not penalize robot every frame", () => {
-        RobotDimensionTracker.setConfigValues(true, 3, 5)
+        RobotDimensionTracker.setConfigValues(true, 3, 5, 1.5, 5)
 
         mockRobot1.getDimensionsWithoutRotation = vi.fn().mockReturnValue({ height: 12, width: 1.0, depth: 1.0 })
 
@@ -151,7 +243,7 @@ describe("RobotDimensionTracker", () => {
     })
 
     test("should penalize multiple robots", () => {
-        RobotDimensionTracker.setConfigValues(true, 3.048, 5)
+        RobotDimensionTracker.setConfigValues(true, 3.048, 5, 1.5, 5)
 
         mockRobot1.getDimensionsWithoutRotation = vi.fn().mockReturnValue({ height: 12, width: 1.0, depth: 1.0 })
         mockRobot2.getDimensionsWithoutRotation = vi.fn().mockReturnValue({ height: 12, width: 1.0, depth: 1.0 })
@@ -162,7 +254,7 @@ describe("RobotDimensionTracker", () => {
     })
 
     test("should not penalize robot if it is not a robot", () => {
-        RobotDimensionTracker.setConfigValues(true, 3, 5)
+        RobotDimensionTracker.setConfigValues(true, 3, 5, 1.5, 5)
 
         mockNonRobot.getDimensions = vi.fn().mockReturnValue({ height: 12, width: 1.0, depth: 1.0 })
 
diff --git a/fission/src/ui/panels/configuring/MatchModeConfigPanel.tsx b/fission/src/ui/panels/configuring/MatchModeConfigPanel.tsx

Original file line number	Diff line number	Diff line change
`@@ -13,7 +13,9 @@ class DefaultMatchModeConfigs {`
`13`	`13`	`endgameTime: 20,`
`14`	`14`	`ignoreRotation: true,`
`15`	`15`	`maxHeight: Infinity,`
`16`		`- heightPenalty: 0,`
	`16`	`+ heightLimitPenalty: 0,`
	`17`	`+ sideMaxExtension: convertFeetToMeters(1.5),`
	`18`	`+ sideExtensionPenalty: 0,`
`17`	`19`	`}`
`18`	`20`	`}`
`19`	`21`
`@@ -27,7 +29,9 @@ class DefaultMatchModeConfigs {`
`27`	`29`	`endgameTime: 20,`
`28`	`30`	`ignoreRotation: true,`
`29`	`31`	`maxHeight: convertFeetToMeters(4),`
`30`		`- heightPenalty: 2,`
	`32`	`+ heightLimitPenalty: 2,`
	`33`	`+ sideMaxExtension: convertFeetToMeters(1),`
	`34`	`+ sideExtensionPenalty: 2,`
`31`	`35`	`}`
`32`	`36`	`}`
`33`	`37`
`@@ -41,7 +45,9 @@ class DefaultMatchModeConfigs {`
`41`	`45`	`endgameTime: 30,`
`42`	`46`	`ignoreRotation: true,`
`43`	`47`	`maxHeight: convertFeetToMeters(6.5),`
`44`		`- heightPenalty: 5,`
	`48`	`+ heightLimitPenalty: 5,`
	`49`	`+ sideMaxExtension: convertFeetToMeters(4),`
	`50`	`+ sideExtensionPenalty: 5,`
`45`	`51`	`}`
`46`	`52`	`}`
`47`	`53`
`@@ -55,7 +61,9 @@ class DefaultMatchModeConfigs {`
`55`	`61`	`endgameTime: 5,`
`56`	`62`	`ignoreRotation: true,`
`57`	`63`	`maxHeight: Infinity,`
`58`		`- heightPenalty: 0,`
	`64`	`+ heightLimitPenalty: 0,`
	`65`	`+ sideMaxExtension: Infinity,`
	`66`	`+ sideExtensionPenalty: 0,`
`59`	`67`	`}`
`60`	`68`	`}`
`61`	`69`