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] Use immutable member functions in ChassisSpeeds (#7545)

Browse Source
This commit is contained in:
Tyler Veness
2024-12-15 16:09:34 -08:00
committed by GitHub
parent 945d416d07
commit 03f0fc4dea
24 changed files with 163 additions and 412 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
View File

@@ -488,7 +488,7 @@ public class MecanumControllerCommand extends Command {
m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get());
var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds);
targetWheelSpeeds.desaturate(m_maxWheelVelocityMetersPerSecond);
targetWheelSpeeds = targetWheelSpeeds.desaturate(m_maxWheelVelocityMetersPerSecond);
double frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond;
double rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond;

2
wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
View File

@@ -167,7 +167,7 @@ void MecanumControllerCommand::Execute() {
m_controller.Calculate(m_pose(), m_desiredState, m_desiredRotation());
auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(targetChassisSpeeds);
targetWheelSpeeds.Desaturate(m_maxWheelVelocity);
targetWheelSpeeds = targetWheelSpeeds.Desaturate(m_maxWheelVelocity);
auto frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeft;
auto rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeft;

15
wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
View File

@@ -4,6 +4,8 @@
#include "Drivetrain.h"
#include <frc/kinematics/ChassisSpeeds.h>
frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const {
return {units::meters_per_second_t{m_frontLeftEncoder.GetRate()},
units::meters_per_second_t{m_frontRightEncoder.GetRate()},
@@ -51,13 +53,12 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
auto wheelSpeeds = m_kinematics.ToWheelSpeeds(frc::ChassisSpeeds::Discretize(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},
period));
wheelSpeeds.Desaturate(kMaxSpeed);
SetSpeeds(wheelSpeeds);
frc::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot};
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.ToRobotRelative(m_gyro.GetRotation2d());
}
SetSpeeds(m_kinematics.ToWheelSpeeds(chassisSpeeds.Discretize(period))
.Desaturate(kMaxSpeed));
}
void Drivetrain::UpdateOdometry() {

15
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -48,14 +48,13 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
auto wheelSpeeds = m_kinematics.ToWheelSpeeds(frc::ChassisSpeeds::Discretize(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot,
m_poseEstimator.GetEstimatedPosition().Rotation())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},
period));
wheelSpeeds.Desaturate(kMaxSpeed);
SetSpeeds(wheelSpeeds);
frc::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot};
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.ToRobotRelative(
m_poseEstimator.GetEstimatedPosition().Rotation());
}
SetSpeeds(m_kinematics.ToWheelSpeeds(chassisSpeeds.Discretize(period))
.Desaturate(kMaxSpeed));
}
void Drivetrain::UpdateOdometry() {

13
wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
View File

@@ -8,17 +8,16 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
auto states =
m_kinematics.ToSwerveModuleStates(frc::ChassisSpeeds::Discretize(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},
period));
frc::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot};
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.ToRobotRelative(m_gyro.GetRotation2d());
}
chassisSpeeds = chassisSpeeds.Discretize(period);
auto states = m_kinematics.ToSwerveModuleStates(chassisSpeeds);
m_kinematics.DesaturateWheelSpeeds(&states, kMaxSpeed);
auto [fl, fr, bl, br] = states;
m_frontLeft.SetDesiredState(fl);
m_frontRight.SetDesiredState(fr);
m_backLeft.SetDesiredState(bl);

13
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -53,17 +53,16 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
auto states =
kDriveKinematics.ToSwerveModuleStates(frc::ChassisSpeeds::Discretize(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},
period));
frc::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot};
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.ToRobotRelative(m_gyro.GetRotation2d());
}
chassisSpeeds = chassisSpeeds.Discretize(period);
auto states = kDriveKinematics.ToSwerveModuleStates(chassisSpeeds);
kDriveKinematics.DesaturateWheelSpeeds(&states, AutoConstants::kMaxSpeed);
auto [fl, fr, bl, br] = states;
m_frontLeft.SetDesiredState(fl);
m_frontRight.SetDesiredState(fr);
m_rearLeft.SetDesiredState(bl);

15
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -12,18 +12,17 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
auto states =
m_kinematics.ToSwerveModuleStates(frc::ChassisSpeeds::Discretize(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot,
m_poseEstimator.GetEstimatedPosition().Rotation())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},
period));
frc::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot};
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.ToRobotRelative(
m_poseEstimator.GetEstimatedPosition().Rotation());
}
chassisSpeeds = chassisSpeeds.Discretize(period);
auto states = m_kinematics.ToSwerveModuleStates(chassisSpeeds);
m_kinematics.DesaturateWheelSpeeds(&states, kMaxSpeed);
auto [fl, fr, bl, br] = states;
m_frontLeft.SetDesiredState(fl);
m_frontRight.SetDesiredState(fr);
m_backLeft.SetDesiredState(bl);

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

