Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Convert static ChassisSpeeds factories to use instance methods and remove deprecation #7433

Closed
wants to merge 3 commits into from
Closed

Convert static ChassisSpeeds factories to use instance methods and remove deprecation #7433

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
3b546c4
Return self reference from ChassisSpeeds modifiers
mjansen4857 Nov 25, 2024
7753b30
Revert "Return self reference from ChassisSpeeds modifiers"
mjansen4857 Nov 29, 2024
d0b3f9a
ChassisSpeeds factories use instance methods & un-deprecate
mjansen4857 Nov 29, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
75 changes: 36 additions & 39 deletions wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -98,33 +98,23 @@ public Twist2d toTwist2d(double dtSeconds) {
* <p>This is useful for compensating for translational skew when translating and rotating a
* swerve drivetrain.
*
* <p>This will create a new ChassisSpeeds object. To modify an existing ChassisSpeeds object
* without creating a new one, use the instance method instead.
*
* @param vxMetersPerSecond Forward velocity.
* @param vyMetersPerSecond Sideways velocity.
* @param omegaRadiansPerSecond Angular velocity.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
* @return Discretized ChassisSpeeds.
* @deprecated Use instance method instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds discretize(
double vxMetersPerSecond,
double vyMetersPerSecond,
double omegaRadiansPerSecond,
double dtSeconds) {
// Construct the desired pose after a timestep, relative to the current pose. The desired pose
// has decoupled translation and rotation.
var desiredDeltaPose =
new Pose2d(
vxMetersPerSecond * dtSeconds,
vyMetersPerSecond * dtSeconds,
new Rotation2d(omegaRadiansPerSecond * dtSeconds));

// Find the chassis translation/rotation deltas in the robot frame that move the robot from its
// current pose to the desired pose
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);
var speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
speeds.discretize(dtSeconds);
return speeds;
}

