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

[wpimath] Revert "ChassisSpeeds fromRelative and discretize methods made instance methods (#7115)" #7549

Merged
merged 1 commit into from
Dec 14, 2024
Merged

[wpimath] Revert "ChassisSpeeds fromRelative and discretize methods made instance methods (#7115)" #7549

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
14 changes: 8 additions & 6 deletions wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -129,12 +129,14 @@ public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d());
}
chassisSpeeds.discretize(periodSeconds);
var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
var mecanumDriveWheelSpeeds =
m_kinematics.toWheelSpeeds(
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot),
periodSeconds));
mecanumDriveWheelSpeeds.desaturate(kMaxSpeed);
setSpeeds(mecanumDriveWheelSpeeds);
}
Expand Down
14 changes: 8 additions & 6 deletions ...es/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -141,12 +141,14 @@ public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds.toRobotRelativeSpeeds(m_poseEstimator.getEstimatedPosition().getRotation());
}
chassisSpeeds.discretize(periodSeconds);
var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
var mecanumDriveWheelSpeeds =
m_kinematics.toWheelSpeeds(
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation())
: new ChassisSpeeds(xSpeed, ySpeed, rot),
periodSeconds));
mecanumDriveWheelSpeeds.desaturate(kMaxSpeed);
setSpeeds(mecanumDriveWheelSpeeds);
}
Expand Down
14 changes: 8 additions & 6 deletions wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,12 +57,14 @@ public Drivetrain() {
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d());
}
chassisSpeeds.discretize(periodSeconds);
var swerveModuleStates = m_kinematics.toWheelSpeeds(chassisSpeeds);
var swerveModuleStates =
m_kinematics.toSwerveModuleStates(
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot),
periodSeconds));
SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed);
m_frontLeft.setDesiredState(swerveModuleStates[0]);
m_frontRight.setDesiredState(swerveModuleStates[1]);
Expand Down
14 changes: 8 additions & 6 deletions ...ava/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -118,12 +118,14 @@ public void resetOdometry(Pose2d pose) {
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
*/
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d());
}
chassisSpeeds.discretize(DriveConstants.kDrivePeriod);
var swerveModuleStates = DriveConstants.kDriveKinematics.toWheelSpeeds(chassisSpeeds);
var swerveModuleStates =
DriveConstants.kDriveKinematics.toSwerveModuleStates(
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot),
DriveConstants.kDrivePeriod));
SwerveDriveKinematics.desaturateWheelSpeeds(
swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond);
m_frontLeft.setDesiredState(swerveModuleStates[0]);
Expand Down
24 changes: 11 additions & 13 deletions ...les/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,11 +36,8 @@ public class Drivetrain {
new SwerveDriveKinematics(
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);

/*
* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings.
* The numbers used
* below are robot specific, and should be tuned.
*/
/* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
below are robot specific, and should be tuned. */
private final SwerveDrivePoseEstimator m_poseEstimator =
new SwerveDrivePoseEstimator(
m_kinematics,
Expand Down Expand Up @@ -69,12 +66,14 @@ public Drivetrain() {
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds.toRobotRelativeSpeeds(m_poseEstimator.getEstimatedPosition().getRotation());
}
chassisSpeeds.discretize(periodSeconds);
var swerveModuleStates = m_kinematics.toWheelSpeeds(chassisSpeeds);
var swerveModuleStates =
m_kinematics.toSwerveModuleStates(
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation())
: new ChassisSpeeds(xSpeed, ySpeed, rot),
periodSeconds));
SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed);
m_frontLeft.setDesiredState(swerveModuleStates[0]);
m_frontRight.setDesiredState(swerveModuleStates[1]);
Expand All @@ -93,8 +92,7 @@ public void updateOdometry() {
m_backRight.getPosition()
});

// Also apply vision measurements. We use 0.3 seconds in the past as an example
// -- on
// Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
// a real robot, this must be calculated based either on latency or timestamps.
m_poseEstimator.addVisionMeasurement(
ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
Expand Down
11 changes: 4 additions & 7 deletions wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -102,21 +102,18 @@ public ChassisSpeeds calculate(

m_poseError = trajectoryPose.relativeTo(currentPose);
m_rotationError = desiredHeading.minus(currentPose.getRotation());
ChassisSpeeds speeds = new ChassisSpeeds(xFF, yFF, thetaFF);

if (!m_enabled) {
speeds.toRobotRelativeSpeeds(currentPose.getRotation());
return speeds;
return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation());
}

// Calculate feedback velocities (based on position error).
double xFeedback = m_xController.calculate(currentPose.getX(), trajectoryPose.getX());
double yFeedback = m_yController.calculate(currentPose.getY(), trajectoryPose.getY());

// Return next output.
speeds.vxMetersPerSecond += xFeedback;
speeds.vyMetersPerSecond += yFeedback;
speeds.toRobotRelativeSpeeds(currentPose.getRotation());
return speeds;
return ChassisSpeeds.fromFieldRelativeSpeeds(
xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.getRotation());
}

