Commit 85f3fee4 authored by NicolaiO's avatar NicolaiO 🐼 Committed by TIGERs GitLab
Browse files

Added estimator for redirected balls with spin.

Closes #1636

See merge request main/Sumatra!1382

sumatra-commit: 05b5e598be93d0fe899b542f50dc403bf50fc7e6
parent 175c60bc
Pipeline #16687 passed with stage
in 3 minutes and 45 seconds
...@@ -4,6 +4,7 @@ ...@@ -4,6 +4,7 @@
package edu.tigers.sumatra.ball.trajectory; package edu.tigers.sumatra.ball.trajectory;
import edu.tigers.sumatra.ball.BallParameters;
import edu.tigers.sumatra.ball.BallState; import edu.tigers.sumatra.ball.BallState;
import edu.tigers.sumatra.math.IMirrorable; import edu.tigers.sumatra.math.IMirrorable;
import edu.tigers.sumatra.math.line.v2.IHalfLine; import edu.tigers.sumatra.math.line.v2.IHalfLine;
...@@ -20,6 +21,13 @@ import java.util.List; ...@@ -20,6 +21,13 @@ import java.util.List;
*/ */
public interface IBallTrajectory extends IMirrorable<IBallTrajectory>, IPlanarCurveProvider public interface IBallTrajectory extends IMirrorable<IBallTrajectory>, IPlanarCurveProvider
{ {
/**
* Get ball parameters for this trajectory.
*
* @return
*/
BallParameters getParameters();
/** /**
* Get initial position in [mm]. * Get initial position in [mm].
* *
...@@ -228,4 +236,12 @@ public interface IBallTrajectory extends IMirrorable<IBallTrajectory>, IPlanarCu ...@@ -228,4 +236,12 @@ public interface IBallTrajectory extends IMirrorable<IBallTrajectory>, IPlanarCu
* @return * @return
*/ */
IBallTrajectory withAdjustedInitialPos(final IVector3 posNow, final double time); IBallTrajectory withAdjustedInitialPos(final IVector3 posNow, final double time);
/**
* Return a new trajectory with new ball parameters.
*
* @param ballParameters
* @return
*/
IBallTrajectory withBallParameters(final BallParameters ballParameters);
} }
...@@ -125,6 +125,13 @@ public class ChipBallTrajectory extends ABallTrajectory ...@@ -125,6 +125,13 @@ public class ChipBallTrajectory extends ABallTrajectory
} }
@Override
public IBallTrajectory withBallParameters(BallParameters ballParameters)
{
return new ChipBallTrajectory(ballParameters, initialPos, initialVel, initialSpin, tInAir, kickPos, kickVel);
}
@Override @Override
public BallState getMilliStateAtTime(final double time) public BallState getMilliStateAtTime(final double time)
{ {
......
...@@ -142,6 +142,13 @@ public class FlatBallTrajectory extends ABallTrajectory ...@@ -142,6 +142,13 @@ public class FlatBallTrajectory extends ABallTrajectory
} }
@Override
public IBallTrajectory withBallParameters(BallParameters ballParameters)
{
return new FlatBallTrajectory(ballParameters, initialPos.getXYVector(), initialVel.getXYVector(), initialSpin);
}
@Override @Override
public ABallTrajectory mirrored() public ABallTrajectory mirrored()
{ {
......
...@@ -64,6 +64,11 @@ public class DrawablePlanarCurve extends ADrawableWithStroke ...@@ -64,6 +64,11 @@ public class DrawablePlanarCurve extends ADrawableWithStroke
points.add(pos); points.add(pos);
vLast = vel; vLast = vel;
} }
if (t > stepSize * 1000)
{
break;
}
} }
points.add(curve.getState(curve.getTEnd()).getPos()); points.add(curve.getState(curve.getTEnd()).getPos());
......
...@@ -46,6 +46,13 @@ public class BallParameters ...@@ -46,6 +46,13 @@ public class BallParameters
) )
private double inertiaDistribution = 0.5; private double inertiaDistribution = 0.5;
@Configurable(
comment = "Amount of spin transferred during a redirect.",
defValue = "0.75",
spezis = { "SUMATRA", "LAB", "TISCH", "ROBOCUP", "ANDRE", "NICOLAI", "SIMULATOR" }
)
private double redirectSpinFactor = 0.75;
@Configurable( @Configurable(
comment = "Fixed velocity where the ball starts to roll [mm/s]", comment = "Fixed velocity where the ball starts to roll [mm/s]",
defValue = "2000.0", defValue = "2000.0",
......
...@@ -95,7 +95,7 @@ public class BallFilterPreprocessor ...@@ -95,7 +95,7 @@ public class BallFilterPreprocessor
MergedBall optMergedBall = ballTrackerMerger.process(ballTrackers, timestamp, lastFilteredBall); MergedBall optMergedBall = ballTrackerMerger.process(ballTrackers, timestamp, lastFilteredBall);
KickEvent optKickEvent = kickDetectors.process(optMergedBall, mergedRobots); KickEvent optKickEvent = kickDetectors.process(optMergedBall, mergedRobots);
KickFitResult optBestKickFitResult = kickEstimators.process(optKickEvent, KickFitResult optBestKickFitResult = kickEstimators.process(optKickEvent,
optMergedBall, mergedRobots, robotInfos, timestamp); optMergedBall, mergedRobots, robotInfos, timestamp, lastFilteredBall);
return new BallFilterPreprocessorOutput(optMergedBall, optKickEvent, optBestKickFitResult); return new BallFilterPreprocessorOutput(optMergedBall, optKickEvent, optBestKickFitResult);
} }
...@@ -157,6 +157,7 @@ public class BallFilterPreprocessor ...@@ -157,6 +157,7 @@ public class BallFilterPreprocessor
private IKickEstimator lastBestEstimator; private IKickEstimator lastBestEstimator;
private long lastKickTimestamp = 0; private long lastKickTimestamp = 0;
private CircularFifoQueue<KickEvent> kickEventHistory = new CircularFifoQueue<>(10); private CircularFifoQueue<KickEvent> kickEventHistory = new CircularFifoQueue<>(10);
private CircularFifoQueue<FilteredVisionBall> filteredBallHistory = new CircularFifoQueue<>(20);
private KickEstimators() private KickEstimators()
...@@ -174,8 +175,10 @@ public class BallFilterPreprocessor ...@@ -174,8 +175,10 @@ public class BallFilterPreprocessor
private KickFitResult process(final KickEvent kickEvent, final MergedBall ball, private KickFitResult process(final KickEvent kickEvent, final MergedBall ball,
final List<FilteredVisionBot> mergedRobots, final Map<BotID, RobotInfo> robotInfos, final List<FilteredVisionBot> mergedRobots, final Map<BotID, RobotInfo> robotInfos,
final long timestamp) final long timestamp, final FilteredVisionBall lastFilteredBall)
{ {
filteredBallHistory.add(lastFilteredBall);
if ((ball != null) && ball.getLatestCamBall().isPresent()) if ((ball != null) && ball.getLatestCamBall().isPresent())
{ {
// add cam ball to all estimators // add cam ball to all estimators
...@@ -188,7 +191,7 @@ public class BallFilterPreprocessor ...@@ -188,7 +191,7 @@ public class BallFilterPreprocessor
estimators.stream() estimators.stream()
.filter(e -> e.isDone(mergedRobots, timestamp)) .filter(e -> e.isDone(mergedRobots, timestamp))
.map(IKickEstimator::getModelIdentResult) .map(IKickEstimator::getModelIdentResult)
.flatMap(Optional::stream) .flatMap(List::stream)
.forEach(this::notifyBallModelIdentificationResult); .forEach(this::notifyBallModelIdentificationResult);
} }
if (lastBestEstimator != null && lastBestEstimator.isDone(mergedRobots, timestamp)) if (lastBestEstimator != null && lastBestEstimator.isDone(mergedRobots, timestamp))
...@@ -269,7 +272,7 @@ public class BallFilterPreprocessor ...@@ -269,7 +272,7 @@ public class BallFilterPreprocessor
if (flatEstimator == null) if (flatEstimator == null)
{ {
flatEstimator = new StraightKickEstimator(kickEvent); flatEstimator = new StraightKickEstimator(kickEvent, filteredBallHistory);
log.debug("Spawned flat estimator"); log.debug("Spawned flat estimator");
} }
...@@ -284,7 +287,7 @@ public class BallFilterPreprocessor ...@@ -284,7 +287,7 @@ public class BallFilterPreprocessor
(lastBestEstimator.getFitResult().get().getKickPos().distanceTo(kickEvent.getPosition()) > 500.0)) (lastBestEstimator.getFitResult().get().getKickPos().distanceTo(kickEvent.getPosition()) > 500.0))
{ {
// large angle deviation or some distance away from last kick, spawn new estimator // large angle deviation or some distance away from last kick, spawn new estimator
flatEstimator = new StraightKickEstimator(kickEvent); flatEstimator = new StraightKickEstimator(kickEvent, filteredBallHistory);
log.debug("Spawned flat estimator due to angle/pos deviation"); log.debug("Spawned flat estimator due to angle/pos deviation");
} else } else
{ {
...@@ -335,7 +338,7 @@ public class BallFilterPreprocessor ...@@ -335,7 +338,7 @@ public class BallFilterPreprocessor
.removeIf(k -> k.getFitResult() .removeIf(k -> k.getFitResult()
.orElse(new KickFitResult(null, 0, .orElse(new KickFitResult(null, 0,
Geometry.getBallFactory() Geometry.getBallFactory()
.createTrajectoryFromBallAtRest(Vector2f.ZERO_VECTOR), timestamp)) .createTrajectoryFromBallAtRest(Vector2f.ZERO_VECTOR), timestamp, ""))
.getAvgDistance() > bestKickFitResult.getAvgDistance()); .getAvgDistance() > bestKickFitResult.getAvgDistance());
} }
...@@ -380,6 +383,9 @@ public class BallFilterPreprocessor ...@@ -380,6 +383,9 @@ public class BallFilterPreprocessor
private void reset() private void reset()
{ {
estimators.clear(); estimators.clear();
kickEventHistory.clear();
filteredBallHistory.clear();
lastBestEstimator = null;
} }
} }
......
/* /*
* Copyright (c) 2009 - 2018, DHBW Mannheim - Tigers Mannheim * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
*/ */
package edu.tigers.sumatra.vision.data; package edu.tigers.sumatra.vision.data;
import edu.tigers.sumatra.math.vector.IVector2; import edu.tigers.sumatra.math.vector.IVector2;
import edu.tigers.sumatra.math.vector.IVector3; import edu.tigers.sumatra.math.vector.IVector3;
import lombok.AllArgsConstructor;
import lombok.Value;
import java.util.Optional;
/** /**
...@@ -12,11 +16,15 @@ import edu.tigers.sumatra.math.vector.IVector3; ...@@ -12,11 +16,15 @@ import edu.tigers.sumatra.math.vector.IVector3;
* *
* @author AndreR <andre@ryll.cc> * @author AndreR <andre@ryll.cc>
*/ */
@Value
@AllArgsConstructor
public class KickSolverResult public class KickSolverResult
{ {
private final IVector2 kickPosition; IVector2 kickPosition;
private final IVector3 kickVelocity; IVector3 kickVelocity;
private final long kickTimestamp; long kickTimestamp;
IVector2 kickSpin;
String solverName;
/** /**
...@@ -24,37 +32,19 @@ public class KickSolverResult ...@@ -24,37 +32,19 @@ public class KickSolverResult
* @param kickVelocity * @param kickVelocity
* @param kickTimestamp * @param kickTimestamp
*/ */
public KickSolverResult(final IVector2 kickPosition, final IVector3 kickVelocity, final long kickTimestamp) public KickSolverResult(final IVector2 kickPosition, final IVector3 kickVelocity, final long kickTimestamp,
String solverName)
{ {
this.kickPosition = kickPosition; this.kickPosition = kickPosition;
this.kickVelocity = kickVelocity; this.kickVelocity = kickVelocity;
this.kickTimestamp = kickTimestamp; this.kickTimestamp = kickTimestamp;
this.kickSpin = null;
this.solverName = solverName;
} }
/** public Optional<IVector2> getKickSpin()
* @return the kickPosition
*/
public IVector2 getKickPosition()
{
return kickPosition;
}
/**
* @return the kickVelocity
*/
public IVector3 getKickVelocity()
{
return kickVelocity;
}
/**
* @return the kickTimestamp
*/
public long getKickTimestamp()
{ {
return kickTimestamp; return Optional.ofNullable(kickSpin);
} }
} }
...@@ -143,7 +143,7 @@ public class ChipKickEstimator implements IKickEstimator ...@@ -143,7 +143,7 @@ public class ChipKickEstimator implements IKickEstimator
currentTraj = Geometry.getBallFactory() currentTraj = Geometry.getBallFactory()
.createTrajectoryFromKickedBallWithoutSpin(event.getPosition(), kickVel); .createTrajectoryFromKickedBallWithoutSpin(event.getPosition(), kickVel);
kickTimestamp = event.getTimestamp(); kickTimestamp = event.getTimestamp();
fitResult = new KickFitResult(new ArrayList<>(), 0, currentTraj, kickTimestamp); fitResult = new KickFitResult(new ArrayList<>(), 0, currentTraj, kickTimestamp, "");
usesPriorKnowledge = true; usesPriorKnowledge = true;
} }
...@@ -168,7 +168,7 @@ public class ChipKickEstimator implements IKickEstimator ...@@ -168,7 +168,7 @@ public class ChipKickEstimator implements IKickEstimator
{ {
if (usesPriorKnowledge) if (usesPriorKnowledge)
{ {
computeFitResult(); computeFitResult("prior");
} }
return; return;
...@@ -205,11 +205,11 @@ public class ChipKickEstimator implements IKickEstimator ...@@ -205,11 +205,11 @@ public class ChipKickEstimator implements IKickEstimator
currentTraj = Geometry.getBallFactory() currentTraj = Geometry.getBallFactory()
.createTrajectoryFromKickedBallWithoutSpin(kickPos, kickVel); .createTrajectoryFromKickedBallWithoutSpin(kickPos, kickVel);
computeFitResult(); computeFitResult(optSolverResult.get().getSolverName());
} }
private void computeFitResult() private void computeFitResult(String solverName)
{ {
List<IVector2> modelPoints = records.stream() List<IVector2> modelPoints = records.stream()
.map(r -> currentTraj.getMilliStateAtTime((r.gettCapture() - kickTimestamp) * 1e-9).getPos() .map(r -> currentTraj.getMilliStateAtTime((r.gettCapture() - kickTimestamp) * 1e-9).getPos()
...@@ -228,7 +228,7 @@ public class ChipKickEstimator implements IKickEstimator ...@@ -228,7 +228,7 @@ public class ChipKickEstimator implements IKickEstimator
} }
// save the fit result // save the fit result
fitResult = new KickFitResult(modelPoints, avgDist, currentTraj, kickTimestamp); fitResult = new KickFitResult(modelPoints, avgDist, currentTraj, kickTimestamp, solverName);
} }
...@@ -356,16 +356,18 @@ public class ChipKickEstimator implements IKickEstimator ...@@ -356,16 +356,18 @@ public class ChipKickEstimator implements IKickEstimator
@Override @Override
public Optional<IBallModelIdentResult> getModelIdentResult() public List<IBallModelIdentResult> getModelIdentResult()
{ {
List<IBallModelIdentResult> result = new ArrayList<>();
if (allRecords.size() < 20) if (allRecords.size() < 20)
{ {
return Optional.empty(); return result;
} }
if (solverNonLin == null) if (solverNonLin == null)
{ {
return Optional.empty(); return result;
} }
long timeAfterKick = allRecords.get(0).gettCapture() + 500_000_000; long timeAfterKick = allRecords.get(0).gettCapture() + 500_000_000;
...@@ -380,11 +382,13 @@ public class ChipKickEstimator implements IKickEstimator ...@@ -380,11 +382,13 @@ public class ChipKickEstimator implements IKickEstimator
ChipKickSolverNonLinIdentDirect identSolver = new ChipKickSolverNonLinIdentDirect( ChipKickSolverNonLinIdentDirect identSolver = new ChipKickSolverNonLinIdentDirect(
solverNonLin.getKickPosition(), solverNonLin.getKickTimestamp(), camCalib, fitResult.getKickVel()); solverNonLin.getKickPosition(), solverNonLin.getKickTimestamp(), camCalib, fitResult.getKickVel());
Optional<IBallModelIdentResult> result = identSolver.identModel(usedRecords); Optional<IBallModelIdentResult> chipResult = identSolver.identModel(usedRecords);
if (result.isPresent()) if (chipResult.isPresent())
{ {
result.add(chipResult.get());
final String lineSeparator = System.lineSeparator(); final String lineSeparator = System.lineSeparator();
log.info("Chip Model:{}{}", lineSeparator, result.get()); log.info("Chip Model:{}{}", lineSeparator, chipResult.get());
} else } else
{ {
log.info("Chip model identification failed."); log.info("Chip model identification failed.");
......
/* /*
* Copyright (c) 2009 - 2018, DHBW Mannheim - Tigers Mannheim * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
*/ */
package edu.tigers.sumatra.vision.kick.estimators; package edu.tigers.sumatra.vision.kick.estimators;
import edu.tigers.sumatra.vision.kick.estimators.chip.ChipKickSolverNonLinIdentDirect.ChipModelIdentResult; import edu.tigers.sumatra.vision.kick.estimators.chip.ChipKickSolverNonLinIdentDirect.ChipModelIdentResult;
import edu.tigers.sumatra.vision.kick.estimators.straight.FlatKickSolverNonLin3Factor;
import edu.tigers.sumatra.vision.kick.estimators.straight.StraightKickSolverNonLinIdentDirect.StraightModelIdentResult; import edu.tigers.sumatra.vision.kick.estimators.straight.StraightKickSolverNonLinIdentDirect.StraightModelIdentResult;
...@@ -13,12 +14,14 @@ import edu.tigers.sumatra.vision.kick.estimators.straight.StraightKickSolverNonL ...@@ -13,12 +14,14 @@ import edu.tigers.sumatra.vision.kick.estimators.straight.StraightKickSolverNonL
public enum EBallModelIdentType public enum EBallModelIdentType
{ {
STRAIGHT_TWO_PHASE(StraightModelIdentResult.getParameterNames()), STRAIGHT_TWO_PHASE(StraightModelIdentResult.getParameterNames()),
CHIP_FIXED_LOSS_PLUS_ROLLING(ChipModelIdentResult.getParameterNames()); CHIP_FIXED_LOSS_PLUS_ROLLING(ChipModelIdentResult.getParameterNames()),
REDIRECT(FlatKickSolverNonLin3Factor.RedirectModelIdentResult.getParameterNames()),
;
private final String[] parameterNames; private final String[] parameterNames;
private EBallModelIdentType(final String[] parameterNames) EBallModelIdentType(final String[] parameterNames)
{ {
this.parameterNames = parameterNames; this.parameterNames = parameterNames;
} }
......
/* /*
* Copyright (c) 2009 - 2016, DHBW Mannheim - Tigers Mannheim * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
*/ */
package edu.tigers.sumatra.vision.kick.estimators; package edu.tigers.sumatra.vision.kick.estimators;
import java.util.List;
import java.util.Optional;
import edu.tigers.sumatra.cam.data.CamBall; import edu.tigers.sumatra.cam.data.CamBall;
import edu.tigers.sumatra.drawable.IDrawableShape; import edu.tigers.sumatra.drawable.IDrawableShape;
import edu.tigers.sumatra.vision.data.FilteredVisionBot; import edu.tigers.sumatra.vision.data.FilteredVisionBot;
import java.util.List;
import java.util.Optional;
/** /**
* @author AndreR <andre@ryll.cc> * @author AndreR <andre@ryll.cc>
...@@ -63,5 +63,5 @@ public interface IKickEstimator ...@@ -63,5 +63,5 @@ public interface IKickEstimator
* *
* @return * @return
*/ */
Optional<IBallModelIdentResult> getModelIdentResult(); List<IBallModelIdentResult> getModelIdentResult();
} }
...@@ -7,6 +7,8 @@ import edu.tigers.sumatra.ball.BallState; ...@@ -7,6 +7,8 @@ import edu.tigers.sumatra.ball.BallState;
import edu.tigers.sumatra.ball.trajectory.IBallTrajectory; import edu.tigers.sumatra.ball.trajectory.IBallTrajectory;
import edu.tigers.sumatra.math.vector.IVector2; import edu.tigers.sumatra.math.vector.IVector2;
import edu.tigers.sumatra.math.vector.IVector3; import edu.tigers.sumatra.math.vector.IVector3;
import lombok.AllArgsConstructor;
import lombok.Builder;
import lombok.Value; import lombok.Value;
import java.util.List; import java.util.List;
...@@ -16,12 +18,15 @@ import java.util.List; ...@@ -16,12 +18,15 @@ import java.util.List;
* Result of a fitted kick. * Result of a fitted kick.
*/ */
@Value @Value
@Builder(setterPrefix = "with", toBuilder = true)
@AllArgsConstructor
public class KickFitResult public class KickFitResult
{ {
List<IVector2> groundProjection; List<IVector2> groundProjection;
double avgDistance; double avgDistance;
IBallTrajectory trajectory; IBallTrajectory trajectory;
long kickTimestamp; long kickTimestamp;