@@ -129,16 +129,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));
mecanumDriveWheelSpeeds.desaturate(kMaxSpeed);
setSpeeds(mecanumDriveWheelSpeeds);
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.toRobotRelative(m_gyro.getRotation2d());
}
setSpeeds(
m_kinematics.toWheelSpeeds(chassisSpeeds.discretize(periodSeconds)).desaturate(kMaxSpeed));
}
/** Updates the field relative position of the robot. */

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

@@ -141,16 +141,13 @@ 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));
mecanumDriveWheelSpeeds.desaturate(kMaxSpeed);
setSpeeds(mecanumDriveWheelSpeeds);
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds =
chassisSpeeds.toRobotRelative(m_poseEstimator.getEstimatedPosition().getRotation());
}
setSpeeds(
m_kinematics.toWheelSpeeds(chassisSpeeds.discretize(periodSeconds)).desaturate(kMaxSpeed));
}
/** Updates the field relative position of the robot. */

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

@@ -57,19 +57,19 @@ 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));
SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed);
m_frontLeft.setDesiredState(swerveModuleStates[0]);
m_frontRight.setDesiredState(swerveModuleStates[1]);
m_backLeft.setDesiredState(swerveModuleStates[2]);
m_backRight.setDesiredState(swerveModuleStates[3]);
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.toRobotRelative(m_gyro.getRotation2d());
}
chassisSpeeds = chassisSpeeds.discretize(periodSeconds);
var states = m_kinematics.toWheelSpeeds(chassisSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(states, kMaxSpeed);
m_frontLeft.setDesiredState(states[0]);
m_frontRight.setDesiredState(states[1]);
m_backLeft.setDesiredState(states[2]);
m_backRight.setDesiredState(states[3]);
}
/** Updates the field relative position of the robot. */

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

@@ -118,20 +118,19 @@ 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));
SwerveDriveKinematics.desaturateWheelSpeeds(
swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond);
m_frontLeft.setDesiredState(swerveModuleStates[0]);
m_frontRight.setDesiredState(swerveModuleStates[1]);
m_rearLeft.setDesiredState(swerveModuleStates[2]);
m_rearRight.setDesiredState(swerveModuleStates[3]);
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.toRobotRelative(m_gyro.getRotation2d());
}
chassisSpeeds = chassisSpeeds.discretize(DriveConstants.kDrivePeriod);
var states = DriveConstants.kDriveKinematics.toWheelSpeeds(chassisSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(states, DriveConstants.kMaxSpeedMetersPerSecond);
m_frontLeft.setDesiredState(states[0]);
m_frontRight.setDesiredState(states[1]);
m_rearLeft.setDesiredState(states[2]);
m_rearRight.setDesiredState(states[3]);
}
/**

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

@@ -66,19 +66,21 @@ 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));
SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed);
m_frontLeft.setDesiredState(swerveModuleStates[0]);
m_frontRight.setDesiredState(swerveModuleStates[1]);
m_backLeft.setDesiredState(swerveModuleStates[2]);
m_backRight.setDesiredState(swerveModuleStates[3]);
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds =
chassisSpeeds.toRobotRelative(m_poseEstimator.getEstimatedPosition().getRotation());
}
chassisSpeeds = chassisSpeeds.discretize(periodSeconds);
var states = m_kinematics.toWheelSpeeds(chassisSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(states, kMaxSpeed);
m_frontLeft.setDesiredState(states[0]);
m_frontRight.setDesiredState(states[1]);
m_backLeft.setDesiredState(states[2]);
m_backRight.setDesiredState(states[3]);
}
/** Updates the field relative position of the robot. */

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

