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] Add ChassisSpeeds method to fix drifting during compound swerve drive maneuvers (#5425)

Browse Source
This commit is contained in:
Joseph Eng
2023-07-18 21:19:55 -07:00
committed by GitHub
parent 1af224c21b
commit 657338715d
28 changed files with 227 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -49,11 +49,14 @@ void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) {
void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot, bool fieldRelative) {
auto wheelSpeeds = m_kinematics.ToWheelSpeeds(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot});
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds(frc::ChassisSpeeds::FromDiscreteSpeeds(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},
period));
wheelSpeeds.Desaturate(kMaxSpeed);
SetSpeeds(wheelSpeeds);
}

2
wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp
View File

@@ -46,7 +46,7 @@ class Robot : public frc::TimedRobot {
const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
Drivetrain::kMaxAngularSpeed;
m_mecanum.Drive(xSpeed, ySpeed, rot, fieldRelative);
m_mecanum.Drive(xSpeed, ySpeed, rot, fieldRelative, GetPeriod());
}
};

2
wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
View File

@@ -35,7 +35,7 @@ class Drivetrain {
void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds);
void Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
bool fieldRelative);
bool fieldRelative, units::second_t period);
void UpdateOdometry();
static constexpr units::meters_per_second_t kMaxSpeed =

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

@@ -46,11 +46,14 @@ void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) {
void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot, bool fieldRelative) {
auto wheelSpeeds = m_kinematics.ToWheelSpeeds(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot});
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds(frc::ChassisSpeeds::FromDiscreteSpeeds(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},
period));
wheelSpeeds.Desaturate(kMaxSpeed);
SetSpeeds(wheelSpeeds);
}

2
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Robot.cpp
View File

@@ -46,7 +46,7 @@ class Robot : public frc::TimedRobot {
const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
Drivetrain::kMaxAngularSpeed;
m_mecanum.Drive(xSpeed, ySpeed, rot, fieldRelative);
m_mecanum.Drive(xSpeed, ySpeed, rot, fieldRelative, GetPeriod());
}
};

2
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h
View File

@@ -36,7 +36,7 @@ class Drivetrain {
void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds);
void Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
bool fieldRelative);
bool fieldRelative, units::second_t period);
void UpdateOdometry();
static constexpr auto kMaxSpeed = 3.0_mps; // 3 meters per second

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

@@ -6,11 +6,14 @@
void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot, bool fieldRelative) {
auto states = m_kinematics.ToSwerveModuleStates(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot});
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
auto states =
m_kinematics.ToSwerveModuleStates(frc::ChassisSpeeds::FromDiscreteSpeeds(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},
period));
m_kinematics.DesaturateWheelSpeeds(&states, kMaxSpeed);

2
wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp
View File

@@ -50,7 +50,7 @@ class Robot : public frc::TimedRobot {
frc::ApplyDeadband(m_controller.GetRightX(), 0.02)) *
Drivetrain::kMaxAngularSpeed;
m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative);
m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative, GetPeriod());
}
};

2
wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
View File

@@ -22,7 +22,7 @@ class Drivetrain {
void Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
bool fieldRelative);
bool fieldRelative, units::second_t period);
void UpdateOdometry();
static constexpr units::meters_per_second_t kMaxSpeed =

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

@@ -51,12 +51,14 @@ void DriveSubsystem::Periodic() {
void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot,
bool fieldRelative) {
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
auto states = kDriveKinematics.ToSwerveModuleStates(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot});
frc::ChassisSpeeds::FromDiscreteSpeeds(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},
period));
kDriveKinematics.DesaturateWheelSpeeds(&states, AutoConstants::kMaxSpeed);

5
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
View File

@@ -4,6 +4,7 @@
#include <numbers>
#include <frc/TimedRobot.h>
#include <frc/geometry/Translation2d.h>
#include <frc/kinematics/SwerveDriveKinematics.h>
#include <frc/trajectory/TrapezoidProfile.h>
@@ -58,6 +59,10 @@ constexpr bool kRearLeftDriveEncoderReversed = true;
constexpr bool kFrontRightDriveEncoderReversed = false;
constexpr bool kRearRightDriveEncoderReversed = true;
// If you call DriveSubsystem::Drive with a different period make sure to update
// this.
constexpr units::second_t kDrivePeriod = frc::TimedRobot::kDefaultPeriod;
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
// These characterization values MUST be determined either experimentally or
// theoretically for *your* robot's drive. The SysId tool provides a convenient

3
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h
View File

