FairportRobotics
/
RoboLib
mirror of https://github.com/FairportRobotics/RoboLib.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

src/robolib/trajectory/Velocity3dTest: Defined unit 

tests for Velocity3d

Also, implement fixes for bugs found in test bringup
This commit is contained in:
Ray Healy 2026-02-14 08:36:57 -05:00
parent b7ee757181
commit 6dfea76969
2 changed files with 127 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/Velocity3d.java
View File

@ -1,15 +1,13 @@
package org.fairportrobotics.frc.robolib.trajectory;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.LinearVelocity;
import edu.wpi.first.units.measure.Time;
public class Velocity3d {
private final LinearVelocity vX;
private final LinearVelocity vY;
private final LinearVelocity vZ;
private final LinearVelocity xVelocity;
private final LinearVelocity yVelocity;
private final LinearVelocity zVelocity;
private Double cache_squareMagnitudeVelocity;
private LinearVelocity cache_magnitudeVelocity;
@ -17,10 +15,10 @@ public class Velocity3d {
private Angle cache_azimuthAngle;
private LinearVelocity cache_horizontalVelocity;
public Velocity3d(LinearVelocity vX, LinearVelocity vY, LinearVelocity vZ) {
this.vX = vX.copy();
this.vY = vY.copy();
this.vZ = vZ.copy();
public Velocity3d(LinearVelocity xVelocity, LinearVelocity yVelocity, LinearVelocity zVelocity) {
this.xVelocity = xVelocity.copy();
this.yVelocity = yVelocity.copy();
this.zVelocity = zVelocity.copy();
this.cache_squareMagnitudeVelocity = null;
this.cache_magnitudeVelocity = null;
@ -69,7 +67,7 @@ public class Velocity3d {
* @return The x-axis component of the velocity
*/
public LinearVelocity getXVelocity() {
return vX;
return xVelocity;
}
@ -77,7 +75,7 @@ public class Velocity3d {
* @return The y-axis component of the velocity
*/
public LinearVelocity getYVelocity() {
return vY;
return yVelocity;
}
@ -85,7 +83,7 @@ public class Velocity3d {
* @return The z-axis component of the velocity
*/
public LinearVelocity getZVelocity() {
return vZ;
return zVelocity;
}
@ -94,9 +92,9 @@ public class Velocity3d {
*/
public double getSquareMagnitude() {
if(this.cache_squareMagnitudeVelocity == null) {
double vX_mps = this.vX.in(Units.MetersPerSecond);
double vY_mps = this.vY.in(Units.MetersPerSecond);
double vZ_mps = this.vZ.in(Units.MetersPerSecond);
double vX_mps = this.xVelocity.in(Units.MetersPerSecond);
double vY_mps = this.yVelocity.in(Units.MetersPerSecond);
double vZ_mps = this.zVelocity.in(Units.MetersPerSecond);
this.cache_squareMagnitudeVelocity = Math.pow(vX_mps, 2) +
Math.pow(vY_mps, 2) +
Math.pow(vZ_mps, 2);
@ -178,7 +176,7 @@ public class Velocity3d {
* @return The elevation angle of the velocity, with respect to the horizontal (XY) components
* of the velocity.
*/
public Angle getElevation() {
public Angle getElevationAngle() {
if(this.cache_eleveationAngle == null) {
double vZ_mps = this.getZVelocity().in(Units.MetersPerSecond);
double vHori_mps = this.getHorizontalVelocity().in(Units.MetersPerSecond);
@ -207,9 +205,9 @@ public class Velocity3d {
*/
public Velocity3d minus(Velocity3d other) {
return new Velocity3d(
this.vX.minus(other.vX),
this.vY.minus(other.vY),
this.vZ.minus(other.vZ)
this.xVelocity.minus(other.xVelocity),
this.yVelocity.minus(other.yVelocity),
this.zVelocity.minus(other.zVelocity)
);
}
@ -227,33 +225,19 @@ public class Velocity3d {
}
/**
* Multiply the velocity by time in order to get a displacement
* @param time The time to compute over
* @return The resulting displacement
*/
public Translation3d times(Time time) {
return new Translation3d(
this.vX.times(time),
this.vY.times(time),
this.vZ.times(time)
);
}
//
// Overrides
//
@Override
public int hashCode() {
return this.vX.hashCode() ^ this.vY.hashCode() ^ this.vZ.hashCode();
return this.xVelocity.hashCode() ^ this.yVelocity.hashCode() ^ this.zVelocity.hashCode();
}
@Override
public String toString() {
return String.format("Velocity3d(X: %s, Y: %s, Z: %s)", vX, vY, vZ);
return String.format("Velocity3d(X: %s, Y: %s, Z: %s)", xVelocity, yVelocity, zVelocity);
}
@ -262,9 +246,9 @@ public class Velocity3d {
if(Velocity3d.class.isInstance(obj)) {
Velocity3d otherVelocity = (Velocity3d) obj;
return
this.vX == otherVelocity.getXVelocity() &&
this.vY == otherVelocity.getYVelocity() &&
this.vZ == otherVelocity.getZVelocity();
this.xVelocity.equals(otherVelocity.getXVelocity()) &&
this.yVelocity.equals(otherVelocity.getYVelocity()) &&
this.zVelocity.equals(otherVelocity.getZVelocity());
}
return false;
}

105
lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/Velocity3dTest.java Normal file
View File

@ -0,0 +1,105 @@
package org.fairportrobotics.frc.robolib.trajectory;
import org.junit.jupiter.api.Test;
import edu.wpi.first.units.Units;
import static org.junit.jupiter.api.Assertions.*;
public class Velocity3dTest {
private static final double epsilon=1e-10;
//
// Test base create and coordinate space transform
//
void testConstructorBase(Velocity3d test) {
// Check cartesian
assertEquals(Math.cos(Math.toRadians(45)), test.getXVelocity().in(Units.MetersPerSecond), epsilon, "X");
assertEquals(Math.sin(Math.toRadians(45)), test.getYVelocity().in(Units.MetersPerSecond), epsilon, "Y");
assertEquals(1.0, test.getZVelocity().in(Units.MetersPerSecond), epsilon, "Z");
// Check cylindrical
assertEquals(1.0, test.getVeritcalVelocity().in(Units.MetersPerSecond), epsilon, "Vertical");
assertEquals(1.0, test.getHorizontalVelocity().in(Units.MetersPerSecond), epsilon, "Horizontal");
assertEquals(45, test.getAzimuthAngle().in(Units.Degrees), epsilon, "Azimuth");
// Check spherical
assertEquals(45, test.getElevationAngle().in(Units.Degrees), epsilon, "Elevation");
assertEquals(Math.sqrt(2.0), test.getSphericalVelocity().in(Units.MetersPerSecond), epsilon, "Magnitude");
}
@Test void testCartesianConstructor() {
Velocity3d test = new Velocity3d(
Units.MetersPerSecond.of(Math.cos(Math.toRadians(45))),
Units.MetersPerSecond.of(Math.sin(Math.toRadians(45))),
Units.MetersPerSecond.of(1.0)
);
testConstructorBase(test);
// Check implicit cast to LinearVelocity
assertEquals(test, new Velocity3d(
Math.cos(Math.toRadians(45)),
Math.sin(Math.toRadians(45)),
1.0
), "Implicit convert"
);
}
@Test void testCylindicalConstructor() {
testConstructorBase(new Velocity3d(
Units.MetersPerSecond.of(1.0),
Units.MetersPerSecond.of(1.0),
Units.Degrees.of(45)
));
}
@Test void testSphericalConstructor() {
testConstructorBase(new Velocity3d(
Units.MetersPerSecond.of(Math.sqrt(2)),
Units.Degrees.of(45),
Units.Degrees.of(45)
));
}
//
// Test additional getters
//
@Test void testMagnitudes() {
Velocity3d test = new Velocity3d(1.0, 1.0, 1.0);
assertEquals(3.0, test.getSquareMagnitude(), epsilon, "Square magnitude");
assertEquals(Math.sqrt(3.0), test.getMagnitude().in(Units.MetersPerSecond), epsilon, "Magnitude");
}
//
// Test transforms
//
@Test void testMinus() {
Velocity3d test = new Velocity3d(1.0, 1.0, 1.0).minus(
new Velocity3d(2.0, 2.0, 2.0)
);
assertEquals(new Velocity3d(-1.0, -1.0, -1.0), test, "Subtract");
}
@Test void testRotate() {
Velocity3d test = new Velocity3d(
Units.MetersPerSecond.of(1.0),
Units.Degrees.of(45),
Units.Degrees.of(45)
).rotate(Units.Degrees.of(45));
assertEquals(
new Velocity3d(
Units.MetersPerSecond.of(1.0),
Units.Degrees.of(45),
Units.Degrees.of(90)
), test, "Rotate"
);
}
}