Commit 22c8f424 authored by NicolaiO's avatar NicolaiO 🐼 Committed by TIGERs GitLab
Browse files

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

Closes #1633

See merge request main/Sumatra!1377

sumatra-commit: 759d3385863bb04be2acbe3609af0257bf29ede8
parent 10715796
Pipeline #16656 failed with stage
in 4 minutes and 8 seconds
/*
* 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;
}
}
......@@ -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;
}
}
/*
* 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);
}
......
/*
* 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);
}
}
/*
* 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.
......
/*
* 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