/**
Expand Down
77 changes: 0 additions & 77 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 @@ -103,9 +103,7 @@ public Twist2d toTwist2d(double dtSeconds) {
* @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,
Expand Down Expand Up @@ -143,9 +141,7 @@ public static ChassisSpeeds discretize(
* @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 @@ -166,9 +162,7 @@ public static ChassisSpeeds discretize(
* @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 All @@ -177,36 +171,6 @@ public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dt
dtSeconds);
}

/**
* Discretizes a continuous-time chassis speed.
*
* <p>This function converts this continuous-time chassis speed into a discrete-time one such that
* when the discrete-time chassis speed is applied for one timestep, the robot moves as if the
* velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt
* along the y-axis, and omega * dt around the z-axis).
*
* <p>This is useful for compensating for translational skew when translating and rotating a
* swerve drivetrain.
*
* @param dtSeconds The duration of the timestep the speeds should be applied for.
*/
public void discretize(double dtSeconds) {
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
vxMetersPerSecond = twist.dx / dtSeconds;
vyMetersPerSecond = twist.dy / dtSeconds;
omegaRadiansPerSecond = twist.dtheta / dtSeconds;
}

/**
* Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
* object.
Expand All @@ -220,9 +184,7 @@ 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,
Expand All @@ -247,9 +209,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 @@ -267,9 +227,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(
ChassisSpeeds fieldRelativeSpeeds, Rotation2d robotAngle) {
return fromFieldRelativeSpeeds(
Expand All @@ -279,21 +237,6 @@ public static ChassisSpeeds fromFieldRelativeSpeeds(
robotAngle);
}

/**
* Converts this field-relative set of speeds into a robot-relative ChassisSpeeds object.
*
* @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.
*/
public void toRobotRelativeSpeeds(Rotation2d robotAngle) {
// CW rotation into chassis frame
var rotated =
new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle.unaryMinus());
vxMetersPerSecond = rotated.getX();
vyMetersPerSecond = rotated.getY();
}

/**
* Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds
* object.
Expand All @@ -307,9 +250,7 @@ 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,
Expand All @@ -333,9 +274,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 @@ -353,9 +292,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(
ChassisSpeeds robotRelativeSpeeds, Rotation2d robotAngle) {
return fromRobotRelativeSpeeds(
Expand All @@ -365,20 +302,6 @@ public static ChassisSpeeds fromRobotRelativeSpeeds(
robotAngle);
}

/**
* Converts this robot-relative set of speeds into a field-relative ChassisSpeeds object.
*
* @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.
*/
public void toFieldRelativeSpeeds(Rotation2d robotAngle) {
// CCW rotation out of chassis frame
var rotated = new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle);
vxMetersPerSecond = rotated.getX();
vyMetersPerSecond = rotated.getY();
}

/**
* Adds two ChassisSpeeds and returns the sum.
*
Expand Down
11 changes: 5 additions & 6 deletions wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,11 +20,10 @@ class ChassisSpeedsTest {
@Test
void testDiscretize() {
final var target = new ChassisSpeeds(1.0, 0.0, 0.5);
final var speeds = new ChassisSpeeds(1.0, 0.0, 0.5);
final var duration = 1.0;
final var dt = 0.01;

speeds.discretize(duration);
final var speeds = ChassisSpeeds.discretize(target, duration);
final var twist =
new Twist2d(
speeds.vxMetersPerSecond * dt,
Expand Down Expand Up @@ -62,8 +61,8 @@ void testMeasureConstructor() {

@Test
void testFromFieldRelativeSpeeds() {
final var chassisSpeeds = new ChassisSpeeds(1.0, 0.0, 0.5);
chassisSpeeds.toRobotRelativeSpeeds(Rotation2d.kCW_Pi_2);
final var chassisSpeeds =
ChassisSpeeds.fromFieldRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.kCW_Pi_2);

assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
Expand All @@ -73,8 +72,8 @@ void testFromFieldRelativeSpeeds() {

@Test
void testFromRobotRelativeSpeeds() {
final var chassisSpeeds = new ChassisSpeeds(1.0, 0.0, 0.5);
chassisSpeeds.toFieldRelativeSpeeds(Rotation2d.fromDegrees(45.0));
final var chassisSpeeds =
ChassisSpeeds.fromRobotRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(45.0));

assertAll(
() -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vxMetersPerSecond, kEpsilon),
Expand Down
Loading