Skip to content

Commit

Permalink
Revert "[wpimath] ChassisSpeeds fromRelative and discretize methods m…
Browse files Browse the repository at this point in the history
…ade instance methods (#7115)"

This reverts commit a3b12b3.
  • Loading branch information
PeterJohnson committed Dec 14, 2024
1 parent 68285da commit c49d05f
Show file tree
Hide file tree
Showing 8 changed files with 52 additions and 127 deletions.
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
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
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
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
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
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
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
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

0 comments on commit c49d05f

Please sign in to comment.