@@ -104,7 +104,7 @@ public class HolonomicDriveController {
m_rotationError = desiredHeading.minus(currentPose.getRotation());
if (!m_enabled) {
return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation());
return new ChassisSpeeds(xFF, yFF, thetaFF).toRobotRelative(currentPose.getRotation());
}
// Calculate feedback velocities (based on position error).
@@ -112,8 +112,8 @@ 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());
return new ChassisSpeeds(xFF + xFeedback, yFF + yFeedback, thetaFF)
.toRobotRelative(currentPose.getRotation());
}
/**

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

@@ -6,7 +6,6 @@ package edu.wpi.first.math.kinematics;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Seconds;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
@@ -16,7 +15,6 @@ import edu.wpi.first.math.kinematics.proto.ChassisSpeedsProto;
import edu.wpi.first.math.kinematics.struct.ChassisSpeedsStruct;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.LinearVelocity;
import edu.wpi.first.units.measure.Time;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
import java.util.Objects;
@@ -90,7 +88,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
/**
* Discretizes a continuous-time chassis speed.
*
* <p>This function converts a continuous-time chassis speed into a discrete-time one such that
* <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).
@@ -98,19 +96,10 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* <p>This is useful for compensating for translational skew when translating and rotating a
* swerve drivetrain.
*
* @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.
*/
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.
public ChassisSpeeds discretize(double dtSeconds) {
var desiredDeltaPose =
new Pose2d(
vxMetersPerSecond * dtSeconds,
@@ -126,70 +115,14 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
}
/**
* Discretizes a continuous-time chassis speed.
* Converts this field-relative set of speeds into a robot-relative ChassisSpeeds object.
*
* <p>This function converts a 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 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.
*/
public static ChassisSpeeds discretize(
LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Time dt) {
return discretize(
vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), dt.in(Seconds));
}
/**
* Discretizes a continuous-time chassis speed.
*
* <p>This function converts a 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 continuousSpeeds The continuous speeds.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
* @return Discretized ChassisSpeeds.
*/
public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dtSeconds) {
return discretize(
continuousSpeeds.vxMetersPerSecond,
continuousSpeeds.vyMetersPerSecond,
continuousSpeeds.omegaRadiansPerSecond,
dtSeconds);
}
/**
* Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
* object.
*
* @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.
* Positive y is to your left when standing behind the alliance wall.
* @param omegaRadiansPerSecond The angular rate of the robot.
* @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.
*/
public static ChassisSpeeds fromFieldRelativeSpeeds(
double vxMetersPerSecond,
double vyMetersPerSecond,
double omegaRadiansPerSecond,
Rotation2d robotAngle) {
public ChassisSpeeds toRobotRelative(Rotation2d robotAngle) {
// CW rotation into chassis frame
var rotated =
new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle.unaryMinus());
@@ -197,111 +130,19 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
}
/**
* Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
* object.
* Converts this robot-relative set of speeds into a field-relative ChassisSpeeds object.
*
* @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
* your left when standing behind the alliance wall.
* @param omega The angular rate of the robot.
* @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.
*/
public static ChassisSpeeds fromFieldRelativeSpeeds(
LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) {
return fromFieldRelativeSpeeds(
vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), robotAngle);
}
/**
* Converts a user provided field-relative ChassisSpeeds object into a robot-relative
* ChassisSpeeds object.
*
* @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.
*/
public static ChassisSpeeds fromFieldRelativeSpeeds(
ChassisSpeeds fieldRelativeSpeeds, Rotation2d robotAngle) {
return fromFieldRelativeSpeeds(
fieldRelativeSpeeds.vxMetersPerSecond,
fieldRelativeSpeeds.vyMetersPerSecond,
fieldRelativeSpeeds.omegaRadiansPerSecond,
robotAngle);
}
/**
* Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds
* object.
*
* @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.
* Positive y is towards the robot's left.
* @param omegaRadiansPerSecond The angular rate of the robot.
* @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.
*/
public static ChassisSpeeds fromRobotRelativeSpeeds(
double vxMetersPerSecond,
double vyMetersPerSecond,
double omegaRadiansPerSecond,
Rotation2d robotAngle) {
public ChassisSpeeds toFieldRelative(Rotation2d robotAngle) {
// CCW rotation out of chassis frame
var rotated = new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle);
return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond);
}
/**
* Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds
* object.
*
* @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
* towards the robot's left.
* @param omega The angular rate of the robot.
* @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.
*/
public static ChassisSpeeds fromRobotRelativeSpeeds(
LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) {
return fromRobotRelativeSpeeds(
vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), robotAngle);
}
/**
* Converts a user provided robot-relative ChassisSpeeds object into a field-relative
* ChassisSpeeds object.
*
* @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.
*/
public static ChassisSpeeds fromRobotRelativeSpeeds(
ChassisSpeeds robotRelativeSpeeds, Rotation2d robotAngle) {
return fromRobotRelativeSpeeds(
robotRelativeSpeeds.vxMetersPerSecond,
robotRelativeSpeeds.vyMetersPerSecond,
robotRelativeSpeeds.omegaRadiansPerSecond,
robotAngle);
}
/**
* Adds two ChassisSpeeds and returns the sum.
*

27
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
View File

@@ -83,23 +83,27 @@ public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSeri
* absolute threshold, while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
* @return Desaturated MecanumDriveWheelSpeeds.
*/
public void desaturate(double attainableMaxSpeedMetersPerSecond) {
public MecanumDriveWheelSpeeds desaturate(double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed =
Math.max(Math.abs(frontLeftMetersPerSecond), Math.abs(frontRightMetersPerSecond));
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearLeftMetersPerSecond));
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearRightMetersPerSecond));
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
frontLeftMetersPerSecond =
frontLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
frontRightMetersPerSecond =
frontRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
rearLeftMetersPerSecond =
rearLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
rearRightMetersPerSecond =
rearRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
return new MecanumDriveWheelSpeeds(
frontLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond,
frontRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond,
rearLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond,
rearRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond);
}
return new MecanumDriveWheelSpeeds(
frontLeftMetersPerSecond,
frontRightMetersPerSecond,
rearLeftMetersPerSecond,
rearRightMetersPerSecond);
}
/**
@@ -111,9 +115,10 @@ public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSeri
* absolute threshold, while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
* @return Desaturated MecanumDriveWheelSpeeds.
*/
public void desaturate(LinearVelocity attainableMaxSpeed) {
desaturate(attainableMaxSpeed.in(MetersPerSecond));
public MecanumDriveWheelSpeeds desaturate(LinearVelocity attainableMaxSpeed) {
return desaturate(attainableMaxSpeed.in(MetersPerSecond));
}
/**

4
wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java
View File

@@ -52,8 +52,8 @@ public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint {
new ChassisSpeeds(xdVelocity, ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
// Get the wheel speeds and normalize them to within the max velocity.
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
wheelSpeeds.desaturate(m_maxSpeedMetersPerSecond);
var wheelSpeeds =
m_kinematics.toWheelSpeeds(chassisSpeeds).desaturate(m_maxSpeedMetersPerSecond);
// Convert normalized wheel speeds back to chassis speeds
var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);

8
wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
View File

@@ -115,8 +115,8 @@ class WPILIB_DLLEXPORT HolonomicDriveController {
m_rotationError = desiredHeading - currentPose.Rotation();
if (!m_enabled) {
return ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, thetaFF,
currentPose.Rotation());
return ChassisSpeeds{xFF, yFF, thetaFF}.ToRobotRelative(
currentPose.Rotation());
}
// Calculate feedback velocities (based on position error).
@@ -126,8 +126,8 @@ class WPILIB_DLLEXPORT HolonomicDriveController {
currentPose.Y().value(), trajectoryPose.Y().value())};
// Return next output.
return ChassisSpeeds::FromFieldRelativeSpeeds(
xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.Rotation());
return ChassisSpeeds{xFF + xFeedback, yFF + yFeedback, thetaFF}
.ToRobotRelative(currentPose.Rotation());
}
/**

112
wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
View File

@@ -61,17 +61,10 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
* This is useful for compensating for translational skew when translating and
* rotating a swerve drivetrain.
*
* @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.
*/
static constexpr ChassisSpeeds Discretize(units::meters_per_second_t vx,
units::meters_per_second_t vy,
units::radians_per_second_t omega,
units::second_t dt) {
constexpr ChassisSpeeds Discretize(units::second_t dt) const {
// Construct the desired pose after a timestep, relative to the current
// pose. The desired pose has decoupled translation and rotation.
Pose2d desiredDeltaPose{vx * dt, vy * dt, omega * dt};
@@ -85,47 +78,17 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
}
/**
* Discretizes a continuous-time chassis speed.
*
* This function converts a 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).
*
* This is useful for compensating for translational skew when translating and
* rotating a swerve drivetrain.
*
* @param continuousSpeeds The continuous speeds.
* @param dt The duration of the timestep the speeds should be applied for.
*
* @return Discretized ChassisSpeeds.
*/
static constexpr ChassisSpeeds Discretize(
const ChassisSpeeds& continuousSpeeds, units::second_t dt) {
return Discretize(continuousSpeeds.vx, continuousSpeeds.vy,
continuousSpeeds.omega, dt);
}
/**
* Converts a user provided field-relative set of speeds into a robot-relative
* Converts this field-relative set of speeds into a robot-relative
* ChassisSpeeds object.
*
* @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 your left when standing behind the alliance wall.
* @param omega The angular rate of the robot.
* @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.
*
* 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.
* of reference.
*/
static constexpr ChassisSpeeds FromFieldRelativeSpeeds(
units::meters_per_second_t vx, units::meters_per_second_t vy,
units::radians_per_second_t omega, const Rotation2d& robotAngle) {
constexpr ChassisSpeeds ToRobotRelative(const Rotation2d& robotAngle) const {
// CW rotation into chassis frame
auto rotated =
Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
@@ -135,45 +98,17 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
}
/**
* Converts a user provided field-relative ChassisSpeeds object into a
* robot-relative ChassisSpeeds object.
*
* @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.
*/
static constexpr ChassisSpeeds FromFieldRelativeSpeeds(
const ChassisSpeeds& fieldRelativeSpeeds, const Rotation2d& robotAngle) {
return FromFieldRelativeSpeeds(fieldRelativeSpeeds.vx,
fieldRelativeSpeeds.vy,
fieldRelativeSpeeds.omega, robotAngle);
}
/**
* Converts a user provided robot-relative set of speeds into a field-relative
* Converts this robot-relative set of speeds into a field-relative
* ChassisSpeeds object.
*
* @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 towards the robot's left.
* @param omega The angular rate of the robot.
* @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.
*
* 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.
* of reference.
*/
static constexpr ChassisSpeeds FromRobotRelativeSpeeds(
units::meters_per_second_t vx, units::meters_per_second_t vy,
units::radians_per_second_t omega, const Rotation2d& robotAngle) {
constexpr ChassisSpeeds ToFieldRelative(const Rotation2d& robotAngle) const {
// CCW rotation out of chassis frame
auto rotated =
Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
@@ -182,27 +117,6 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
units::meters_per_second_t{rotated.Y().value()}, omega};
}
/**
* Converts a user provided robot-relative ChassisSpeeds object into a
* field-relative ChassisSpeeds object.
*
* @param robotRelativeSpeeds The ChassisSpeeds object representing the speeds
* in the robot frame of reference. Positive x is the towards 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.
*/
static constexpr ChassisSpeeds FromRobotRelativeSpeeds(
const ChassisSpeeds& robotRelativeSpeeds, const Rotation2d& robotAngle) {
return FromRobotRelativeSpeeds(robotRelativeSpeeds.vx,
robotRelativeSpeeds.vy,
robotRelativeSpeeds.omega, robotAngle);
}
/**
* Adds two ChassisSpeeds and returns the sum.
*

12
wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
View File

@@ -49,8 +49,10 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelSpeeds {
* threshold, while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
* @return Desaturated MecanumDriveWheelSpeeds.
*/
constexpr void Desaturate(units::meters_per_second_t attainableMaxSpeed) {
constexpr MecanumDriveWheelSpeeds Desaturate(
units::meters_per_second_t attainableMaxSpeed) const {
std::array<units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
rearLeft, rearRight};
units::meters_per_second_t realMaxSpeed = units::math::abs(
@@ -63,11 +65,11 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelSpeeds {
for (int i = 0; i < 4; ++i) {
wheelSpeeds[i] = wheelSpeeds[i] / realMaxSpeed * attainableMaxSpeed;
}
frontLeft = wheelSpeeds[0];
frontRight = wheelSpeeds[1];
rearLeft = wheelSpeeds[2];
rearRight = wheelSpeeds[3];
return MecanumDriveWheelSpeeds{wheelSpeeds[0], wheelSpeeds[1],
wheelSpeeds[2], wheelSpeeds[3]};
}
return *this;
}
/**

6
wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
View File

@@ -32,9 +32,9 @@ class WPILIB_DLLEXPORT MecanumDriveKinematicsConstraint
units::meters_per_second_t velocity) const override {
auto xVelocity = velocity * pose.Rotation().Cos();
auto yVelocity = velocity * pose.Rotation().Sin();
auto wheelSpeeds = m_kinematics.ToWheelSpeeds(
{xVelocity, yVelocity, velocity * curvature});
wheelSpeeds.Desaturate(m_maxSpeed);
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds({xVelocity, yVelocity, velocity * curvature})
.Desaturate(m_maxSpeed);
auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);

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

@@ -23,7 +23,7 @@ class ChassisSpeedsTest {
final var duration = 1.0;
final var dt = 0.01;
final var speeds = ChassisSpeeds.discretize(target, duration);
final var speeds = target.discretize(duration);
final var twist =
new Twist2d(
speeds.vxMetersPerSecond * dt,
@@ -60,9 +60,8 @@ class ChassisSpeedsTest {
}
@Test
void testFromFieldRelativeSpeeds() {
final var chassisSpeeds =
ChassisSpeeds.fromFieldRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.kCW_Pi_2);
void testToRobotRelative() {
final var chassisSpeeds = new ChassisSpeeds(1.0, 0.0, 0.5).toRobotRelative(Rotation2d.kCW_Pi_2);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
@@ -71,9 +70,9 @@ class ChassisSpeedsTest {
}
@Test
void testFromRobotRelativeSpeeds() {
void testToFieldRelative() {
final var chassisSpeeds =
ChassisSpeeds.fromRobotRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(45.0));
new ChassisSpeeds(1.0, 0.0, 0.5).toFieldRelative(Rotation2d.fromDegrees(45.0));
assertAll(
() -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vxMetersPerSecond, kEpsilon),

6
wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
View File

@@ -227,8 +227,7 @@ class MecanumDriveKinematicsTest {
@Test
void testDesaturate() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7);
wheelSpeeds.desaturate(5.5);
var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7).desaturate(5.5);
double factor = 5.5 / 7.0;
@@ -241,8 +240,7 @@ class MecanumDriveKinematicsTest {
@Test
void testDesaturateNegativeSpeeds() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(-5, 6, 4, -7);
wheelSpeeds.desaturate(5.5);
var wheelSpeeds = new MecanumDriveWheelSpeeds(-5, 6, 4, -7).desaturate(5.5);
final double kFactor = 5.5 / 7.0;

15
wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
View File

@@ -15,7 +15,7 @@ TEST(ChassisSpeedsTest, Discretize) {
constexpr units::second_t duration = 1_s;
constexpr units::second_t dt = 10_ms;
const auto speeds = frc::ChassisSpeeds::Discretize(target, duration);
const auto speeds = target.Discretize(duration);
const frc::Twist2d twist{speeds.vx * dt, speeds.vy * dt, speeds.omega * dt};
frc::Pose2d pose;
@@ -29,18 +29,19 @@ TEST(ChassisSpeedsTest, Discretize) {
pose.Rotation().Radians().value(), kEpsilon);
}
TEST(ChassisSpeedsTest, FromFieldRelativeSpeeds) {
const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
1.0_mps, 0.0_mps, 0.5_rad_per_s, -90.0_deg);
TEST(ChassisSpeedsTest, ToRobotRelative) {
const auto chassisSpeeds =
frc::ChassisSpeeds{1_mps, 0_mps, 0.5_rad_per_s}.ToRobotRelative(
-90.0_deg);
EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), kEpsilon);
EXPECT_NEAR(1.0, chassisSpeeds.vy.value(), kEpsilon);
EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon);
}
TEST(ChassisSpeedsTest, FromRobotRelativeSpeeds) {
const auto chassisSpeeds = frc::ChassisSpeeds::FromRobotRelativeSpeeds(
1.0_mps, 0.0_mps, 0.5_rad_per_s, 45.0_deg);
TEST(ChassisSpeedsTest, ToFieldRelative) {
const auto chassisSpeeds =
frc::ChassisSpeeds{1_mps, 0_mps, 0.5_rad_per_s}.ToFieldRelative(45.0_deg);
EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vx.value(), kEpsilon);
EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vy.value(), kEpsilon);

8
wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
View File

@@ -205,8 +205,8 @@ TEST_F(MecanumDriveKinematicsTest,
}
TEST_F(MecanumDriveKinematicsTest, Desaturate) {
MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps};
wheelSpeeds.Desaturate(5.5_mps);
auto wheelSpeeds =
MecanumDriveWheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps}.Desaturate(5.5_mps);
double kFactor = 5.5 / 7.0;
@@ -217,8 +217,8 @@ TEST_F(MecanumDriveKinematicsTest, Desaturate) {
}
TEST_F(MecanumDriveKinematicsTest, DesaturateNegativeSpeeds) {
MecanumDriveWheelSpeeds wheelSpeeds{-5_mps, 6_mps, 4_mps, -7_mps};
wheelSpeeds.Desaturate(5.5_mps);
auto wheelSpeeds =
MecanumDriveWheelSpeeds{-5_mps, 6_mps, 4_mps, -7_mps}.Desaturate(5.5_mps);
constexpr double kFactor = 5.5 / 7.0;