Resolve "Redesign handling of ball data and output of vision filter"

Closes #1633

See merge request main/Sumatra!1377

sumatra-commit: 759d3385863bb04be2acbe3609af0257bf29ede8

modules/common/src/main/java/edu/tigers/sumatra/ball/BallParameters.java

+/*
+ * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
+ */
+
+package edu.tigers.sumatra.ball;
+
+import com.sleepycat.persist.model.Persistent;
+import lombok.AccessLevel;
+import lombok.AllArgsConstructor;
+import lombok.Builder;
+import lombok.Value;
+
+
+/**
+ * All parameters required for ball models.
+ */
+@Persistent
+@Value
+@Builder(setterPrefix = "with", toBuilder = true)
+@AllArgsConstructor(access = AccessLevel.PRIVATE)
+public class BallParameters
+{
+	/**
+	 * Radius of the ball in [mm].
+	 */
+	double ballRadius;
+	/**
+	 * Sliding acceleration in [mm/s^2], expected to be negative.
+	 */
+	double accSlide;
+	/**
+	 * Rolling acceleration in [mm/s^2], expected to be negative.
+	 */
+	double accRoll;
+	/**
+	 * Ball inertia distribution between 0.4 (massive sphere) and 0.66 (hollow sphere).
+	 */
+	double inertiaDistribution;
+	/**
+	 * Chip kick velocity damping factor in XY direction for the first hop.
+	 */
+	double chipDampingXYFirstHop;
+	/**
+	 * Chip kick velocity damping factor in XY direction for all following hops.
+	 */
+	double chipDampingXYOtherHops;
+	/**
+	 * Chip kick velocity damping factor in Z direction.
+	 */
+	double chipDampingZ;
+	/**
+	 * If a chipped ball does not reach this height it is considered rolling [mm].
+	 */
+	double minHopHeight;
+	/**
+	 * Max. ball height that can be intercepted by robots [mm].
+	 */
+	double maxInterceptableHeight;
+
+
+	/**
+	 * Create an empty default state. Required for {@link Persistent}.
+	 */
+	private BallParameters()
+	{
+		ballRadius = 0;
+		accSlide = 0;
+		accRoll = 0;
+		inertiaDistribution = 0;
+		chipDampingXYFirstHop = 0;
+		chipDampingXYOtherHops = 0;
+		chipDampingZ = 0;
+		minHopHeight = 0;
+		maxInterceptableHeight = 0;
+	}
+}

modules/moduli-vision/src/main/java/edu/tigers/sumatra/vision/data/BallTrajectoryState.java → modules/common/src/main/java/edu/tigers/sumatra/ball/BallState.java

@@ -2,11 +2,14 @@
  * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
  */
 
-package edu.tigers.sumatra.vision.data;
+package edu.tigers.sumatra.ball;
 
 import com.sleepycat.persist.model.Persistent;
 import edu.tigers.sumatra.math.IMirrorable;
+import edu.tigers.sumatra.math.SumatraMath;
+import edu.tigers.sumatra.math.vector.IVector2;
 import edu.tigers.sumatra.math.vector.IVector3;
+import edu.tigers.sumatra.math.vector.Vector2f;
 import edu.tigers.sumatra.math.vector.Vector3;
 import edu.tigers.sumatra.math.vector.Vector3f;
 import lombok.AccessLevel;
@@ -19,13 +22,13 @@ import lombok.Value;
 /**
  * Data structure for the ball state at a certain time.
  * <br>
- * <b>WARNING: Units of this class are [mm], [mm/s], [mm/s^2] !!!</b>
+ * <b>WARNING: Units of this class are [mm], [mm/s] ([rad/s]), [mm/s^2] !!!</b>
  */
 @Persistent
 @Value
 @Builder(setterPrefix = "with", toBuilder = true)
 @AllArgsConstructor(access = AccessLevel.PRIVATE)