@@ -43,7 +43,8 @@ class DriveSubsystem : public frc2::Subsystem {
*/
void Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
bool fieldRelative);
bool fieldRelative,
units::second_t period = DriveConstants::kDrivePeriod);
/**
* Resets the drive encoders to currently read a position of 0.

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

@@ -10,11 +10,14 @@
void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot, bool fieldRelative) {
auto states = m_kinematics.ToSwerveModuleStates(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot});
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
auto states =
m_kinematics.ToSwerveModuleStates(frc::ChassisSpeeds::FromDiscreteSpeeds(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},
period));
m_kinematics.DesaturateWheelSpeeds(&states, kMaxSpeed);

2
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Robot.cpp
View File

@@ -46,7 +46,7 @@ class Robot : public frc::TimedRobot {
const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
Drivetrain::kMaxAngularSpeed;
m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative);
m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative, GetPeriod());
}
};

2
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h
View File

@@ -23,7 +23,7 @@ class Drivetrain {
void Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
bool fieldRelative);
bool fieldRelative, units::second_t period);
void UpdateOdometry();
static constexpr auto kMaxSpeed = 3.0_mps; // 3 meters per second

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

@@ -128,12 +128,16 @@ public class Drivetrain {
* @param rot Angular rate of the robot.
* @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) {
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var mecanumDriveWheelSpeeds =
m_kinematics.toWheelSpeeds(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot));
ChassisSpeeds.fromDiscreteSpeeds(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot),
periodSeconds));
mecanumDriveWheelSpeeds.desaturate(kMaxSpeed);
setSpeeds(mecanumDriveWheelSpeeds);
}

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java
View File

@@ -44,6 +44,6 @@ public class Robot extends TimedRobot {
// the right by default.
final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative);
m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative, getPeriod());
}
}

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

@@ -140,12 +140,16 @@ public class Drivetrain {
* @param rot Angular rate of the robot.
* @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) {
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var mecanumDriveWheelSpeeds =
m_kinematics.toWheelSpeeds(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot));
ChassisSpeeds.fromDiscreteSpeeds(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot),
periodSeconds));
mecanumDriveWheelSpeeds.desaturate(kMaxSpeed);
setSpeeds(mecanumDriveWheelSpeeds);
}

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Robot.java
View File

@@ -44,6 +44,6 @@ public class Robot extends TimedRobot {
// the right by default.
final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative);
m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative, getPeriod());
}
}

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

@@ -55,12 +55,16 @@ public class Drivetrain {
* @param rot Angular rate of the robot.
* @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) {
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var swerveModuleStates =
m_kinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot));
ChassisSpeeds.fromDiscreteSpeeds(
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]);

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java
View File

@@ -51,6 +51,6 @@ public class Robot extends TimedRobot {
-m_rotLimiter.calculate(MathUtil.applyDeadband(m_controller.getRightX(), 0.02))
* Drivetrain.kMaxAngularSpeed;
m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative);
m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative, getPeriod());
}
}

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java
View File

@@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj.examples.swervecontrollercommand;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.TimedRobot;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -48,6 +49,9 @@ public final class Constants {
public static final boolean kFrontRightDriveEncoderReversed = false;
public static final boolean kRearRightDriveEncoderReversed = true;
// If you call DriveSubsystem.drive() with a different period make sure to update this.
public static final double kDrivePeriod = TimedRobot.kDefaultPeriod;
public static final double kTrackWidth = 0.5;
// Distance between centers of right and left wheels on robot
public static final double kWheelBase = 0.7;

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

@@ -120,9 +120,12 @@ public class DriveSubsystem extends Subsystem {
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
var swerveModuleStates =
DriveConstants.kDriveKinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot));
ChassisSpeeds.fromDiscreteSpeeds(
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]);

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

@@ -64,12 +64,16 @@ public class Drivetrain {
* @param rot Angular rate of the robot.
* @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) {
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
var swerveModuleStates =
m_kinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeed, ySpeed, rot));
ChassisSpeeds.fromDiscreteSpeeds(
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]);

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Robot.java
View File

@@ -44,6 +44,6 @@ public class Robot extends TimedRobot {
// the right by default.
final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative);
m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative, getPeriod());
}
}

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

@@ -4,6 +4,7 @@
package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
/**
@@ -43,6 +44,54 @@ public class ChassisSpeeds {
this.omegaRadiansPerSecond = omegaRadiansPerSecond;
}
/**
* Converts from a chassis speed for a discrete timestep into chassis speed for continuous time.
*
* <p>The difference between applying a chassis speed for a discrete timestep vs. continuously is
* that applying for a discrete timestep is just scaling the velocity components by the time and
* adding, while when applying continuously the changes to the heading affect the direction the
* translational components are applied to relative to the field.
*
* @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 ChassisSpeeds that can be applied continuously to produce the discrete chassis speeds.
*/
public static ChassisSpeeds fromDiscreteSpeeds(
double vxMetersPerSecond,
double vyMetersPerSecond,
double omegaRadiansPerSecond,
double dtSeconds) {
var desiredDeltaPose =
new Pose2d(
vxMetersPerSecond * dtSeconds,
vyMetersPerSecond * dtSeconds,
new Rotation2d(omegaRadiansPerSecond * dtSeconds));
var twist = new Pose2d().log(desiredDeltaPose);
return new ChassisSpeeds(twist.dx / dtSeconds, twist.dy / dtSeconds, twist.dtheta / dtSeconds);
}
/**
* Converts from a chassis speed for a discrete timestep into chassis speed for continuous time.
*
* <p>The difference between applying a chassis speed for a discrete timestep vs. continuously is
* that applying for a discrete timestep is just scaling the velocity components by the time and
* adding, while when applying continuously the changes to the heading affect the direction the
* translational components are applied to relative to the field.
*
* @param discreteSpeeds The speeds for a discrete timestep.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
* @return ChassisSpeeds that can be applied continuously to produce the discrete chassis speeds.
*/
public static ChassisSpeeds fromDiscreteSpeeds(ChassisSpeeds discreteSpeeds, double dtSeconds) {
return fromDiscreteSpeeds(
discreteSpeeds.vxMetersPerSecond,
discreteSpeeds.vyMetersPerSecond,
discreteSpeeds.omegaRadiansPerSecond,
dtSeconds);
}
/**
* Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
* object.

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

@@ -6,6 +6,7 @@
#include <wpi/SymbolExports.h>
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "units/angular_velocity.h"
#include "units/velocity.h"
@@ -38,6 +39,55 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
*/
units::radians_per_second_t omega = 0_rad_per_s;
/**
* Converts from a chassis speed for a discrete timestep into chassis speed
* for continuous time.
*
* The difference between applying a chassis speed for a discrete timestep vs.
* continuously is that applying for a discrete timestep is just scaling the
* velocity components by the time and adding, while when applying
* continuously the changes to the heading affect the direction the
* translational components are applied to relative to the field.
*
* @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 ChassisSpeeds that can be applied continuously to produce the
* discrete ChassisSpeeds.
*/
static ChassisSpeeds FromDiscreteSpeeds(units::meters_per_second_t vx,
units::meters_per_second_t vy,
units::radians_per_second_t omega,
units::second_t dt) {
Pose2d desiredDeltaPose{vx * dt, vy * dt, omega * dt};
auto twist = Pose2d{}.Log(desiredDeltaPose);
return {twist.dx / dt, twist.dy / dt, twist.dtheta / dt};
}
/**
* Converts from a chassis speed for a discrete timestep into chassis speed
* for continuous time.
*
* The difference between applying a chassis speed for a discrete timestep vs.
* continuously is that applying for a discrete timestep is just scaling the
* velocity components by the time and adding, while when applying
* continuously the changes to the heading affect the direction the
* translational components are applied to relative to the field.
*
* @param discreteSpeeds The speeds for a discrete timestep.
* @param dt The duration of the timestep the speeds should be applied for.
*
* @return ChassisSpeeds that can be applied continuously to produce the
* discrete ChassisSpeeds.
*/
static ChassisSpeeds FromDiscreteSpeeds(const ChassisSpeeds& discreteSpeeds,
units::second_t dt) {
return FromDiscreteSpeeds(discreteSpeeds.vx, discreteSpeeds.vy,
discreteSpeeds.omega, dt);
}
/**
* Converts a user provided field-relative set of speeds into a robot-relative
* ChassisSpeeds object.

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

@@ -7,12 +7,40 @@ package edu.wpi.first.math.kinematics;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Twist2d;
import org.junit.jupiter.api.Test;
class ChassisSpeedsTest {
private static final double kEpsilon = 1E-9;
@Test
void testVeeringCorrection() {
final var duration = 1.0; // duration of observation
final var dt = 0.01; // time increment for simulation
final var target = new ChassisSpeeds(1.0, 0.0, 0.5);
final var speeds = ChassisSpeeds.fromDiscreteSpeeds(target, duration);
final var twist =
new Twist2d(
speeds.vxMetersPerSecond * dt,
speeds.vyMetersPerSecond * dt,
speeds.omegaRadiansPerSecond * dt);
var pose = new Pose2d();
for (double time = 0; time < duration; time += dt) {
pose = pose.exp(twist);
}
final var result = pose; // For lambda capture
assertAll(
() -> assertEquals(target.vxMetersPerSecond * duration, result.getX(), kEpsilon),
() -> assertEquals(target.vyMetersPerSecond * duration, result.getY(), kEpsilon),
() ->
assertEquals(
target.omegaRadiansPerSecond * duration,
result.getRotation().getRadians(),
kEpsilon));
}
@Test
void testFieldRelativeConstruction() {
final var chassisSpeeds =