Commit 7f4f3966 authored by NicolaiO's avatar NicolaiO 🐼 Committed by TIGERs GitLab
Browse files

Resolve "Keeper should "overshoot" the ball intersection line. To catch a very...

Resolve "Keeper should "overshoot" the ball intersection line. To catch a very fast ball that would otherwise be missed"

Closes #1474

See merge request main/Sumatra!1528

sumatra-commit: 686f94071ab5a655c70b6965dca4f1b999bb48c3
parent 448d9b2c
Pipeline #17848 passed with stage
in 12 minutes and 24 seconds
......@@ -5,6 +5,7 @@
plugins {
id 'sumatra.java'
id 'sumatra.test'
id 'sumatra.test.spock'
id 'java-library'
}
......
......@@ -15,7 +15,9 @@ import java.util.function.UnaryOperator;
public final class BangBangTrajectoryFactory
{
private static final double MAX_VEL_TOLERANCE = 0.2;
private static final float SYNC_ACCURACY = 1e-3f;
static final float SYNC_ACCURACY = 1e-3f;
static final UnaryOperator<Float> ALPHA_FN_ASYNC = alpha -> alpha + (((float) AngleMath.PI_HALF - alpha)
* 0.5f);
public BangBangTrajectory2DAsync async(
......@@ -34,7 +36,6 @@ public final class BangBangTrajectoryFactory
final var startToTarget = s1.subtractNew(s0).turn(-rotation);
final var v0Rotated = v0.turnNew(-rotation);
final UnaryOperator<Float> alphaFn = alpha -> alpha + (((float) AngleMath.PI_HALF - alpha) * 0.5f);
BangBangTrajectory2D child = new BangBangTrajectory2D().generate(
Vector2f.ZERO_VECTOR,
startToTarget,
......@@ -42,7 +43,8 @@ public final class BangBangTrajectoryFactory
(float) vmax,
(float) acc,
SYNC_ACCURACY,
alphaFn);
ALPHA_FN_ASYNC
);
return new BangBangTrajectory2DAsync(child, s0, rotation);
}
......@@ -62,7 +64,8 @@ public final class BangBangTrajectoryFactory
(float) vmax,
(float) acc,
SYNC_ACCURACY,
f -> f);
f -> f
);
}
......
/*
* Copyright (c) 2009 - 2020, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.sumatra.trajectory;
import edu.tigers.sumatra.math.vector.IVector2;
import edu.tigers.sumatra.math.vector.Vector2;
import edu.tigers.sumatra.planarcurve.PlanarCurve;
import edu.tigers.sumatra.planarcurve.PlanarCurveSegment;
import static edu.tigers.sumatra.math.SumatraMath.sqrt;
import static java.lang.Math.max;
import static java.lang.Math.pow;
/**
* Util class for BangBangTrajectories
*/
public final class BangBangTrajectoryMath
{
private static final BangBangTrajectoryFactory TRAJECTORY_FACTORY = new BangBangTrajectoryFactory();
private static final PlanarCurveFactory PLANAR_CURVE_FACTORY = new PlanarCurveFactory();
private BangBangTrajectoryMath()
{
}
/**
* Calculate brake distance as 2-dimensional vector.
*
* @param initialVel
* @param brkMax
* @return Vector2 containing brake distance
*/
public static IVector2 brakeDistanceVector(final IVector2 initialVel, final double brkMax)
{
double absSpeed = initialVel.getLength();
double distance = 0.5 * (1.0 / brkMax) * absSpeed * absSpeed;
Vector2 direction = initialVel.normalizeNew();
return direction.multiply(distance);
}
/**
* Calculate shortest possible break distance
*
* @param initialVel
* @param brkMax
* @return shortest possible break distance as double
*/
public static double brakeDistance(final IVector2 initialVel, final double brkMax)
{
return brakeDistanceVector(initialVel, brkMax).getLength();
}
/**
* Create new BangBangTrajectory2D to brake as fast as possible
*
* @param curPos
* @param initialVel
* @param brkMax
* @return new BangBangTrajectory2D that can be applied to bot in order to brake as fast as possible
*/
public static BangBangTrajectory2D createShortestBrakeTrajectory(final IVector2 curPos, final IVector2 initialVel,
final double brkMax)
{
IVector2 brakeVector = brakeDistanceVector(initialVel, brkMax);
return TRAJECTORY_FACTORY
.sync(curPos, curPos.addNew(brakeVector), initialVel, initialVel.getLength(), brkMax);
}
/**
* Calculate the time to brake as max brake Acceleration
*
* @param initialVel
* @param brkMax
* @return
*/
public static double timeToBrake(final IVector2 initialVel, final double brkMax)
{
return sqrt((2.0 * Math.abs(brakeDistance(initialVel, brkMax))) / Math.abs(brkMax));
}
/**
* Calculate distance to achieve maximal velocity given. vmax is parallel to initial vel.
*
* @param initialVel
* @param maxVel
* @param maxAcc
* @return distance
*/
public static double distanceToAchieveMaxVelocity(final IVector2 initialVel, final double maxVel,
final double maxAcc)
{
return distanceToAchieveMaxVelocity(initialVel.getLength(), maxVel, maxAcc);
}
private static double distanceToAchieveMaxVelocity(final double initialVel, final double maxVel,
final double maxAcc)
{
double t1 = (maxVel - initialVel) / maxAcc; // acceleration phase
double tb = maxVel / maxAcc; // brake phase
double s1 = (initialVel * t1) + (0.5 * maxAcc * t1 * t1); // distance in acceleration phase
double sb = (maxVel * tb) - (0.5 * maxAcc * tb * tb); // distance in brake phase
return s1 + sb;
}
/**
* Calculate distance to achieve maximal velocity given. vmax is parallel to initial vel.
*
* @param initialVel
* @param maxVel
* @param maxAcc
* @return distance as vector
*/
public static IVector2 distanceVectorToAchieveMaxVelocity(final IVector2 initialVel, final double maxVel,
final double maxAcc)
{
IVector2 direction = initialVel.normalizeNew();
return direction.multiplyNew(distanceToAchieveMaxVelocity(initialVel, maxVel, maxAcc));
}
/**
* Analytically look for the maximum velocity in trajectory
*
* @param traj
* @return vmax
*/
public static double maxVelocityOfTrajectory(final BangBangTrajectory2D traj)
{
double vmax = 0;
PlanarCurve curve = PLANAR_CURVE_FACTORY.getPlanarCurve(traj);
for (PlanarCurveSegment seg : curve.getSegments())
{
double vel = seg.getVel().getLength2();
if (vel > vmax)
{
vmax = vel;
}
}
return vmax * 1e-3;
}
/**
* Get virtual (not necessarily reachable) destination to generate trajectory in order to reach given point at given
* time. Target velocity does not have to be zero
*
* @param curPos
* @param initialVel
* @param desiredPos
* @param maxAcc
* @param maxVel
* @param time
* @param tolerance
* @return
*/
public static IVector2 getVirtualDestinationToReachPositionInTime(final IVector2 curPos, final IVector2 initialVel,
final IVector2 desiredPos, final double maxAcc, final double maxVel, final double time,
final double tolerance)
{
ITrajectory<IVector2> direct = TRAJECTORY_FACTORY
.sync(curPos.multiplyNew(1e-3), desiredPos.multiplyNew(1e-3),
initialVel, maxVel, maxAcc);
IVector2 delta = desiredPos.subtractNew(curPos);
if (direct.getPositionMM(time).isCloseTo(desiredPos, tolerance)
|| ((time * initialVel.getLength2() * 1e3) > delta.getLength2()) || (time < 0))
{
return desiredPos;
}
double projectedVel = initialVel.scalarProduct(delta.multiplyNew(1e-3)) / delta.multiplyNew(1e-3).getLength2();
double timeToMaxSpeed = (maxVel - projectedVel) / maxAcc; // time to accelerate to maximum speed
double uniformVelocityTime = max(time - timeToMaxSpeed, 0);
double accelerationTime = Math.min(timeToMaxSpeed, time);
double longestPossibleDistance = 1000
* ((0.5 * maxAcc * accelerationTime * accelerationTime) + (maxVel * uniformVelocityTime));
if (longestPossibleDistance < delta.getLength())
{
// it won't be possible to reach target in given time slot. Desperate maxvel destination will be given.
return curPos.addNew(delta.normalizeNew().multiplyNew(delta.getLength2() * 10));
}
double accelerationPhaseTime = ((sqrt(pow(maxVel, 2) +
(((-2 * projectedVel) - (2 * maxAcc * time)) * maxVel)
+ pow(projectedVel, 2) + (2 * maxAcc * 1e-3 * delta.getLength2()))
- maxVel) + projectedVel) / maxAcc;
double staticVelocityPhaseTime = time - accelerationPhaseTime;
double breakPhaseTime = maxVel / maxAcc;
double virtualDistance = ((projectedVel * accelerationPhaseTime) + (0.5 * maxAcc * pow(accelerationPhaseTime, 2))
+ (maxVel * (staticVelocityPhaseTime + breakPhaseTime))) - (0.5 * maxAcc * pow(breakPhaseTime, 2));
virtualDistance *= 1e3;
return delta.scaleToNew(virtualDistance).addNew(curPos);
}
}
\ No newline at end of file
/*
* Copyright (c) 2009 - 2022, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.sumatra.trajectory;
import edu.tigers.sumatra.math.AngleMath;
import edu.tigers.sumatra.math.SumatraMath;
import edu.tigers.sumatra.math.vector.IVector2;
import edu.tigers.sumatra.math.vector.Vector2;
import edu.tigers.sumatra.math.vector.Vector2f;
import net.jafama.DoubleWrapper;
import net.jafama.FastMath;
import java.util.Optional;
import java.util.function.UnaryOperator;
/**
* Generate virtual destinations such that a position will be reached in time
* Might also be referred as Overshooting
*/
public class DestinationForTimedPositionCalc
{
/**
* @param s0 [m] start position
* @param s1 [m] target position
* @param v0 [m/s] initial velocity
* @param vMax [m/s] maximum velocity
* @param aMax [m/s²] maximum acceleration
* @param targetTime [s] target time
* @return [m] virtual destination
*/
public IVector2 destinationForBangBang2dSync(
IVector2 s0,
IVector2 s1,
IVector2 v0,
double vMax,
double aMax,
double targetTime
)
{
return destinationForBangBang2D(
s0,
s1,
v0,
(float) vMax,
(float) aMax,
(float) targetTime,
alpha -> alpha
);
}
/**
* @param s0 [m] start position
* @param s1 [m] target position
* @param v0 [m/s] initial velocity
* @param vMax [m/s] maximum velocity
* @param aMax [m/s²] maximum acceleration
* @param targetTime [s] target time
* @param primaryDirection Primary Moving Direction
* @return [m] virtual destination
*/
public IVector2 destinationForBangBang2dAsync(
IVector2 s0,
IVector2 s1,
IVector2 v0,
double vMax,
double aMax,
double targetTime,
IVector2 primaryDirection
)
{
var rotation = primaryDirection.getAngle();
var startToTarget = s1.subtractNew(s0).turn(-rotation);
var v0Rotated = v0.turnNew(-rotation);
return destinationForBangBang2D(
Vector2f.ZERO_VECTOR,
startToTarget,
v0Rotated,
(float) vMax,
(float) aMax,
(float) targetTime,
BangBangTrajectoryFactory.ALPHA_FN_ASYNC
).turnNew(rotation).add(s0);
}
IVector2 destinationForBangBang2D(
IVector2 s0,
IVector2 s1,
IVector2 v0,
float vMax,
float aMax,
float targetTime,
UnaryOperator<Float> alphaFn
)
{
var v0x = (float) v0.x();
var v0y = (float) v0.y();
var distance = s1.subtractNew(s0);
var distanceX = (float) distance.x();
var distanceY = (float) distance.y();
float inc = (float) AngleMath.PI / 8.0f;
float alpha = (float) AngleMath.PI / 4.0f;
TimedPos1D x = new TimedPos1D(0, 0);
TimedPos1D y = new TimedPos1D(0, 0);
// binary search, some iterations (fixed)
while (inc > 1e-7)
{
DoubleWrapper cos = new DoubleWrapper();
final float sA = (float) FastMath.sinAndCos(alphaFn.apply(alpha), cos);
final float cA = (float) cos.value;
x = getTimedPos1D(distanceX, v0x, vMax * cA, aMax * cA, targetTime);
y = getTimedPos1D(distanceY, v0y, vMax * sA, aMax * sA, targetTime);
double diff = Math.abs(x.time() - y.time());
if (diff < BangBangTrajectoryFactory.SYNC_ACCURACY)
{
break;
}
if (x.time() > y.time())
{
alpha -= inc;
} else
{
alpha += inc;
}
inc *= 0.5f;
}
return Vector2.fromXY(x.pos() + s0.x(), y.pos() + s0.y());
}
/**
* @param s TargetPosition - StartPosition
* @param v0 initialVel
* @param vMax maximal absolute velocity
* @param aMax maximal absolute acceleration
* @param tt TargetTime
* @return
*/
TimedPos1D getTimedPos1D(float s, float v0, float vMax, float aMax, float tt)
{
// Hit Windows:
// Either our v0 is low enough that we could stop before reaching the goal target:
//
// | |----------------------------------------------------
// | Direct Hit
// -|---------------------------------------------------------> tt
//
// Or our v0 is so high that we will always overshoot. Is target time low enough to direct hit or do we
// need to overshoot and recover?
//
// | |----------| |------------------------
// | Direct Hit Overshoot and Recover
// -|---------------------------------------------------------> tt
var aDec = v0 >= 0 ? -aMax : aMax;
var sZeroVel = 0.5f * v0 * (-v0 / aDec);
var v1Max = s >= 0 ? vMax : -vMax;
if ((s >= 0.f) != (v0 > 0.f) // If v0 and s0 -> sT are in a different direction -> no forced overshoot
// abs(sZeroVel) > abs(s), without this breaking is always possible -> no forced overshoot
|| (s >= 0) == (sZeroVel < s)
// Determine if we can be slow enough -> no forced overshoot
|| calcSlowestDirectTime(s, v0, aMax) >= tt
)
{
// We can directly hit the timed target position
return calcFastestDirect(s, v0, v1Max, aMax, tt);
} else
{
// Calculate necessary time to break to zero
var tBreaking = Math.abs(v0 / aMax);
// Calc the fastest overshoot by starting at sZeroVel in opposed direction with v0=0.0
var timed = calcFastestDirect(s - sZeroVel, 0.f, -v1Max, aMax, tt - tBreaking);
// Extend TimedPos1D to accommodate breaking
return new TimedPos1D(timed.pos() + sZeroVel, timed.time() + tBreaking);
}
}
private float calcSlowestDirectTime(
float s,
float v0,
float aMax
)
{
var aDec = (v0 >= 0) ? -aMax : aMax;
var sqrt = (float) SumatraMath.sqrt(v0 * v0 + 2 * aDec * (s));
return (v0 >= 0.f) ? ((-v0 + sqrt) / aDec) : ((-v0 - sqrt) / aDec);
}
/**
* @param s TargetPos - InitialPos
* @param v0 initialVel
* @param v1Max maximal velocity but with the same sign (direction) as s
* @param aMax maximal acceleration
* @param tt TargetTime
* @return
*/
private TimedPos1D calcFastestDirect(
float s,
float v0,
float v1Max,
float aMax,
float tt
)
{
// Possible Fastest Directs:
// - Straight too slow
// - Trapezoidal too slow
// - Trapezoidal finishing early
// - Trapezoidal direct hit
// - Triangular too slow
// - Triangular finishing early
// - Triangular direct hit
var aDec = v1Max >= 0 ? -aMax : aMax;
var trapezoidal = calcFastestDirectTrapezoidal(s, v0, v1Max, aMax, aDec, tt);
return trapezoidal.orElseGet(() -> calcFastestDirectTriangular(s, v0, v1Max, aMax, aDec, tt));
}
private Optional<TimedPos1D> calcFastestDirectTrapezoidal(
float s,
float v0,
float v1Max,
float aMax,
float aDec,
float tt
)
{
// Full acceleration for s01 to reach v1Max
var aAcc = v0 >= v1Max ? -aMax : aMax;
var t01 = (v1Max - v0) / aAcc;
var s01 = 0.5f * (v1Max + v0) * t01;
if ((s >= 0.0f) == (s <= s01))
{
// We are not able to accel to v1Max before reaching s -> No Trapezoidal form possible
return Optional.empty();
}
var s13 = s - s01;
var t23 = -v1Max / aDec;
var s23 = 0.5f * v1Max * t23;
// Determining if "Trapezoidal too slow"
// v1Max,v2 _________
// /| | |\
// / | | | \ s reached at t=t2
// / | | | \
// / | | | \
// v0,v3 / | | | \
// ---|-----|---|---|-----|----------->
// t0 t1 tt t2 t3
// |-t01-|--t12--|-t23-|
var t12TooSlow = (s13) / v1Max;
if (t01 + t12TooSlow >= tt)
{
return Optional.of(new TimedPos1D(s + s23, t01 + t12TooSlow + t23));
}
// Determine if "Trapezoidal finishing early"
// v1Max,v2 _________
// /| |\
// / | | \ s reached at t=t3
// / | | \
// / | | \
// v0,v3 / | | \
// ---|-----|-------|-----|-----|----->
// t0 t1 t2 t3 tt
// |-t01-|--t12--|-t23-|
var s12Early = s13 - s23;
var t12Early = s12Early / v1Max;
if (t12Early >= 0.0f && t01 + t12Early + t23 <= tt)
{
return Optional.of(new TimedPos1D(s, t01 + t12Early + t23));
}
// Determine if "Trapezoidal direct hit"
// v1Max,v2 _________
// /| |\
// / | | \ tt = t3
// / | | \ s reached at t=tt
// v3 / | | \
// / | | |\
// v0,v4 / | | | \
// ---|------|-------|---|--|--------->
// t0 t1 t2 tt t4
// |-t01--|--t12--|t23|
// |----t13----|
// https://www.wolframalpha.com/input?i=solve+v_0*t_1+%3Dv_0*t_2%2B1%2F2*a*Power%5Bt_2%2C2%5D%2Bv_1*t_3%2C+v_1+%3D+v_0%2Ba*t_2%2C+t_1%2Bt%3Dt_2%2Bt_3+for+v_1%2Ct_1%2C++t_2
var t13 = tt - t01;
var t23Direct = (float) SumatraMath.sqrt(2 * (s13 - t13 * v1Max) / aDec);
var t12Direct = t13 - t23Direct;
if (t12Direct > 0 && t23Direct < t23)
{
var v3 = v1Max + aDec * t23Direct;
var t34 = -v3 / aDec;