/**
Expand All @@ -138,14 +128,15 @@ public static ChassisSpeeds discretize(
* <p>This is useful for compensating for translational skew when translating and rotating a
* swerve drivetrain.
*
* <p>This will create a new ChassisSpeeds object. To modify an existing ChassisSpeeds object
* without creating a new one, use the instance method instead.
*
* @param vx Forward velocity.
* @param vy Sideways velocity.
* @param omega Angular velocity.
* @param dt The duration of the timestep the speeds should be applied for.
* @return Discretized ChassisSpeeds.
* @deprecated Use instance method instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds discretize(
LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Time dt) {
return discretize(
Expand All @@ -163,12 +154,13 @@ public static ChassisSpeeds discretize(
* <p>This is useful for compensating for translational skew when translating and rotating a
* swerve drivetrain.
*
* <p>This will create a new ChassisSpeeds object. To modify an existing ChassisSpeeds object
* without creating a new one, use the instance method instead.
*
* @param continuousSpeeds The continuous speeds.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
* @return Discretized ChassisSpeeds.
* @deprecated Use instance method instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dtSeconds) {
return discretize(
continuousSpeeds.vxMetersPerSecond,
Expand Down Expand Up @@ -211,6 +203,9 @@ public void discretize(double dtSeconds) {
* Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
* object.
*
* <p>This will create a new ChassisSpeeds object. To modify an existing ChassisSpeeds object
* without creating a new one, use the instance method instead.
*
* @param vxMetersPerSecond The component of speed in the x direction relative to the field.
* Positive x is away from your alliance wall.
* @param vyMetersPerSecond The component of speed in the y direction relative to the field.
Expand All @@ -220,24 +215,24 @@ public void discretize(double dtSeconds) {
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
* @deprecated Use toRobotRelativeSpeeds instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromFieldRelativeSpeeds(
double vxMetersPerSecond,
double vyMetersPerSecond,
double omegaRadiansPerSecond,
Rotation2d robotAngle) {
// CW rotation into chassis frame
var rotated =
new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle.unaryMinus());
return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond);
var speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
speeds.toRobotRelativeSpeeds(robotAngle);
return speeds;
}

/**
* Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
* object.
*
* <p>This will create a new ChassisSpeeds object. To modify an existing ChassisSpeeds object
* without creating a new one, use the instance method instead.
*
* @param vx The component of speed in the x direction relative to the field. Positive x is away
* from your alliance wall.
* @param vy The component of speed in the y direction relative to the field. Positive y is to
Expand All @@ -247,9 +242,7 @@ public static ChassisSpeeds fromFieldRelativeSpeeds(
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
* @deprecated Use toRobotRelativeSpeeds instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromFieldRelativeSpeeds(
LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) {
return fromFieldRelativeSpeeds(
Expand All @@ -260,16 +253,17 @@ public static ChassisSpeeds fromFieldRelativeSpeeds(
* Converts a user provided field-relative ChassisSpeeds object into a robot-relative
* ChassisSpeeds object.
*
* <p>This will create a new ChassisSpeeds object. To modify an existing ChassisSpeeds object
* without creating a new one, use the instance method instead.
*
* @param fieldRelativeSpeeds The ChassisSpeeds object representing the speeds in the field frame
* of reference. Positive x is away from your alliance wall. Positive y is to your left when
* standing behind the alliance wall.
* @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
* @deprecated Use toRobotRelativeSpeeds instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromFieldRelativeSpeeds(
ChassisSpeeds fieldRelativeSpeeds, Rotation2d robotAngle) {
return fromFieldRelativeSpeeds(
Expand Down Expand Up @@ -298,6 +292,9 @@ public void toRobotRelativeSpeeds(Rotation2d robotAngle) {
* Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds
* object.
*
* <p>This will create a new ChassisSpeeds object. To modify an existing ChassisSpeeds object
* without creating a new one, use the instance method instead.
*
* @param vxMetersPerSecond The component of speed in the x direction relative to the robot.
* Positive x is towards the robot's front.
* @param vyMetersPerSecond The component of speed in the y direction relative to the robot.
Expand All @@ -307,23 +304,24 @@ public void toRobotRelativeSpeeds(Rotation2d robotAngle) {
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the field's frame of reference.
* @deprecated Use toFieldRelativeSpeeds instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromRobotRelativeSpeeds(
double vxMetersPerSecond,
double vyMetersPerSecond,
double omegaRadiansPerSecond,
Rotation2d robotAngle) {
// CCW rotation out of chassis frame
var rotated = new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle);
return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond);
var speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
speeds.toFieldRelativeSpeeds(robotAngle);
return speeds;
}

/**
* Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds
* object.
*
* <p>This will create a new ChassisSpeeds object. To modify an existing ChassisSpeeds object
* without creating a new one, use the instance method instead.
*
* @param vx The component of speed in the x direction relative to the robot. Positive x is
* towards the robot's front.
* @param vy The component of speed in the y direction relative to the robot. Positive y is
Expand All @@ -333,9 +331,7 @@ public static ChassisSpeeds fromRobotRelativeSpeeds(
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the field's frame of reference.
* @deprecated Use toFieldRelativeSpeeds instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromRobotRelativeSpeeds(
LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) {
return fromRobotRelativeSpeeds(
Expand All @@ -346,16 +342,17 @@ public static ChassisSpeeds fromRobotRelativeSpeeds(
* Converts a user provided robot-relative ChassisSpeeds object into a field-relative
* ChassisSpeeds object.
*
* <p>This will create a new ChassisSpeeds object. To modify an existing ChassisSpeeds object
* without creating a new one, use the instance method instead.
*
* @param robotRelativeSpeeds The ChassisSpeeds object representing the speeds in the robot frame
* of reference. Positive x is towards the robot's front. Positive y is towards the robot's
* left.
* @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the field's frame of reference.
* @deprecated Use toFieldRelativeSpeeds instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromRobotRelativeSpeeds(
ChassisSpeeds robotRelativeSpeeds, Rotation2d robotAngle) {
return fromRobotRelativeSpeeds(
Expand Down
Loading