-public class BallTrajectoryState implements IMirrorable<BallTrajectoryState>
+public class BallState implements IMirrorable<BallState>
 {
 	/**
 	 * Position in [mm]
@@ -46,65 +49,59 @@ public class BallTrajectoryState implements IMirrorable<BallTrajectoryState>
 	IVector3 acc;
 
 	/**
-	 * The velocity where the ball turns to roll
+	 * The spin of the ball in [rad/s], positive spin corresponds to positive linear velocity
 	 */
-	double vSwitchToRoll;
-
-	/**
-	 * Was the ball chipped?
-	 */
-	boolean chipped;
-
-	/**
-	 * The spin of the ball
-	 */
-	double spin;
+	IVector2 spin;
 
 
 	/**
 	 * Create an empty default state. Required for {@link Persistent}.
 	 */
-	public BallTrajectoryState()
+	public BallState()
 	{
 		pos = Vector3f.ZERO_VECTOR;
 		vel = Vector3f.ZERO_VECTOR;
 		acc = Vector3f.ZERO_VECTOR;
-		vSwitchToRoll = 0;
-		chipped = false;
-		spin = 0;
+		spin = Vector2f.ZERO_VECTOR;
 	}
 
 
 	@Override
-	public BallTrajectoryState mirrored()
+	public BallState mirrored()
 	{
 		return toBuilder()
 				.withPos(Vector3.from2d(pos.getXYVector().multiplyNew(-1), pos.getXYZVector().z()))
 				.withVel(Vector3.from2d(vel.getXYVector().multiplyNew(-1), vel.getXYZVector().z()))
 				.withAcc(Vector3.from2d(acc.getXYVector().multiplyNew(-1), acc.getXYZVector().z()))
+				.withSpin(spin.multiplyNew(-1))
 				.build();
 	}
 
 
 	/**
-	 * Get ball trajectory.
+	 * Is this a chipped ball?
+	 * Note that an originally chipped ball enters rolling state at some point and even though it was chipped it
+	 * won't be reported as such by this method.
 	 *
-	 * @param timestampNow
 	 * @return
 	 */
-	public ABallTrajectory getTrajectory(final long timestampNow)
+	public boolean isChipped()
 	{
-		if (isChipped())
-		{
-			return new ChipBallTrajectory(timestampNow, pos, vel, spin);
-		}
-
-		long switchTimestamp = timestampNow;
-		if (getAcc().getLength2() > 1e-6 && vSwitchToRoll > 0.0)
-		{
-			switchTimestamp =
-					timestampNow + (long) (((vel.getLength2() - vSwitchToRoll) / getAcc().getLength2()) * 1e9);
-		}
-		return new StraightBallTrajectory(timestampNow, getPos(), getVel(), switchTimestamp);
+		return pos.z() > 0 || !SumatraMath.isZero(vel.z()) || !SumatraMath.isZero(acc.z());
+	}
+
+
+	/**
+	 * Is this ball rolling based on its spin and velocity?
+	 *
+	 * @param ballRadius in [mm]
+	 * @return
+	 */
+	public boolean isRolling(double ballRadius)
+	{
+		// compute relative velocity of ball to ground surface, if ball is rolling this is close to zero
+		IVector2 contactVelocity = vel.getXYVector().subtractNew(spin.multiplyNew(ballRadius));
+
+		return contactVelocity.getLength2() < 0.01;
 	}
 }

modules/moduli-wp/src/main/java/edu/tigers/sumatra/wp/ball/trajectory/ABallTrajectory.java → modules/common/src/main/java/edu/tigers/sumatra/ball/trajectory/ABallTrajectory.java

 /*
- * Copyright (c) 2009 - 2020, DHBW Mannheim - TIGERs Mannheim
+ * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
  */
 
-package edu.tigers.sumatra.wp.ball.trajectory;
+package edu.tigers.sumatra.ball.trajectory;
 
+import com.sleepycat.persist.model.Persistent;
+import edu.tigers.sumatra.ball.BallParameters;
 import edu.tigers.sumatra.math.line.v2.IHalfLine;
 import edu.tigers.sumatra.math.line.v2.ILineSegment;
 import edu.tigers.sumatra.math.line.v2.Lines;
-import edu.tigers.sumatra.math.vector.IVector;
 import edu.tigers.sumatra.math.vector.IVector2;
+import edu.tigers.sumatra.math.vector.IVector3;
+import edu.tigers.sumatra.math.vector.Vector2f;
+import edu.tigers.sumatra.math.vector.Vector3f;
+import lombok.Getter;
 
 import java.util.Collections;
 import java.util.List;
