Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Java: add static instantiations of common rotations (#6563)

Browse Source 
C++ doesn't need this because it supports value types, which are much
cheaper to construct. constexpr is also available to make construction
zero-cost.
This commit is contained in:
Tyler Veness
2024-05-03 12:39:35 -07:00
committed by GitHub
parent 9c7120e6bf
commit e172aa66f7
75 changed files with 499 additions and 429 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java
View File

@@ -27,7 +27,7 @@ public final class ComputerVisionUtil {
* either be a constant for a rigidly mounted camera, or variable if the camera is mounted to
* a turret. If the camera was mounted 3 inches in front of the "origin" (usually physical
* center) of the robot, this would be new Transform3d(Units.inchesToMeters(3.0), 0.0, 0.0,
* new Rotation3d()).
* Rotation3d.kZero).
* @return The robot's field-relative pose.
*/
public static Pose3d objectToRobotPose(

6
wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
View File

@@ -22,9 +22,9 @@ import edu.wpi.first.math.util.Units;
* point toward. This heading reference is profiled for smoothness.
*/
public class HolonomicDriveController {
private Pose2d m_poseError = new Pose2d();
private Rotation2d m_rotationError = new Rotation2d();
private Pose2d m_poseTolerance = new Pose2d();
private Pose2d m_poseError = Pose2d.kZero;
private Rotation2d m_rotationError = Rotation2d.kZero;
private Pose2d m_poseTolerance = Pose2d.kZero;
private boolean m_enabled = true;
private final PIDController m_xController;

4
wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java
View File

@@ -37,8 +37,8 @@ public class RamseteController {
private final double m_zeta;
private Pose2d m_poseError = new Pose2d();
private Pose2d m_poseTolerance = new Pose2d();
private Pose2d m_poseError = Pose2d.kZero;
private Pose2d m_poseTolerance = Pose2d.kZero;
private boolean m_enabled = true;
/**

11
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
View File

@@ -26,13 +26,20 @@ import java.util.Objects;
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, StructSerializable {
/**
* A preallocated Pose2d representing the origin.
*
* <p>This exists to avoid allocations for common poses.
*/
public static final Pose2d kZero = new Pose2d();
private final Translation2d m_translation;
private final Rotation2d m_rotation;
/** Constructs a pose at the origin facing toward the positive X axis. */
public Pose2d() {
m_translation = new Translation2d();
m_rotation = new Rotation2d();
m_translation = Translation2d.kZero;
m_rotation = Rotation2d.kZero;
}
/**

11
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
View File

@@ -20,13 +20,20 @@ import java.util.Objects;
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, StructSerializable {
/**
* A preallocated Pose3d representing the origin.
*
* <p>This exists to avoid allocations for common poses.
*/
public static final Pose3d kZero = new Pose3d();
private final Translation3d m_translation;
private final Rotation3d m_rotation;
/** Constructs a pose at the origin facing toward the positive X axis. */
public Pose3d() {
m_translation = new Translation3d();
m_rotation = new Rotation3d();
m_translation = Translation3d.kZero;
m_rotation = Rotation3d.kZero;
}
/**

49
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
View File

@@ -32,6 +32,55 @@ import java.util.Objects;
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Rotation2d
implements Interpolatable<Rotation2d>, ProtobufSerializable, StructSerializable {
/**
* A preallocated Rotation2d representing no rotation.
*
* <p>This exists to avoid allocations for common rotations.
*/
public static final Rotation2d kZero = new Rotation2d();
/**
* A preallocated Rotation2d representing a clockwise rotation by π/2 rad (90°).
*
* <p>This exists to avoid allocations for common rotations.
*/
public static final Rotation2d kCW_Pi_2 = new Rotation2d(-Math.PI / 2);
/**
* A preallocated Rotation2d representing a clockwise rotation by 90° (π/2 rad).
*
* <p>This exists to avoid allocations for common rotations.
*/
public static final Rotation2d kCW_90deg = kCW_Pi_2;
/**
* A preallocated Rotation2d representing a counterclockwise rotation by π/2 rad (90°).
*
* <p>This exists to avoid allocations for common rotations.
*/
public static final Rotation2d kCCW_Pi_2 = new Rotation2d(Math.PI / 2);
/**
* A preallocated Rotation2d representing a counterclockwise rotation by 90° (π/2 rad).
*
* <p>This exists to avoid allocations for common rotations.
*/
public static final Rotation2d kCCW_90deg = kCCW_Pi_2;
/**
* A preallocated Rotation2d representing a counterclockwise rotation by π rad (180°).
*
* <p>This exists to avoid allocations for common rotations.
*/
public static final Rotation2d kPi = new Rotation2d(Math.PI);
/**
* A preallocated Rotation2d representing a counterclockwise rotation by 180° (π rad).
*
* <p>This exists to avoid allocations for common rotations.
*/
public static final Rotation2d k180deg = kPi;
private final double m_value;
private final double m_cos;
private final double m_sin;

7
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
View File

@@ -28,6 +28,13 @@ import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Rotation3d
implements Interpolatable<Rotation3d>, ProtobufSerializable, StructSerializable {
/**
* A preallocated Rotation3d representing no rotation.
*
* <p>This exists to avoid allocations for common rotations.
*/
public static final Rotation3d kZero = new Rotation3d();
private final Quaternion m_q;
/** Constructs a Rotation3d representing no rotation. */

13
wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
View File

@@ -16,6 +16,13 @@ import java.util.Objects;
/** Represents a transformation for a Pose2d in the pose's frame. */
public class Transform2d implements ProtobufSerializable, StructSerializable {
/**
* A preallocated Transform2d representing no transformation.
*
* <p>This exists to avoid allocations for common transformations.
*/
public static final Transform2d kZero = new Transform2d();
private final Translation2d m_translation;
private final Rotation2d m_rotation;
@@ -74,8 +81,8 @@ public class Transform2d implements ProtobufSerializable, StructSerializable {
/** Constructs the identity transform -- maps an initial pose to itself. */
public Transform2d() {
m_translation = new Translation2d();
m_rotation = new Rotation2d();
m_translation = Translation2d.kZero;
m_rotation = Rotation2d.kZero;
}
/**
@@ -106,7 +113,7 @@ public class Transform2d implements ProtobufSerializable, StructSerializable {
* @return The composition of the two transformations.
*/
public Transform2d plus(Transform2d other) {
return new Transform2d(new Pose2d(), new Pose2d().transformBy(this).transformBy(other));
return new Transform2d(Pose2d.kZero, Pose2d.kZero.transformBy(this).transformBy(other));
}
/**

13
wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
View File

@@ -12,6 +12,13 @@ import java.util.Objects;
/** Represents a transformation for a Pose3d in the pose's frame. */
public class Transform3d implements ProtobufSerializable, StructSerializable {
/**
* A preallocated Transform3d representing no transformation.
*
* <p>This exists to avoid allocations for common transformations.
*/
public static final Transform3d kZero = new Transform3d();
private final Translation3d m_translation;
private final Rotation3d m_rotation;
@@ -59,8 +66,8 @@ public class Transform3d implements ProtobufSerializable, StructSerializable {
/** Constructs the identity transform -- maps an initial pose to itself. */
public Transform3d() {
m_translation = new Translation3d();
m_rotation = new Rotation3d();
m_translation = Translation3d.kZero;
m_rotation = Rotation3d.kZero;
}
/**
@@ -91,7 +98,7 @@ public class Transform3d implements ProtobufSerializable, StructSerializable {
* @return The composition of the two transformations.
*/
public Transform3d plus(Transform3d other) {
return new Transform3d(new Pose3d(), new Pose3d().transformBy(this).transformBy(other));
return new Transform3d(Pose3d.kZero, Pose3d.kZero.transformBy(this).transformBy(other));
}
/**

7
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
View File

@@ -36,6 +36,13 @@ import java.util.Objects;
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Translation2d
implements Interpolatable<Translation2d>, ProtobufSerializable, StructSerializable {
/**
* A preallocated Translation2d representing the origin.
*
* <p>This exists to avoid allocations for common translations.
*/
public static final Translation2d kZero = new Translation2d();
private final double m_x;
private final double m_y;

7
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
View File

@@ -34,6 +34,13 @@ import java.util.Objects;
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Translation3d
implements Interpolatable<Translation3d>, ProtobufSerializable, StructSerializable {
/**
* A preallocated Translation3d representing the origin.
*
* <p>This exists to avoid allocations for common translations.
*/
public static final Translation3d kZero = new Translation3d();
private final double m_x;
private final double m_y;
private final double m_z;

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
View File

@@ -109,7 +109,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
// Find the chassis translation/rotation deltas in the robot frame that move the robot from its
// current pose to the desired pose
var twist = new Pose2d().log(desiredDeltaPose);
var twist = Pose2d.kZero.log(desiredDeltaPose);
// Turn the chassis translation/rotation deltas into average velocities
return new ChassisSpeeds(twist.dx / dtSeconds, twist.dy / dtSeconds, twist.dtheta / dtSeconds);

4
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
View File

@@ -70,7 +70,7 @@ public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPo
*/
public DifferentialDriveOdometry(
Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
this(gyroAngle, leftDistanceMeters, rightDistanceMeters, new Pose2d());
this(gyroAngle, leftDistanceMeters, rightDistanceMeters, Pose2d.kZero);
}
/**
@@ -82,7 +82,7 @@ public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPo
*/
public DifferentialDriveOdometry(
Rotation2d gyroAngle, Measure<Distance> leftDistance, Measure<Distance> rightDistance) {
this(gyroAngle, leftDistance, rightDistance, new Pose2d());
this(gyroAngle, leftDistance, rightDistance, Pose2d.kZero);
}
/**

4
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
View File

@@ -46,7 +46,7 @@ public class MecanumDriveKinematics
private final Translation2d m_rearLeftWheelMeters;
private final Translation2d m_rearRightWheelMeters;
private Translation2d m_prevCoR = new Translation2d();
private Translation2d m_prevCoR = Translation2d.kZero;
/** MecanumDriveKinematics protobuf for serialization. */
public static final MecanumDriveKinematicsProto proto = new MecanumDriveKinematicsProto();
@@ -140,7 +140,7 @@ public class MecanumDriveKinematics
*/
@Override
public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
return toWheelSpeeds(chassisSpeeds, new Translation2d());
return toWheelSpeeds(chassisSpeeds, Translation2d.kZero);
}
/**

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
View File

@@ -45,6 +45,6 @@ public class MecanumDriveOdometry extends Odometry<MecanumDriveWheelPositions> {
MecanumDriveKinematics kinematics,
Rotation2d gyroAngle,
MecanumDriveWheelPositions wheelPositions) {
this(kinematics, gyroAngle, wheelPositions, new Pose2d());
this(kinematics, gyroAngle, wheelPositions, Pose2d.kZero);
}
}

6
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
View File

@@ -65,7 +65,7 @@ public class SwerveDriveKinematics
private final int m_numModules;
private final Translation2d[] m_modules;
private Rotation2d[] m_moduleHeadings;
private Translation2d m_prevCoR = new Translation2d();
private Translation2d m_prevCoR = Translation2d.kZero;
/**
* Constructs a swerve drive kinematics object. This takes in a variable number of module
@@ -84,7 +84,7 @@ public class SwerveDriveKinematics
m_numModules = moduleTranslationsMeters.length;
m_modules = Arrays.copyOf(moduleTranslationsMeters, m_numModules);
m_moduleHeadings = new Rotation2d[m_numModules];
Arrays.fill(m_moduleHeadings, new Rotation2d());
Arrays.fill(m_moduleHeadings, Rotation2d.kZero);
m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
for (int i = 0; i < m_numModules; i++) {
@@ -196,7 +196,7 @@ public class SwerveDriveKinematics
* @return An array containing the module states.
*/
public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds) {
return toSwerveModuleStates(chassisSpeeds, new Translation2d());
return toSwerveModuleStates(chassisSpeeds, Translation2d.kZero);
}
@Override

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
View File

@@ -51,7 +51,7 @@ public class SwerveDriveOdometry extends Odometry<SwerveDriveWheelPositions> {
SwerveDriveKinematics kinematics,
Rotation2d gyroAngle,
SwerveModulePosition[] modulePositions) {
this(kinematics, gyroAngle, modulePositions, new Pose2d());
this(kinematics, gyroAngle, modulePositions, Pose2d.kZero);
}
/**

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
View File

@@ -27,7 +27,7 @@ public class SwerveModulePosition
public double distanceMeters;
/** Angle of the module. */
public Rotation2d angle = Rotation2d.fromDegrees(0);
public Rotation2d angle = Rotation2d.kZero;
/** SwerveModulePosition protobuf for serialization. */
public static final SwerveModulePositionProto proto = new SwerveModulePositionProto();

5
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
View File

@@ -23,7 +23,7 @@ public class SwerveModuleState
public double speedMetersPerSecond;
/** Angle of the module. */
public Rotation2d angle = Rotation2d.fromDegrees(0);
public Rotation2d angle = Rotation2d.kZero;
/** SwerveModuleState protobuf for serialization. */
public static final SwerveModuleStateProto proto = new SwerveModuleStateProto();
@@ -102,8 +102,7 @@ public class SwerveModuleState
var delta = desiredState.angle.minus(currentAngle);
if (Math.abs(delta.getDegrees()) > 90.0) {
return new SwerveModuleState(
-desiredState.speedMetersPerSecond,
desiredState.angle.rotateBy(Rotation2d.fromDegrees(180.0)));
-desiredState.speedMetersPerSecond, desiredState.angle.rotateBy(Rotation2d.kPi));
} else {
return new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
}

2
wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java
View File

@@ -27,6 +27,6 @@ public class PoseWithCurvature {
/** Constructs a PoseWithCurvature with default values. */
public PoseWithCurvature() {
poseMeters = new Pose2d();
poseMeters = Pose2d.kZero;
}
}

2
wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
View File

@@ -295,7 +295,7 @@ public class Trajectory implements ProtobufSerializable {
/** Default constructor. */
public State() {
poseMeters = new Pose2d();
poseMeters = Pose2d.kZero;
}
/**

15
wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java
View File

@@ -21,6 +21,8 @@ import java.util.function.BiConsumer;
/** Helper class used to generate trajectories with various constraints. */
public final class TrajectoryGenerator {
private static final Transform2d kFlip = new Transform2d(Translation2d.kZero, Rotation2d.kPi);
private static final Trajectory kDoNothingTrajectory =
new Trajectory(List.of(new Trajectory.State()));
private static BiConsumer<String, StackTraceElement[]> errorFunc;
@@ -62,8 +64,6 @@ public final class TrajectoryGenerator {
List<Translation2d> interiorWaypoints,
Spline.ControlVector end,
TrajectoryConfig config) {
final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
// Clone the control vectors.
var newInitial = new Spline.ControlVector(initial.x, initial.y);
var newEnd = new Spline.ControlVector(end.x, end.y);
@@ -91,7 +91,7 @@ public final class TrajectoryGenerator {
// Change the points back to their original orientation.
if (config.isReversed()) {
for (var point : points) {
point.poseMeters = point.poseMeters.plus(flip);
point.poseMeters = point.poseMeters.plus(kFlip);
point.curvatureRadPerMeter *= -1;
}
}
@@ -140,7 +140,6 @@ public final class TrajectoryGenerator {
*/
public static Trajectory generateTrajectory(
ControlVectorList controlVectors, TrajectoryConfig config) {
final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
final var newControlVectors = new ArrayList<Spline.ControlVector>(controlVectors.size());
// Create a new control vector list, flipping the orientation if reversed.
@@ -168,7 +167,7 @@ public final class TrajectoryGenerator {
// Change the points back to their original orientation.
if (config.isReversed()) {
for (var point : points) {
point.poseMeters = point.poseMeters.plus(flip);
point.poseMeters = point.poseMeters.plus(kFlip);
point.curvatureRadPerMeter *= -1;
}
}
@@ -194,12 +193,10 @@ public final class TrajectoryGenerator {
* @return The generated trajectory.
*/
public static Trajectory generateTrajectory(List<Pose2d> waypoints, TrajectoryConfig config) {
final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
List<Pose2d> newWaypoints = new ArrayList<>();
if (config.isReversed()) {
for (Pose2d originalWaypoint : waypoints) {
newWaypoints.add(originalWaypoint.plus(flip));
newWaypoints.add(originalWaypoint.plus(kFlip));
}
} else {
newWaypoints.addAll(waypoints);
@@ -220,7 +217,7 @@ public final class TrajectoryGenerator {
// Change the points back to their original orientation.
if (config.isReversed()) {
for (var point : points) {
point.poseMeters = point.poseMeters.plus(flip);
point.poseMeters = point.poseMeters.plus(kFlip);
point.curvatureRadPerMeter *= -1;
}
}