Commit 4e5e5d40 authored by MarkG's avatar MarkG Committed by TIGERs GitLab
Browse files

Resolve "Moving robot horizon is wrong"

Closes #1729

See merge request main/Sumatra!1499

sumatra-commit: c7b911f550de53602f44253560e77991567814ae
parent bba9e226
Pipeline #17580 passed with stage
in 5 minutes and 33 seconds
......@@ -5,6 +5,7 @@ plugins {
dependencies {
// Support old junit4 syntax for now
testImplementation 'junit:junit:4.13.2'
permitTestUnusedDeclared 'junit:junit:4.13.2'
// Support junit 5
testImplementation 'org.junit.jupiter:junit-jupiter:5.8.2'
permitTestUnusedDeclared 'org.junit.jupiter:junit-jupiter:5.8.2'
......
......@@ -12,7 +12,7 @@ plugins {
dependencies {
implementation 'com.github.g3force:String2ValueConverter:2.2.1'
implementation 'org.apache.commons:commons-math3:3.6.1'
api 'org.apache.commons:commons-math3:3.6.1'
implementation 'commons-lang:commons-lang:2.6'
implementation 'org.apache.logging.log4j:log4j-api:2.17.1'
......
......@@ -25,15 +25,15 @@ public abstract class ACircle extends ACircular implements ICircle
{
return CircleMath.isPointInCircle(this, point, margin);
}
@Override
public List<IVector2> tangentialIntersections(final IVector2 externalPoint)
{
return CircleMath.tangentialIntersections(this, externalPoint);
}
@Override
public IEllipse projectToGround(final IVector3 origin, final double height)
{
......@@ -41,15 +41,22 @@ public abstract class ACircle extends ACircular implements ICircle
{
throw new IllegalArgumentException("origin.z() must be above height");
}
IVector2 newCenter = Vector3.from2d(center(), height).projectToGroundNew(origin);
double dist = center().distanceTo(origin.getXYVector());
IVector2 projected = Vector3.fromXYZ(dist + radius(), radius(), height)
.projectToGroundNew(Vector3.fromXYZ(0, 0, origin.z()));
return Ellipse.createTurned(newCenter, projected.x() - dist, projected.y(),
origin.getXYVector().subtractNew(newCenter).getAngle());
}
@Override
public double distanceTo(IVector2 point)
{
return center().distanceTo(point) - radius();
}
}
......@@ -4,17 +4,18 @@
package edu.tigers.sumatra.math.circle;
import java.util.List;
import edu.tigers.sumatra.math.ellipse.IEllipse;
import edu.tigers.sumatra.math.vector.IEuclideanDistance;
import edu.tigers.sumatra.math.vector.IVector2;
import edu.tigers.sumatra.math.vector.IVector3;
import java.util.List;
/**
* Circle interface.
*/
public interface ICircle extends ICircular
public interface ICircle extends ICircular, IEuclideanDistance
{
/**
* Get the intersection points of the two tangential lines that cross the external points.
......@@ -23,14 +24,17 @@ public interface ICircle extends ICircular
* @return two tangential intersections
*/
List<IVector2> tangentialIntersections(final IVector2 externalPoint);
/**
* Project <i>this</i> to ground assuming it is at <i>height</i> height and the projection origin is <i>origin</i>.
*
*
* @param origin Projection origin
* @param height Height of the circle before projection.
* @return Projected circle => ellipse
*/
IEllipse projectToGround(final IVector3 origin, final double height);
@Override
ICircle withMargin(final double margin);
}
......@@ -10,6 +10,7 @@ import edu.tigers.sumatra.math.vector.Vector2f;
import org.apache.commons.lang.Validate;
import java.util.function.Function;
import java.util.function.UnaryOperator;
public final class BangBangTrajectoryFactory
......@@ -34,7 +35,7 @@ public final class BangBangTrajectoryFactory
final var startToTarget = s1.subtractNew(s0).turn(-rotation);
final var v0Rotated = v0.turnNew(-rotation);
final Function<Float, Float> alphaFn = alpha -> alpha + (((float) AngleMath.PI_HALF - alpha) * 0.5f);
final UnaryOperator<Float> alphaFn = alpha -> alpha + (((float) AngleMath.PI_HALF - alpha) * 0.5f);
BangBangTrajectory2D child = new BangBangTrajectory2D().generate(
Vector2f.ZERO_VECTOR,
startToTarget,
......@@ -66,6 +67,18 @@ public final class BangBangTrajectoryFactory
}
public ITrajectory<Double> single(
final double initialPos,
final double finalPos,
final double initialVel,
final double maxVel,
final double maxAcc
)
{
return singleDim(initialPos, finalPos, initialVel, maxVel, maxAcc);
}
public BangBangTrajectory1D singleDim(
final double initialPos,
final double finalPos,
......
/*
* *********************************************************
* Copyright (c) 2009 - 2014, DHBW Mannheim - Tigers Mannheim
* Project: TIGERS - Sumatra
* Date: Oct 31, 2014
* Author(s): Nicolai Ommer <nicolai.ommer@gmail.com>
* *********************************************************
*/
package edu.tigers.sumatra.trajectory;
import edu.tigers.sumatra.math.vector.IVector2;
import edu.tigers.sumatra.math.vector.IVector3;
import edu.tigers.sumatra.math.vector.VectorMath;
/**
* Util class for splines
*
* @author Nicolai Ommer <nicolai.ommer@gmail.com>
*/
public final class TrajectoryMath
{
private static final double DT_PRECISE = 0.01;
private TrajectoryMath()
{
}
/**
* for the defense points, LOW PRECISION, BAD PERFORMANCE
*
* @param spline
* @param drivenWay [mm]
* @return
*/
public static double timeAfterDrivenWay(final ITrajectory<IVector2> spline, final double drivenWay)
{
return timeAfterDrivenWay(spline, drivenWay, DT_PRECISE);
}
/**
* @param spline
* @param drivenWay [mm]
* @param dt [s]
* @return
*/
public static double timeAfterDrivenWay(final ITrajectory<IVector2> spline, final double drivenWay, final double dt)
{
double length = 0;
double drivenWayMeters = drivenWay / 1000;
for (double t = 0.0; t < spline.getTotalTime(); t += dt)
{
length += spline.getVelocity(t).getLength2() * dt;
if (length >= drivenWayMeters)
{
return t;
}
}
return spline.getTotalTime();
}
/**
* Calculate the length of the spline by integration
*
* @param spline
* @return distance [mm]
*/
public static double length(final ITrajectory<IVector3> spline)
{
return length(spline, DT_PRECISE);
}
/**
* Calculate the length of the spline by integration
*
* @param spline
* @param dt smaller->more precision, less performance; e.g. 0.01
* @return distance [mm]
*/
public static double length(final ITrajectory<IVector3> spline, final double dt)
{
double length = 0;
for (double t = 0.0; t < spline.getTotalTime(); t += dt)
{
length += spline.getVelocity(t).getLength2() * dt;
}
return length * 1000;
}
/**
* Find the time where p is nearest to the spline
*
* @param spline
* @param p
* @return
*/
public static double timeNearest2Point(final ITrajectory<IVector3> spline, final IVector2 p)
{
return timeNearest2Point(spline, p, 0.1);
}
/**
* Find the time where p is nearest to the spline
* Take care of the size of dt!
*
* @param spline
* @param p
* @param dt
* @return
*/
public static double timeNearest2Point(final ITrajectory<IVector3> spline, final IVector2 p, final double dt)
{
double nearestDist = Double.MAX_VALUE;
double time = 0;
assert Double.isFinite(spline.getTotalTime());
for (double t = 0; t < spline.getTotalTime(); t += dt)
{
IVector2 p2 = spline.getPositionMM(t).getXYVector();
double dist = VectorMath.distancePP(p, p2);
if (dist < nearestDist)
{
nearestDist = dist;
time = t;
}
}
return time;
}
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment