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

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

Browse Source
This commit is contained in:
Nicholas Armstrong
2024-10-22 19:23:17 -04:00
committed by GitHub
parent cbc9264468
commit a3b12b3bd9
8 changed files with 127 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
View File

@@ -136,14 +136,12 @@ public class Drivetrain {
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var mecanumDriveWheelSpeeds =
m_kinematics.toWheelSpeeds(
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot),
periodSeconds));
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d());
}
chassisSpeeds.discretize(periodSeconds);
var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
mecanumDriveWheelSpeeds.desaturate(kMaxSpeed);
setSpeeds(mecanumDriveWheelSpeeds);
}

14
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
View File

@@ -148,14 +148,12 @@ public class Drivetrain {
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var mecanumDriveWheelSpeeds =
m_kinematics.toWheelSpeeds(
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation())
: new ChassisSpeeds(xSpeed, ySpeed, rot),
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);
mecanumDriveWheelSpeeds.desaturate(kMaxSpeed);
setSpeeds(mecanumDriveWheelSpeeds);
}

14
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
View File

@@ -57,14 +57,12 @@ public class Drivetrain {
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var swerveModuleStates =
m_kinematics.toSwerveModuleStates(
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot),
periodSeconds));
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d());
}
chassisSpeeds.discretize(periodSeconds);
var swerveModuleStates = m_kinematics.toWheelSpeeds(chassisSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed);
m_frontLeft.setDesiredState(swerveModuleStates[0]);
m_frontRight.setDesiredState(swerveModuleStates[1]);

14
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
View File

@@ -118,14 +118,12 @@ public class DriveSubsystem extends SubsystemBase {
* @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 swerveModuleStates =
DriveConstants.kDriveKinematics.toSwerveModuleStates(
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot),
DriveConstants.kDrivePeriod));
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d());
}
chassisSpeeds.discretize(DriveConstants.kDrivePeriod);
var swerveModuleStates = DriveConstants.kDriveKinematics.toWheelSpeeds(chassisSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(
swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond);
m_frontLeft.setDesiredState(swerveModuleStates[0]);

24
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
View File

@@ -36,8 +36,11 @@ 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,
@@ -66,14 +69,12 @@ public class Drivetrain {
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var swerveModuleStates =
m_kinematics.toSwerveModuleStates(
ChassisSpeeds.discretize(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation())
: new ChassisSpeeds(xSpeed, ySpeed, rot),
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);
SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed);
m_frontLeft.setDesiredState(swerveModuleStates[0]);
m_frontRight.setDesiredState(swerveModuleStates[1]);
@@ -92,7 +93,8 @@ public class Drivetrain {
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(

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

@@ -102,9 +102,10 @@ public class HolonomicDriveController {
m_poseError = trajectoryPose.relativeTo(currentPose);
m_rotationError = desiredHeading.minus(currentPose.getRotation());
ChassisSpeeds speeds = new ChassisSpeeds(xFF, yFF, thetaFF);
if (!m_enabled) {
return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation());
speeds.toRobotRelativeSpeeds(currentPose.getRotation());
return speeds;
}
// Calculate feedback velocities (based on position error).
@@ -112,8 +113,10 @@ public class HolonomicDriveController {
double yFeedback = m_yController.calculate(currentPose.getY(), trajectoryPose.getY());
// Return next output.
return ChassisSpeeds.fromFieldRelativeSpeeds(
xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.getRotation());
speeds.vxMetersPerSecond += xFeedback;
speeds.vyMetersPerSecond += yFeedback;
speeds.toRobotRelativeSpeeds(currentPose.getRotation());
return speeds;
}
/**

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

@@ -103,7 +103,9 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* @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,
@@ -141,7 +143,9 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* @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(
@@ -162,7 +166,9 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* @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,
@@ -171,6 +177,36 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
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.
@@ -184,7 +220,9 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* 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,
@@ -209,7 +247,9 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* 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(
@@ -227,7 +267,9 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* 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(
@@ -237,6 +279,21 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
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.
@@ -250,7 +307,9 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* 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,
@@ -274,7 +333,9 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* 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(
@@ -292,7 +353,9 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* 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(
@@ -302,6 +365,20 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
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.
*

11
wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
View File

@@ -20,10 +20,11 @@ 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;
final var speeds = ChassisSpeeds.discretize(target, duration);
speeds.discretize(duration);
final var twist =
new Twist2d(
speeds.vxMetersPerSecond * dt,
@@ -61,8 +62,8 @@ class ChassisSpeedsTest {
@Test
void testFromFieldRelativeSpeeds() {
final var chassisSpeeds =
ChassisSpeeds.fromFieldRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.kCW_Pi_2);
final var chassisSpeeds = new ChassisSpeeds(1.0, 0.0, 0.5);
chassisSpeeds.toRobotRelativeSpeeds(Rotation2d.kCW_Pi_2);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
@@ -72,8 +73,8 @@ class ChassisSpeedsTest {
@Test
void testFromRobotRelativeSpeeds() {
final var chassisSpeeds =
ChassisSpeeds.fromRobotRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(45.0));
final var chassisSpeeds = new ChassisSpeeds(1.0, 0.0, 0.5);
chassisSpeeds.toFieldRelativeSpeeds(Rotation2d.fromDegrees(45.0));
assertAll(
() -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vxMetersPerSecond, kEpsilon),