@@ -17,8 +22,48 @@ import java.util.List;
 /**
  * Common base implementation for ball trajectories.
  */
+@Persistent
 public abstract class ABallTrajectory implements IBallTrajectory
 {
+	/**
+	 * Parameters for this trajectory.
+	 */
+	@Getter
+	protected BallParameters parameters;
+
+
+	/**
+	 * Initial position in [mm].
+	 */
+	@Getter
+	protected IVector3 initialPos;
+
+
+	/**
+	 * Kick velocity in [mm/s].
+	 */
+	protected IVector3 initialVel;
+
+
+	/**
+	 * Kick spin in [rad/s].
+	 */
+	@Getter
+	protected IVector2 initialSpin;
+
+
+	/**
+	 * Create an empty default state. Required for {@link Persistent}.
+	 */
+	protected ABallTrajectory()
+	{
+		parameters = BallParameters.builder().build();
+		initialPos = Vector3f.ZERO_VECTOR;
+		initialVel = Vector3f.ZERO_VECTOR;
+		initialSpin = Vector2f.ZERO_VECTOR;
+	}
+
+
 	/**
 	 * Get the time when the ball comes to rest.
 	 *
@@ -49,39 +94,46 @@ public abstract class ABallTrajectory implements IBallTrajectory
 
 
 	@Override
-	public IVector getPosByTime(final double time)
+	public IVector3 getInitialVel()
+	{
+		return initialVel.multiplyNew(0.001);
+	}
+
+
+	@Override
+	public IVector3 getPosByTime(final double time)
 	{
-		return getMilliStateAtTime(getTKickToNow() + time).getPos();
+		return getMilliStateAtTime(time).getPos();
 	}
 
 
 	@Override
-	public IVector getVelByTime(final double time)
+	public IVector3 getVelByTime(final double time)
 	{
-		return getMilliStateAtTime(getTKickToNow() + time).getVel().multiplyNew(0.001);
+		return getMilliStateAtTime(time).getVel().multiplyNew(0.001);
 	}
 
 
 	@Override
-	public IVector getAccByTime(final double time)
+	public IVector3 getAccByTime(final double time)
 	{
-		return getMilliStateAtTime(getTKickToNow() + time).getAcc().multiplyNew(0.001);
+		return getMilliStateAtTime(time).getAcc().multiplyNew(0.001);
 	}
 
 
 	@Override
-	public double getSpinByTime(final double time)
+	public IVector2 getSpinByTime(final double time)
 	{
-		return getMilliStateAtTime(getTKickToNow() + time).getSpin();
+		return getMilliStateAtTime(time).getSpin();
 	}
 
 
 	@Override
-	public IVector getPosByVel(final double targetVelocity)
+	public IVector3 getPosByVel(final double targetVelocity)
 	{
 		if (getAbsVelByTime(0) < targetVelocity)
 		{
-			return getPosByTime(0);
+			return initialPos;
 		}
 
 		double time = getTimeByVel(targetVelocity);
@@ -92,15 +144,14 @@ public abstract class ABallTrajectory implements IBallTrajectory
 	@Override
 	public double getTimeByDist(final double travelDistance)
 	{
-		double distToNow = getKickPos().getXYVector().distanceTo(getPosByTime(0).getXYVector());
-		return getTimeByDistanceInMillimeters(travelDistance + distToNow) - getTKickToNow();
+		return getTimeByDistanceInMillimeters(travelDistance);
 	}
 
 
 	@Override
 	public double getTimeByVel(final double targetVelocity)
 	{
-		return getTimeByVelocityInMillimetersPerSec(targetVelocity * 1000.0) - getTKickToNow();
+		return getTimeByVelocityInMillimetersPerSec(targetVelocity * 1000.0);
 	}
 
 
@@ -115,58 +166,58 @@ public abstract class ABallTrajectory implements IBallTrajectory
 	@Override
 	public double getAbsVelByPos(final IVector2 targetPosition)
 	{
-		return getAbsVelByDist(getPosByTime(0).getXYVector().distanceTo(targetPosition));
+		return getAbsVelByDist(initialPos.getXYVector().distanceTo(targetPosition));
 	}
 
 
 	@Override
 	public double getTimeByPos(final IVector2 targetPosition)
 	{
-		return getTimeByDist(getPosByTime(0).getXYVector().distanceTo(targetPosition));
+		return getTimeByDist(initialPos.getXYVector().distanceTo(targetPosition));
 	}
 
 
 	@Override
 	public double getDistByTime(final double time)
 	{
-		return getPosByTime(0).getXYVector().distanceTo(getPosByTime(time).getXYVector());
+		return initialPos.getXYVector().distanceTo(getPosByTime(time).getXYVector());
 	}
 
 
 	@Override
 	public double getAbsVelByTime(final double time)
 	{
-		return getMilliStateAtTime(getTKickToNow() + time).getVel().getLength2() * 0.001;
+		return getMilliStateAtTime(time).getVel().getLength2() * 0.001;
 	}
 
 
 	@Override
 	public boolean isInterceptableByTime(final double time)
 	{
-		return getPosByTime(time).getXYZVector().z() < 150;
+		return getPosByTime(time).getXYZVector().z() < parameters.getMaxInterceptableHeight();
 	}
 
 
 	@Override
 	public boolean isRollingByTime(final double time)
 	{
-		return getPosByTime(time).getXYZVector().z() < 10;
+		return getPosByTime(time).getXYZVector().z() < parameters.getMinHopHeight();
 	}
 
 
 	@Override
 	public IHalfLine getTravelLine()
 	{
-		IVector2 finalPos = getPosByTime(getTimeAtRest() - getTKickToNow()).getXYVector();
-		return Lines.halfLineFromPoints(getPosByTime(0).getXYVector(), finalPos);
+		IVector2 finalPos = getPosByTime(getTimeAtRest()).getXYVector();
+		return Lines.halfLineFromPoints(initialPos.getXYVector(), finalPos);
 	}
 
 
 	@Override
 	public ILineSegment getTravelLineSegment()
 	{
-		IVector2 finalPos = getPosByTime(getTimeAtRest() - getTKickToNow()).getXYVector();
-		return Lines.segmentFromPoints(getPosByTime(0).getXYVector(), finalPos);
+		IVector2 finalPos = getPosByTime(getTimeAtRest()).getXYVector();
+		return Lines.segmentFromPoints(initialPos.getXYVector(), finalPos);
 	}
 
 

modules/common/src/main/java/edu/tigers/sumatra/ball/trajectory/BallFactory.java

+/*
+ * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
+ */
+
+package edu.tigers.sumatra.ball.trajectory;
+
+import edu.tigers.sumatra.ball.BallParameters;
+import edu.tigers.sumatra.ball.BallState;
+import edu.tigers.sumatra.ball.trajectory.chipped.ChipBallConsultant;
+import edu.tigers.sumatra.ball.trajectory.chipped.ChipBallTrajectory;
+import edu.tigers.sumatra.ball.trajectory.flat.FlatBallConsultant;
+import edu.tigers.sumatra.ball.trajectory.flat.FlatBallTrajectory;
+import edu.tigers.sumatra.math.vector.IVector2;
+import edu.tigers.sumatra.math.vector.IVector3;
+import edu.tigers.sumatra.math.vector.Vector2f;
+import lombok.Getter;
+import lombok.RequiredArgsConstructor;
+
+
+/**
+ * Factory class for creating classes for the configured ball models.
+ */
+@RequiredArgsConstructor
+public final class BallFactory
+{
+	@Getter
+	private final BallParameters ballParams;
+
+
+	public IBallTrajectory createTrajectoryFromState(final BallState state)
+	{
+		if (state.isChipped())
+		{
+			return ChipBallTrajectory.fromState(ballParams, state.getPos(), state.getVel(), state.getSpin());
+		}
+
+		return FlatBallTrajectory
+				.fromState(ballParams, state.getPos().getXYVector(), state.getVel().getXYVector(), state.getSpin());
+	}
+
+
+	public IBallTrajectory createTrajectoryFromBallAtRest(final IVector2 pos)
+	{
+		return FlatBallTrajectory.fromKick(ballParams, pos, Vector2f.ZERO_VECTOR, Vector2f.ZERO_VECTOR);
+	}
+
+
+	public IBallTrajectory createTrajectoryFromRollingBall(final IVector2 pos, final IVector2 vel)
+	{
+		IVector2 spin = vel.multiplyNew(1.0 / ballParams.getBallRadius());
+
+		return FlatBallTrajectory.fromKick(ballParams, pos, vel, spin);
+	}
+
+
+	public IBallTrajectory createTrajectoryFromKickedBallWithoutSpin(final IVector2 pos, final IVector3 vel)
+	{
+		if (vel.z() > 0)
+		{
+			return ChipBallTrajectory.fromKick(ballParams, pos, vel, Vector2f.ZERO_VECTOR);
+		}
+
+		return FlatBallTrajectory.fromKick(ballParams, pos, vel.getXYVector(), Vector2f.ZERO_VECTOR);
+	}
+
+
+	public IBallTrajectory createTrajectoryFromKickedBall(final IVector2 pos, final IVector3 vel, final IVector2 spin)
+	{
+		if (vel.z() > 0)
+		{
+			return ChipBallTrajectory.fromKick(ballParams, pos, vel, spin);
+		}
+
+		return FlatBallTrajectory.fromKick(ballParams, pos, vel.getXYVector(), spin);
+	}
+
+
+	/**
+	 * Create a consultant for straight kicks with the default configured implementation
+	 *
+	 * @return a new ball consultant for straight kicks
+	 */
+	public IFlatBallConsultant createFlatConsultant()
+	{
+		return new FlatBallConsultant(ballParams);
+	}
+
+
+	/**
+	 * Create a consultant for chip kicks with the default configured implementation
+	 *
+	 * @return a new ball consultant for chip kicks
+	 */
+	public IChipBallConsultant createChipConsultant()
+	{
+		return new ChipBallConsultant(ballParams);
+	}
+}

modules/moduli-wp/src/main/java/edu/tigers/sumatra/wp/ball/trajectory/IBallConsultant.java → modules/common/src/main/java/edu/tigers/sumatra/ball/trajectory/IBallConsultant.java

 /*
- * Copyright (c) 2009 - 2020, DHBW Mannheim - TIGERs Mannheim
+ * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
  */
 
-package edu.tigers.sumatra.wp.ball.trajectory;
+package edu.tigers.sumatra.ball.trajectory;
 
 /**
  * Consultant for chipped balls.

modules/moduli-wp/src/main/java/edu/tigers/sumatra/wp/ball/trajectory/IBallTrajectory.java → modules/common/src/main/java/edu/tigers/sumatra/ball/trajectory/IBallTrajectory.java

 /*
- * Copyright (c) 2009 - 2020, DHBW Mannheim - TIGERs Mannheim
+ * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
  */
 
-package edu.tigers.sumatra.wp.ball.trajectory;
+package edu.tigers.sumatra.ball.trajectory;
 
+import edu.tigers.sumatra.ball.BallState;
 import edu.tigers.sumatra.math.IMirrorable;
 import edu.tigers.sumatra.math.line.v2.IHalfLine;
 import edu.tigers.sumatra.math.line.v2.ILineSegment;
-import edu.tigers.sumatra.math.vector.IVector;
 import edu.tigers.sumatra.math.vector.IVector2;
+import edu.tigers.sumatra.math.vector.IVector3;
 import edu.tigers.sumatra.planarcurve.IPlanarCurveProvider;
-import edu.tigers.sumatra.vision.data.BallTrajectoryState;
 
 import java.util.List;
 
@@ -20,40 +20,70 @@ import java.util.List;
  */
 public interface IBallTrajectory extends IMirrorable<IBallTrajectory>, IPlanarCurveProvider
 {
+	/**
+	 * Get initial position in [mm].
+	 *
+	 * @return
+	 */
+	IVector3 getInitialPos();
+
+	/**
+	 * Get initial velocity in [m/s].
+	 *
+	 * @return
+	 */
+	IVector3 getInitialVel();
+
+	/**
+	 * Get initial spin in [rad/s].
+	 *
+	 * @return
+	 */
+	IVector2 getInitialSpin();
+
+	/**
+	 * Get the state at the specified time in [s] after initial state in milli-units.
+	 *
+	 * @param time in [s]
+	 * @return
+	 */
+	BallState getMilliStateAtTime(final double time);
+
+
 	/**
 	 * Get the position for a given time