Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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

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);