Skip to content

Commit 13c0055

Browse files
Superbro525AltSuperbro525Alt
committed
a
1 parent 0855ecc commit 13c0055

File tree

2 files changed

+228
-11
lines changed

2 files changed

+228
-11
lines changed

‎src/main/java/org/curtinfrc/frc2026/util/Repulsor/Predictive/PredictiveCollectSecondaryRankers.java‎

Lines changed: 46 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,6 @@
2121
import edu.wpi.first.math.geometry.Pose2d;
2222
import edu.wpi.first.math.geometry.Rotation2d;
2323
import edu.wpi.first.math.geometry.Translation2d;
24-
import edu.wpi.first.wpilibj.Timer;
2524
import org.curtinfrc.frc2026.util.Repulsor.Predictive.Internal.CollectEval;
2625
import org.curtinfrc.frc2026.util.Repulsor.Predictive.Internal.IntentAggCont;
2726
import org.curtinfrc.frc2026.util.Repulsor.Predictive.Internal.ResourceRegions;
@@ -99,11 +98,18 @@ void addDepletedMark(
9998
private static final double HIERARCHICAL_ANCHOR_MAX_DIST_M = 0.55;
10099
private static final double HIERARCHICAL_ANCHOR_EVIDENCE_SCALE = 0.75;
101100
private static final double HIERARCHICAL_ANCHOR_UNITS_SCALE = 0.60;
101+
private static final double SECONDARY_ANCHOR_EVIDENCE_SCALE = 0.65;
102+
private static final double SECONDARY_ANCHOR_UNITS_SCALE = 0.55;
103+
private static final double SECONDARY_FINAL_UNITS_SCALE = 0.70;
102104
private static final double ACTIVITY_CAP = 1.05;
103105
private static final int ENEMY_REGIONS_MAX = 24;
104106

105107
private PredictiveCollectSecondaryRankers() {}
106108

109+
private static double nowSec() {
110+
return System.nanoTime() / 1e9;
111+
}
112+
107113
static Translation2d anchorHierarchicalPointToFuel(
108114
SpatialDyn dyn, Translation2d seed, double cellM, double minUnits, double minEv) {
109115
if (dyn == null || seed == null) return null;
@@ -219,7 +225,7 @@ static PointCandidate rankCollectHierarchical(
219225
- e.allyIntent * COLLECT_ALLY_INTENT_COST
220226
- activity
221227
- e.depleted * DEPLETED_PEN_W
222-
+ api.regionBanditBonus(dyn, cpt, Timer.getFPGATimestamp());
228+
+ api.regionBanditBonus(dyn, cpt, nowSec());
223229

224230
if (ev < minEv) score -= 2.00;
225231

@@ -275,7 +281,7 @@ static PointCandidate rankCollectHierarchical(
275281

276282
CollectEval e =
277283
api.evalCollectPoint(ourPos, cap, p, goal, cellM, dyn, enemyIntent, allyIntent);
278-
e.banditBonus = api.regionBanditBonus(dyn, p, Timer.getFPGATimestamp());
284+
e.banditBonus = api.regionBanditBonus(dyn, p, nowSec());
279285
e.score += e.banditBonus;
280286

281287
if (e.units < minUnits * 0.55 || e.count < Math.max(1, minCount - 1)) e.score -= 2.75;
@@ -325,7 +331,7 @@ static PointCandidate rankCollectHierarchical(
325331
order[best] = tmp;
326332
}
327333

328-
double now = Timer.getFPGATimestamp();
334+
double now = nowSec();
329335

330336
for (int oi = 0; oi < candN; oi++) {
331337
int ci = order[oi];
@@ -402,20 +408,37 @@ static Translation2d bestCollectHotspot(Api api, Translation2d[] points, double
402408
SpatialDyn dyn = api.cachedDyn();
403409
if (dyn == null || dyn.resources.isEmpty()) return null;
404410

411+
double totalEv = dyn.totalEvidence();
412+
double minUnits = Math.max(0.02, api.dynamicMinUnits(totalEv));
413+
double minEv = Math.max(0.01, api.minEvidence(totalEv));
414+
405415
double half = Math.max(0.05, cellM * 0.5);
416+
double rCore = Math.max(0.05, PredictiveFieldStateOps.coreRadiusFor(cellM));
406417
Translation2d best = null;
407418
double bestU = 0.0;
408419

409420
for (Translation2d p : points) {
410421
if (p == null) continue;
411-
double u = dyn.valueInSquare(p, half);
422+
Translation2d anchored =
423+
anchorHierarchicalPointToFuel(
424+
dyn,
425+
p,
426+
cellM,
427+
Math.max(0.02, minUnits * SECONDARY_ANCHOR_UNITS_SCALE),
428+
minEv * SECONDARY_ANCHOR_EVIDENCE_SCALE);
429+
if (anchored == null) continue;
430+
431+
int coreCount = dyn.countResourcesWithin(anchored, rCore);
432+
if (coreCount < 1) continue;
433+
434+
double u = dyn.valueInSquare(anchored, half);
412435
if (u > bestU) {
413436
bestU = u;
414-
best = p;
437+
best = anchored;
415438
}
416439
}
417440

418-
double min = Math.max(0.02, COLLECT_FINE_MIN_UNITS * 0.5);
441+
double min = Math.max(0.02, minUnits * SECONDARY_FINAL_UNITS_SCALE);
419442
return bestU >= min ? best : null;
420443
}
421444

@@ -472,17 +495,29 @@ static PointCandidate rankCollectPoints(
472495
Translation2d p = targets[i];
473496
if (p == null) continue;
474497

498+
Translation2d anchored =
499+
anchorHierarchicalPointToFuel(
500+
dyn,
501+
p,
502+
COLLECT_CELL_M,
503+
Math.max(0.02, minUnits * SECONDARY_ANCHOR_UNITS_SCALE),
504+
minEv * SECONDARY_ANCHOR_EVIDENCE_SCALE);
505+
if (anchored == null) continue;
506+
475507
CollectEval e =
476-
api.evalCollectPoint(ourPos, cap, p, goal, COLLECT_CELL_M, dyn, enemyIntent, allyIntent);
477-
e.banditBonus = api.regionBanditBonus(dyn, p, Timer.getFPGATimestamp());
508+
api.evalCollectPoint(
509+
ourPos, cap, anchored, goal, COLLECT_CELL_M, dyn, enemyIntent, allyIntent);
510+
e.banditBonus = api.regionBanditBonus(dyn, anchored, nowSec());
478511
e.score += e.banditBonus;
479512

480513
if (e.units < minUnits * 0.55 || e.count < Math.max(1, minCount - 1)) e.score -= 2.75;
481514
if (e.evidence < minEv) e.score -= 2.25;
515+
if (dyn.countResourcesWithin(anchored, PredictiveFieldStateOps.coreRadiusFor(COLLECT_CELL_M))
516+
< 1) e.score -= 3.0;
482517

483518
if (bestE == null || e.score > bestE.score + 1e-9) {
484519
bestE = e;
485-
bestP = p;
520+
bestP = anchored;
486521
}
487522
}
488523

@@ -494,7 +529,7 @@ static PointCandidate rankCollectPoints(
494529
return null;
495530
}
496531

497-
api.setLastReturnedCollect(bestP, Timer.getFPGATimestamp());
532+
api.setLastReturnedCollect(bestP, nowSec());
498533

499534
Logger.recordOutput("Repulsor/ChosenCollect", new Pose2d(bestP, new Rotation2d()));
500535

‎src/test/java/org/curtinfrc/frc2026/util/Repulsor/Predictive/PredictiveCollectSecondaryRankersTest.java‎

Lines changed: 182 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,12 +3,17 @@
33
import static org.junit.jupiter.api.Assertions.assertEquals;
44
import static org.junit.jupiter.api.Assertions.assertNotNull;
55
import static org.junit.jupiter.api.Assertions.assertNull;
6+
import static org.junit.jupiter.api.Assertions.assertTrue;
67

78
import edu.wpi.first.math.geometry.Translation2d;
89
import java.util.HashMap;
910
import java.util.List;
1011
import java.util.Set;
12+
import org.curtinfrc.frc2026.util.Repulsor.Predictive.Internal.CollectEval;
13+
import org.curtinfrc.frc2026.util.Repulsor.Predictive.Internal.IntentAggCont;
14+
import org.curtinfrc.frc2026.util.Repulsor.Predictive.Internal.ResourceRegions;
1115
import org.curtinfrc.frc2026.util.Repulsor.Predictive.Model.DynamicObject;
16+
import org.curtinfrc.frc2026.util.Repulsor.Predictive.Model.PointCandidate;
1217
import org.curtinfrc.frc2026.util.Repulsor.Predictive.Model.ResourceSpec;
1318
import org.junit.jupiter.api.Test;
1419

@@ -65,6 +70,183 @@ void anchorHierarchicalPointToFuelRejectsWeakFuel() {
6570
assertNull(anchored);
6671
}
6772

73+
@Test
74+
void bestCollectHotspotAnchorsToRealFuel() {
75+
SpatialDyn dyn =
76+
makeDyn(
77+
List.of(
78+
new DynamicObject(
79+
"f1", "fuel", new Translation2d(4.0, 2.0), new Translation2d(), 0.05)),
80+
1.0);
81+
82+
TestApi api = new TestApi(dyn);
83+
Translation2d out =
84+
PredictiveCollectSecondaryRankers.bestCollectHotspot(
85+
api, new Translation2d[] {new Translation2d(4.30, 2.0)}, 0.75);
86+
87+
assertNotNull(out);
88+
assertTrue(out.getDistance(new Translation2d(4.0, 2.0)) <= 0.25);
89+
}
90+
91+
@Test
92+
void rankCollectPointsReturnsNullWhenNoFuelNearCandidates() {
93+
SpatialDyn dyn =
94+
makeDyn(
95+
List.of(
96+
new DynamicObject(
97+
"f1", "fuel", new Translation2d(4.0, 2.0), new Translation2d(), 0.05)),
98+
1.0);
99+
TestApi api = new TestApi(dyn);
100+
101+
PointCandidate out =
102+
PredictiveCollectSecondaryRankers.rankCollectPoints(
103+
api,
104+
new Translation2d(2.0, 2.0),
105+
3.0,
106+
new Translation2d[] {new Translation2d(6.2, 2.0)},
107+
1,
108+
8);
109+
110+
assertNull(out);
111+
}
112+
113+
@Test
114+
void rankCollectPointsAnchorsOffFuelCandidateToRealFuel() {
115+
SpatialDyn dyn =
116+
makeDyn(
117+
List.of(
118+
new DynamicObject(
119+
"f1", "fuel", new Translation2d(4.0, 2.0), new Translation2d(), 0.05)),
120+
1.0);
121+
TestApi api = new TestApi(dyn);
122+
123+
PointCandidate out =
124+
PredictiveCollectSecondaryRankers.rankCollectPoints(
125+
api,
126+
new Translation2d(3.2, 2.0),
127+
3.0,
128+
new Translation2d[] {new Translation2d(4.30, 2.0)},
129+
1,
130+
8);
131+
132+
assertNotNull(out);
133+
assertTrue(out.point.getDistance(new Translation2d(4.0, 2.0)) <= 0.25);
134+
}
135+
136+
private static final class TestApi implements PredictiveCollectSecondaryRankers.Api {
137+
private final SpatialDyn dyn;
138+
private Translation2d[] candidates = new Translation2d[0];
139+
140+
TestApi(SpatialDyn dyn) {
141+
this.dyn = dyn;
142+
}
143+
144+
@Override
145+
public SpatialDyn cachedDyn() {
146+
return dyn;
147+
}
148+
149+
@Override
150+
public void setCollectContext(
151+
Translation2d ourPos, double ourSpeedCap, int goalUnits, double cellM) {}
152+
153+
@Override
154+
public void sweepDepletedMarks() {}
155+
156+
@Override
157+
public Translation2d[] buildCollectCandidates(Translation2d[] gridPoints, SpatialDyn dyn) {
158+
candidates = gridPoints != null ? gridPoints : new Translation2d[0];
159+
return candidates;
160+
}
161+
162+
@Override
163+
public double dynamicMinUnits(double totalEvidence) {
164+
return 0.06;
165+
}
166+
167+
@Override
168+
public int dynamicMinCount(double totalEvidence) {
169+
return 1;
170+
}
171+
172+
@Override
173+
public double minEvidence(double totalEvidence) {
174+
return 0.03;
175+
}
176+
177+
@Override
178+
public ResourceRegions buildResourceRegions(SpatialDyn dyn, int maxRegions) {
179+
return new ResourceRegions(new Translation2d[0], new double[0]);
180+
}
181+
182+
@Override
183+
public IntentAggCont enemyIntentToRegions(ResourceRegions regs) {
184+
return new IntentAggCont(new Translation2d[0], new double[0], 0, 1.0);
185+
}
186+
187+
@Override
188+
public IntentAggCont allyIntentToRegions(ResourceRegions regs) {
189+
return new IntentAggCont(new Translation2d[0], new double[0], 0, 1.0);
190+
}
191+
192+
@Override
193+
public double estimateTravelTime(Translation2d a, Translation2d b, double speed) {
194+
return a.getDistance(b) / Math.max(0.2, speed);
195+
}
196+
197+
@Override
198+
public CollectEval evalCollectPoint(
199+
Translation2d ourPos,
200+
double ourSpeedCap,
201+
Translation2d p,
202+
int goalUnits,
203+
double cellM,
204+
SpatialDyn dyn,
205+
IntentAggCont enemyIntent,
206+
IntentAggCont allyIntent) {
207+
CollectEval e = new CollectEval();
208+
e.p = p;
209+
e.eta = estimateTravelTime(ourPos, p, ourSpeedCap);
210+
e.units = dyn.valueInSquare(p, 0.38);
211+
e.count = dyn.countResourcesWithin(p, 0.42);
212+
e.evidence = dyn.evidenceMassWithin(p, 0.85);
213+
e.value = e.units;
214+
e.enemyPressure = 0.0;
215+
e.allyCongestion = 0.0;
216+
e.enemyIntent = 0.0;
217+
e.allyIntent = 0.0;
218+
e.depleted = 0.0;
219+
e.score = e.units - (0.1 * e.eta);
220+
return e;
221+
}
222+
223+
@Override
224+
public double allyRadialDensity(Translation2d p, double sigma) {
225+
return 0.0;
226+
}
227+
228+
@Override
229+
public double enemyRadialDensity(Translation2d p, double sigma) {
230+
return 0.0;
231+
}
232+
233+
@Override
234+
public double regionBanditBonus(SpatialDyn dyn, Translation2d p, double now) {
235+
return 0.0;
236+
}
237+
238+
@Override
239+
public void addDepletedMark(
240+
Translation2d p, double radiusM, double strength, double ttlS, boolean merge) {}
241+
242+
@Override
243+
public void addDepletedRing(
244+
Translation2d p, double r0, double r1, double strength, double ttlS) {}
245+
246+
@Override
247+
public void setLastReturnedCollect(Translation2d p, double nowS) {}
248+
}
249+
68250
private static SpatialDyn makeDyn(List<DynamicObject> objects, double fuelUnitValue) {
69251
HashMap<String, ResourceSpec> specs = new HashMap<>();
70252
specs.put("fuel", new ResourceSpec(0.10, fuelUnitValue, 0.06));

0 commit comments

Comments
 (0)