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

[wpimath] Replace Speeds with Velocities (#8479)

Browse Source 
I left "free speed" alone since that's the technical term for it. In
general, velocity is a vector quantity, and speed is a magnitude (i.e.,
a strictly positive value).

This PR also replaces the speed verbiage in MotorController with duty
cycle.

Fixes #8423.
This commit is contained in:
Tyler Veness
2026-03-06 14:19:15 -08:00
committed by GitHub
parent 1e39f39128
commit 9bd9656871
594 changed files with 8073 additions and 7875 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpimath/src/main/native/include/wpi/math/controller/BangBangController.hpp
View File

@@ -23,7 +23,7 @@ namespace wpi::math {
*
* <p>Note that this is an *asymmetric* bang-bang controller - it will not exert
* any control effort in the reverse direction (e.g. it won't try to slow down
* an over-speeding shooter wheel). This asymmetry is *extremely important.*
* an overspeeding shooter wheel). This asymmetry is *extremely important*.
* Bang-bang control is extremely simple, but also potentially hazardous. Always
* ensure that your motor controllers are set to "coast" before attempting to
* control them with a bang-bang controller.

13
wpimath/src/main/native/include/wpi/math/controller/LTVUnicycleController.hpp
View File

@@ -7,7 +7,7 @@
#include <Eigen/Core>
#include "wpi/math/geometry/Pose2d.hpp"
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include "wpi/math/trajectory/Trajectory.hpp"
#include "wpi/math/util/StateSpaceUtil.hpp"
#include "wpi/units/angular_velocity.hpp"
@@ -104,9 +104,10 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
* @param linearVelocityRef The desired linear velocity.
* @param angularVelocityRef The desired angular velocity.
*/
ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
wpi::units::meters_per_second_t linearVelocityRef,
wpi::units::radians_per_second_t angularVelocityRef);
ChassisVelocities Calculate(
const Pose2d& currentPose, const Pose2d& poseRef,
wpi::units::meters_per_second_t linearVelocityRef,
wpi::units::radians_per_second_t angularVelocityRef);
/**
* Returns the linear and angular velocity outputs of the LTV controller.
@@ -118,8 +119,8 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
* @param desiredState The desired pose, linear velocity, and angular velocity
* from a trajectory.
*/
ChassisSpeeds Calculate(const Pose2d& currentPose,
const Trajectory::State& desiredState) {
ChassisVelocities Calculate(const Pose2d& currentPose,
const Trajectory::State& desiredState) {
return Calculate(currentPose, desiredState.pose, desiredState.velocity,
desiredState.velocity * desiredState.curvature);
}

2
wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator.hpp
View File

@@ -31,7 +31,7 @@ namespace wpi::math {
*/
class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
: public PoseEstimator<DifferentialDriveWheelPositions,
DifferentialDriveWheelSpeeds,
DifferentialDriveWheelVelocities,
DifferentialDriveWheelAccelerations> {
public:
/**

2
wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator3d.hpp
View File

@@ -35,7 +35,7 @@ namespace wpi::math {
*/
class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d
: public PoseEstimator3d<DifferentialDriveWheelPositions,
DifferentialDriveWheelSpeeds,
DifferentialDriveWheelVelocities,
DifferentialDriveWheelAccelerations> {
public:
/**

3
wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator.hpp
View File

@@ -28,7 +28,8 @@ namespace wpi::math {
* odometry.
*/
class WPILIB_DLLEXPORT MecanumDrivePoseEstimator
: public PoseEstimator<MecanumDriveWheelPositions, MecanumDriveWheelSpeeds,
: public PoseEstimator<MecanumDriveWheelPositions,
MecanumDriveWheelVelocities,
MecanumDriveWheelAccelerations> {
public:
/**

2
wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator3d.hpp
View File

@@ -33,7 +33,7 @@ namespace wpi::math {
*/
class WPILIB_DLLEXPORT MecanumDrivePoseEstimator3d
: public PoseEstimator3d<MecanumDriveWheelPositions,
MecanumDriveWheelSpeeds,
MecanumDriveWheelVelocities,
MecanumDriveWheelAccelerations> {
public:
/**

11
wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp
View File

@@ -39,10 +39,10 @@ namespace wpi::math {
* never call it, then this class will behave like regular encoder odometry.
*
* @tparam WheelPositions Wheel positions type.
* @tparam WheelSpeeds Wheel speeds type.
* @tparam WheelVelocities Wheel velocities type.
* @tparam WheelAccelerations Wheel accelerations type.
*/
template <typename WheelPositions, typename WheelSpeeds,
template <typename WheelPositions, typename WheelVelocities,
typename WheelAccelerations>
class WPILIB_DLLEXPORT PoseEstimator {
public:
@@ -64,8 +64,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
* less.
*/
PoseEstimator(
Kinematics<WheelPositions, WheelSpeeds, WheelAccelerations>& kinematics,
Odometry<WheelPositions, WheelSpeeds, WheelAccelerations>& odometry,
Kinematics<WheelPositions, WheelVelocities, WheelAccelerations>&
kinematics,
Odometry<WheelPositions, WheelVelocities, WheelAccelerations>& odometry,
const wpi::util::array<double, 3>& stateStdDevs,
const wpi::util::array<double, 3>& visionMeasurementStdDevs)
: m_odometry(odometry) {
@@ -478,7 +479,7 @@ class WPILIB_DLLEXPORT PoseEstimator {
static constexpr wpi::units::second_t kBufferDuration = 1.5_s;
Odometry<WheelPositions, WheelSpeeds, WheelAccelerations>& m_odometry;
Odometry<WheelPositions, WheelVelocities, WheelAccelerations>& m_odometry;
// Diagonal of process noise covariance matrix Q
wpi::util::array<double, 3> m_q{wpi::util::empty_array};

11
wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp
View File

@@ -44,10 +44,10 @@ namespace wpi::math {
* never call it, then this class will behave like regular encoder odometry.
*
* @tparam WheelPositions Wheel positions type.
* @tparam WheelSpeeds Wheel speeds type.
* @tparam WheelVelocities Wheel velocities type.
* @tparam WheelAccelerations Wheel accelerations type.
*/
template <typename WheelPositions, typename WheelSpeeds,
template <typename WheelPositions, typename WheelVelocities,
typename WheelAccelerations>
class WPILIB_DLLEXPORT PoseEstimator3d {
public:
@@ -69,8 +69,9 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
* vision pose measurement less.
*/
PoseEstimator3d(
Kinematics<WheelPositions, WheelSpeeds, WheelAccelerations>& kinematics,
Odometry3d<WheelPositions, WheelSpeeds, WheelAccelerations>& odometry,
Kinematics<WheelPositions, WheelVelocities, WheelAccelerations>&
kinematics,
Odometry3d<WheelPositions, WheelVelocities, WheelAccelerations>& odometry,
const wpi::util::array<double, 4>& stateStdDevs,
const wpi::util::array<double, 4>& visionMeasurementStdDevs)
: m_odometry(odometry) {
@@ -492,7 +493,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
static constexpr wpi::units::second_t kBufferDuration = 1.5_s;
Odometry3d<WheelPositions, WheelSpeeds, WheelAccelerations>& m_odometry;
Odometry3d<WheelPositions, WheelVelocities, WheelAccelerations>& m_odometry;
// Diagonal of process noise covariance matrix Q
wpi::util::array<double, 4> m_q{wpi::util::empty_array};

2
wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp
View File

@@ -29,7 +29,7 @@ template <size_t NumModules>
class SwerveDrivePoseEstimator
: public PoseEstimator<
wpi::util::array<SwerveModulePosition, NumModules>,
wpi::util::array<SwerveModuleState, NumModules>,
wpi::util::array<SwerveModuleVelocity, NumModules>,
wpi::util::array<SwerveModuleAcceleration, NumModules>> {
public:
/**

2
wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator3d.hpp
View File

@@ -33,7 +33,7 @@ template <size_t NumModules>
class SwerveDrivePoseEstimator3d
: public PoseEstimator3d<
wpi::util::array<SwerveModulePosition, NumModules>,
wpi::util::array<SwerveModuleState, NumModules>,
wpi::util::array<SwerveModuleVelocity, NumModules>,
wpi::util::array<SwerveModuleAcceleration, NumModules>> {
public:
/**

13
wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp
View File

@@ -12,10 +12,7 @@
namespace wpi::math {
/**
* Represents the acceleration of a robot chassis. Although this struct contains
* similar members compared to a ChassisSpeeds, they do NOT represent the same
* thing. Whereas a ChassisSpeeds object represents a robot's velocity, a
* ChassisAccelerations object represents a robot's acceleration.
* Represents robot chassis accelerations.
*
* A strictly non-holonomic drivetrain, such as a differential drive, should
* never have an ay component because it can never move sideways. Holonomic
@@ -82,7 +79,7 @@ struct WPILIB_DLLEXPORT ChassisAccelerations {
/**
* Adds two ChassisAccelerations and returns the sum.
*
* <p>For example, ChassisAccelerations{1.0, 0.5, 1.5} +
* For example, ChassisAccelerations{1.0, 0.5, 1.5} +
* ChassisAccelerations{2.0, 1.5, 0.5} = ChassisAccelerations{3.0, 2.0, 2.0}
*
* @param other The ChassisAccelerations to add.
@@ -98,7 +95,7 @@ struct WPILIB_DLLEXPORT ChassisAccelerations {
* Subtracts the other ChassisAccelerations from the current
* ChassisAccelerations and returns the difference.
*
* <p>For example, ChassisAccelerations{5.0, 4.0, 2.0} -
* For example, ChassisAccelerations{5.0, 4.0, 2.0} -
* ChassisAccelerations{1.0, 2.0, 1.0} = ChassisAccelerations{4.0, 2.0, 1.0}
*
* @param other The ChassisAccelerations to subtract.
@@ -124,7 +121,7 @@ struct WPILIB_DLLEXPORT ChassisAccelerations {
* Multiplies the ChassisAccelerations by a scalar and returns the new
* ChassisAccelerations.
*
* <p>For example, ChassisAccelerations{2.0, 2.5, 1.0} * 2
* For example, ChassisAccelerations{2.0, 2.5, 1.0} * 2
* = ChassisAccelerations{4.0, 5.0, 2.0}
*
* @param scalar The scalar to multiply by.
@@ -139,7 +136,7 @@ struct WPILIB_DLLEXPORT ChassisAccelerations {
* Divides the ChassisAccelerations by a scalar and returns the new
* ChassisAccelerations.
*
* <p>For example, ChassisAccelerations{2.0, 2.5, 1.0} / 2
* For example, ChassisAccelerations{2.0, 2.5, 1.0} / 2
* = ChassisAccelerations{1.0, 1.25, 0.5}
*
* @param scalar The scalar to divide by.

200
wpimath/src/main/native/include/wpi/math/kinematics/ChassisSpeeds.hpp
View File

@@ -1,200 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "wpi/math/geometry/Pose2d.hpp"
#include "wpi/math/geometry/Rotation2d.hpp"
#include "wpi/units/angular_velocity.hpp"
#include "wpi/units/velocity.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace wpi::math {
/**
* Represents the speed of a robot chassis. Although this struct contains
* similar members compared to a Twist2d, they do NOT represent the same thing.
* Whereas a Twist2d represents a change in pose w.r.t to the robot frame of
* reference, a ChassisSpeeds struct represents a robot's velocity.
*
* A strictly non-holonomic drivetrain, such as a differential drive, should
* never have a dy component because it can never move sideways. Holonomic
* drivetrains such as swerve and mecanum will often have all three components.
*/
struct WPILIB_DLLEXPORT ChassisSpeeds {
/**
* Velocity along the x-axis. (Fwd is +)
*/
wpi::units::meters_per_second_t vx = 0_mps;
/**
* Velocity along the y-axis. (Left is +)
*/
wpi::units::meters_per_second_t vy = 0_mps;
/**
* Represents the angular velocity of the robot frame. (CCW is +)
*/
wpi::units::radians_per_second_t omega = 0_rad_per_s;
/**
* Creates a Twist2d from ChassisSpeeds.
*
* @param dt The duration of the timestep.
*
* @return Twist2d.
*/
constexpr Twist2d ToTwist2d(wpi::units::second_t dt) const {
return Twist2d{vx * dt, vy * dt, omega * dt};
}
/**
* 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 holonomic (swerve or mecanum) drivetrain. However, scaling down
* the ChassisSpeeds after discretizing (e.g., when desaturating swerve module
* speeds) rotates the direction of net motion in the opposite direction of
* rotational velocity, introducing a different translational skew which is
* not accounted for by discretization.
*
* @param dt The duration of the timestep the speeds should be applied for.
* @return Discretized ChassisSpeeds.
*/
constexpr ChassisSpeeds Discretize(wpi::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.
Transform2d desiredTransform{vx * dt, vy * dt, omega * dt};
// Find the chassis translation/rotation deltas in the robot frame that move
// the robot from its current pose to the desired pose
auto twist = desiredTransform.Log();
// Turn the chassis translation/rotation deltas into average velocities
return {twist.dx / dt, twist.dy / dt, twist.dtheta / dt};
}
/**
* 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.
* @return ChassisSpeeds object representing the speeds in the robot's frame
* of reference.
*/
constexpr ChassisSpeeds ToRobotRelative(const Rotation2d& robotAngle) const {
// CW rotation into chassis frame
auto rotated = Translation2d{wpi::units::meter_t{vx.value()},
wpi::units::meter_t{vy.value()}}
.RotateBy(-robotAngle);
return {wpi::units::meters_per_second_t{rotated.X().value()},
wpi::units::meters_per_second_t{rotated.Y().value()}, omega};
}
/**
* 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.
* @return ChassisSpeeds object representing the speeds in the field's frame
* of reference.
*/
constexpr ChassisSpeeds ToFieldRelative(const Rotation2d& robotAngle) const {
// CCW rotation out of chassis frame
auto rotated = Translation2d{wpi::units::meter_t{vx.value()},
wpi::units::meter_t{vy.value()}}
.RotateBy(robotAngle);
return {wpi::units::meters_per_second_t{rotated.X().value()},
wpi::units::meters_per_second_t{rotated.Y().value()}, omega};
}
/**
* Adds two ChassisSpeeds and returns the sum.
*
* <p>For example, ChassisSpeeds{1.0, 0.5, 1.5} + ChassisSpeeds{2.0, 1.5, 0.5}
* = ChassisSpeeds{3.0, 2.0, 2.0}
*
* @param other The ChassisSpeeds to add.
*
* @return The sum of the ChassisSpeeds.
*/
constexpr ChassisSpeeds operator+(const ChassisSpeeds& other) const {
return {vx + other.vx, vy + other.vy, omega + other.omega};
}
/**
* Subtracts the other ChassisSpeeds from the current ChassisSpeeds and
* returns the difference.
*
* <p>For example, ChassisSpeeds{5.0, 4.0, 2.0} - ChassisSpeeds{1.0, 2.0, 1.0}
* = ChassisSpeeds{4.0, 2.0, 1.0}
*
* @param other The ChassisSpeeds to subtract.
*
* @return The difference between the two ChassisSpeeds.
*/
constexpr ChassisSpeeds operator-(const ChassisSpeeds& other) const {
return *this + -other;
}
/**
* Returns the inverse of the current ChassisSpeeds.
* This is equivalent to negating all components of the ChassisSpeeds.
*
* @return The inverse of the current ChassisSpeeds.
*/
constexpr ChassisSpeeds operator-() const { return {-vx, -vy, -omega}; }
/**
* Multiplies the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
*
* <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} * 2
* = ChassisSpeeds{4.0, 5.0, 1.0}
*
* @param scalar The scalar to multiply by.
*
* @return The scaled ChassisSpeeds.
*/
constexpr ChassisSpeeds operator*(double scalar) const {
return {scalar * vx, scalar * vy, scalar * omega};
}
/**
* Divides the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
*
* <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} / 2
* = ChassisSpeeds{1.0, 1.25, 0.5}
*
* @param scalar The scalar to divide by.
*
* @return The scaled ChassisSpeeds.
*/
constexpr ChassisSpeeds operator/(double scalar) const {
return operator*(1.0 / scalar);
}
/**
* Compares the ChassisSpeeds with another ChassisSpeed.
*
* @param other The other ChassisSpeeds.
*
* @return The result of the comparison. Is true if they are the same.
*/
constexpr bool operator==(const ChassisSpeeds& other) const = default;
};
} // namespace wpi::math
#include "wpi/math/kinematics/proto/ChassisSpeedsProto.hpp"
#include "wpi/math/kinematics/struct/ChassisSpeedsStruct.hpp"

207
wpimath/src/main/native/include/wpi/math/kinematics/ChassisVelocities.hpp Normal file
View File

@@ -0,0 +1,207 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "wpi/math/geometry/Pose2d.hpp"
#include "wpi/math/geometry/Rotation2d.hpp"
#include "wpi/units/angular_velocity.hpp"
#include "wpi/units/velocity.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace wpi::math {
/**
* Represents robot chassis velocities.
*
* Although this struct contains similar members compared to a Twist2d, they do
* NOT represent the same thing. Whereas a Twist2d represents a change in pose
* w.r.t to the robot frame of reference, a ChassisVelocities struct represents
* a robot's velocities.
*
* A strictly non-holonomic drivetrain, such as a differential drive, should
* never have a dy component because it can never move sideways. Holonomic
* drivetrains such as swerve and mecanum will often have all three components.
*/
struct WPILIB_DLLEXPORT ChassisVelocities {
/**
* Velocity along the x-axis. (Fwd is +)
*/
wpi::units::meters_per_second_t vx = 0_mps;
/**
* Velocity along the y-axis. (Left is +)
*/
wpi::units::meters_per_second_t vy = 0_mps;
/**
* Represents the angular velocity of the robot frame. (CCW is +)
*/
wpi::units::radians_per_second_t omega = 0_rad_per_s;
/**
* Creates a Twist2d from ChassisVelocities.
*
* @param dt The duration of the timestep.
*
* @return Twist2d.
*/
constexpr Twist2d ToTwist2d(wpi::units::second_t dt) const {
return Twist2d{vx * dt, vy * dt, omega * dt};
}
/**
* Discretizes continuous-time chassis velocities.
*
* This function converts continuous-time chassis velocities into
* discrete-time ones such that when the discrete-time chassis velocities are
* 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 holonomic (swerve or mecanum) drivetrain. However, scaling down
* the ChassisVelocities after discretizing (e.g., when desaturating swerve
* module velocities) rotates the direction of net motion in the opposite
* direction of rotational velocity, introducing a different translational
* skew which is not accounted for by discretization.
*
* @param dt The duration of the timestep the velocities should be applied
* for.
* @return Discretized ChassisVelocities.
*/
constexpr ChassisVelocities Discretize(wpi::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.
Transform2d desiredTransform{vx * dt, vy * dt, omega * dt};
// Find the chassis translation/rotation deltas in the robot frame that move
// the robot from its current pose to the desired pose
auto twist = desiredTransform.Log();
// Turn the chassis translation/rotation deltas into average velocities
return {twist.dx / dt, twist.dy / dt, twist.dtheta / dt};
}
/**
* Converts this field-relative set of velocities into a robot-relative
* ChassisVelocities 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.
* @return ChassisVelocities object representing the velocities in the robot's
* frame of reference.
*/
constexpr ChassisVelocities ToRobotRelative(
const Rotation2d& robotAngle) const {
// CW rotation into chassis frame
auto rotated = Translation2d{wpi::units::meter_t{vx.value()},
wpi::units::meter_t{vy.value()}}
.RotateBy(-robotAngle);
return {wpi::units::meters_per_second_t{rotated.X().value()},
wpi::units::meters_per_second_t{rotated.Y().value()}, omega};
}
/**
* Converts this robot-relative set of velocities into a field-relative
* ChassisVelocities 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.
* @return ChassisVelocities object representing the velocities in the field's
* frame of reference.
*/
constexpr ChassisVelocities ToFieldRelative(
const Rotation2d& robotAngle) const {
// CCW rotation out of chassis frame
auto rotated = Translation2d{wpi::units::meter_t{vx.value()},
wpi::units::meter_t{vy.value()}}
.RotateBy(robotAngle);
return {wpi::units::meters_per_second_t{rotated.X().value()},
wpi::units::meters_per_second_t{rotated.Y().value()}, omega};
}
/**
* Adds two ChassisVelocities and returns the sum.
*
* For example, ChassisVelocities{1.0, 0.5, 1.5} +
* ChassisVelocities{2.0, 1.5, 0.5} = ChassisVelocities{3.0, 2.0, 2.0}
*
* @param other The ChassisVelocities to add.
*
* @return The sum of the ChassisVelocities.
*/
constexpr ChassisVelocities operator+(const ChassisVelocities& other) const {
return {vx + other.vx, vy + other.vy, omega + other.omega};
}
/**
* Subtracts the other ChassisVelocities from the current ChassisVelocities
* and returns the difference.
*
* For example, ChassisVelocities{5.0, 4.0, 2.0} -
* ChassisVelocities{1.0, 2.0, 1.0} = ChassisVelocities{4.0, 2.0, 1.0}
*
* @param other The ChassisVelocities to subtract.
*
* @return The difference between the two ChassisVelocities.
*/
constexpr ChassisVelocities operator-(const ChassisVelocities& other) const {
return *this + -other;
}
/**
* Returns the inverse of the current ChassisVelocities.
* This is equivalent to negating all components of the ChassisVelocities.
*
* @return The inverse of the current ChassisVelocities.
*/
constexpr ChassisVelocities operator-() const { return {-vx, -vy, -omega}; }
/**
* Multiplies the ChassisVelocities by a scalar and returns the new
* ChassisVelocities.
*
* For example, ChassisVelocities{2.0, 2.5, 1.0} * 2
* = ChassisVelocities{4.0, 5.0, 1.0}
*
* @param scalar The scalar to multiply by.
*
* @return The scaled ChassisVelocities.
*/
constexpr ChassisVelocities operator*(double scalar) const {
return {scalar * vx, scalar * vy, scalar * omega};
}
/**
* Divides the ChassisVelocities by a scalar and returns the new
* ChassisVelocities.
*
* For example, ChassisVelocities{2.0, 2.5, 1.0} / 2
* = ChassisVelocities{1.0, 1.25, 0.5}
*
* @param scalar The scalar to divide by.
*
* @return The scaled ChassisVelocities.
*/
constexpr ChassisVelocities operator/(double scalar) const {
return operator*(1.0 / scalar);
}
/**
* Compares the ChassisVelocities with another ChassisVelocity.
*
* @param other The other ChassisVelocities.
*
* @return The result of the comparison. Is true if they are the same.
*/
constexpr bool operator==(const ChassisVelocities& other) const = default;
};
} // namespace wpi::math
#include "wpi/math/kinematics/proto/ChassisVelocitiesProto.hpp"
#include "wpi/math/kinematics/struct/ChassisVelocitiesStruct.hpp"

47
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveKinematics.hpp
View File

@@ -8,10 +8,10 @@
#include "wpi/math/geometry/Twist2d.hpp"
#include "wpi/math/kinematics/ChassisAccelerations.hpp"
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelPositions.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp"
#include "wpi/math/kinematics/Kinematics.hpp"
#include "wpi/math/util/MathShared.hpp"
#include "wpi/units/angle.hpp"
@@ -23,22 +23,22 @@ namespace wpi::math {
* Helper class that converts a chassis velocity (dx and dtheta components) to
* left and right wheel velocities for a differential drive.
*
* Inverse kinematics converts a desired chassis speed into left and right
* Inverse kinematics converts a desired chassis velocity into left and right
* velocity components whereas forward kinematics converts left and right
* component velocities into a linear and angular chassis speed.
* component velocities into a linear and angular chassis velocity.
*/
class WPILIB_DLLEXPORT DifferentialDriveKinematics
: public Kinematics<DifferentialDriveWheelPositions,
DifferentialDriveWheelSpeeds,
DifferentialDriveWheelVelocities,
DifferentialDriveWheelAccelerations> {
public:
/**
* Constructs a differential drive kinematics object.
*
* @param trackwidth The trackwidth of the drivetrain. Theoretically, this is
* the distance between the left wheels and right wheels. However, the
* empirical value may be larger than the physical measured value due to
* scrubbing effects.
* the distance between the left wheels and right wheels. However, the
* empirical value may be larger than the physical measured value due to
* scrubbing effects.
*/
constexpr explicit DifferentialDriveKinematics(wpi::units::meter_t trackwidth)
: trackwidth(trackwidth) {
@@ -49,30 +49,33 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics
}
/**
* Returns a chassis speed from left and right component velocities using
* Returns a chassis velocity from left and right component velocities using
* forward kinematics.
*
* @param wheelSpeeds The left and right velocities.
* @return The chassis speed.
* @param wheelVelocities The left and right velocities.
* @return The chassis velocity.
*/
constexpr ChassisSpeeds ToChassisSpeeds(
const DifferentialDriveWheelSpeeds& wheelSpeeds) const override {
return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
(wheelSpeeds.right - wheelSpeeds.left) / trackwidth * 1_rad};
constexpr ChassisVelocities ToChassisVelocities(
const DifferentialDriveWheelVelocities& wheelVelocities) const override {
return {
(wheelVelocities.left + wheelVelocities.right) / 2.0, 0_mps,
(wheelVelocities.right - wheelVelocities.left) / trackwidth * 1_rad};
}
/**
* Returns left and right component velocities from a chassis speed using
* Returns left and right component velocities from a chassis velocity using
* inverse kinematics.
*
* @param chassisSpeeds The linear and angular (dx and dtheta) components that
* represent the chassis' speed.
* @param chassisVelocities The linear and angular (dx and dtheta) components
* that represent the chassis' velocity.
* @return The left and right velocities.
*/
constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds) const override {
return {chassisSpeeds.vx - trackwidth / 2 * chassisSpeeds.omega / 1_rad,
chassisSpeeds.vx + trackwidth / 2 * chassisSpeeds.omega / 1_rad};
constexpr DifferentialDriveWheelVelocities ToWheelVelocities(
const ChassisVelocities& chassisVelocities) const override {
return {
chassisVelocities.vx - trackwidth / 2 * chassisVelocities.omega / 1_rad,
chassisVelocities.vx +
trackwidth / 2 * chassisVelocities.omega / 1_rad};
}
/**

4
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry.hpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Pose2d.hpp"
#include "wpi/math/kinematics/DifferentialDriveKinematics.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelPositions.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp"
#include "wpi/math/kinematics/Odometry.hpp"
#include "wpi/units/length.hpp"
#include "wpi/util/SymbolExports.hpp"
@@ -27,7 +27,7 @@ namespace wpi::math {
*/
class WPILIB_DLLEXPORT DifferentialDriveOdometry
: public Odometry<DifferentialDriveWheelPositions,
DifferentialDriveWheelSpeeds,
DifferentialDriveWheelVelocities,
DifferentialDriveWheelAccelerations> {
public:
/**

4
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry3d.hpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Pose3d.hpp"
#include "wpi/math/kinematics/DifferentialDriveKinematics.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelPositions.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp"
#include "wpi/math/kinematics/Odometry3d.hpp"
#include "wpi/units/length.hpp"
#include "wpi/util/SymbolExports.hpp"
@@ -27,7 +27,7 @@ namespace wpi::math {
*/
class WPILIB_DLLEXPORT DifferentialDriveOdometry3d
: public Odometry3d<DifferentialDriveWheelPositions,
DifferentialDriveWheelSpeeds,
DifferentialDriveWheelVelocities,
DifferentialDriveWheelAccelerations> {
public:
/**

8
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp
View File

@@ -25,7 +25,7 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelAccelerations {
/**
* Adds two DifferentialDriveWheelAccelerations and returns the sum.
*
* <p>For example, DifferentialDriveWheelAccelerations{1.0, 0.5} +
* For example, DifferentialDriveWheelAccelerations{1.0, 0.5} +
* DifferentialDriveWheelAccelerations{2.0, 1.5} =
* DifferentialDriveWheelAccelerations{3.0, 2.0}
*
@@ -42,7 +42,7 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelAccelerations {
* Subtracts the other DifferentialDriveWheelAccelerations from the current
* DifferentialDriveWheelAccelerations and returns the difference.
*
* <p>For example, DifferentialDriveWheelAccelerations{5.0, 4.0} -
* For example, DifferentialDriveWheelAccelerations{5.0, 4.0} -
* DifferentialDriveWheelAccelerations{1.0, 2.0} =
* DifferentialDriveWheelAccelerations{4.0, 2.0}
*
@@ -70,7 +70,7 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelAccelerations {
* Multiplies the DifferentialDriveWheelAccelerations by a scalar and returns
* the new DifferentialDriveWheelAccelerations.
*
* <p>For example, DifferentialDriveWheelAccelerations{2.0, 2.5} * 2
* For example, DifferentialDriveWheelAccelerations{2.0, 2.5} * 2
* = DifferentialDriveWheelAccelerations{4.0, 5.0}
*
* @param scalar The scalar to multiply by.
@@ -85,7 +85,7 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelAccelerations {
* Divides the DifferentialDriveWheelAccelerations by a scalar and returns the
* new DifferentialDriveWheelAccelerations.
*
* <p>For example, DifferentialDriveWheelAccelerations{2.0, 2.5} / 2
* For example, DifferentialDriveWheelAccelerations{2.0, 2.5} / 2
* = DifferentialDriveWheelAccelerations{1.0, 1.25}
*
* @param scalar The scalar to divide by.

126
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp
View File

@@ -1,126 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "wpi/units/math.hpp"
#include "wpi/units/velocity.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace wpi::math {
/**
* Represents the wheel speeds for a differential drive drivetrain.
*/
struct WPILIB_DLLEXPORT DifferentialDriveWheelSpeeds {
/**
* Speed of the left side of the robot.
*/
wpi::units::meters_per_second_t left = 0_mps;
/**
* Speed of the right side of the robot.
*/
wpi::units::meters_per_second_t right = 0_mps;
/**
* Renormalizes the wheel speeds if either side is above the specified
* maximum.
*
* Sometimes, after inverse kinematics, the requested speed from one or more
* wheels may be above the max attainable speed for the driving motor on that
* wheel. To fix this issue, one can reduce all the wheel speeds to make sure
* that all requested module speeds are at-or-below the absolute threshold,
* while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
*/
constexpr void Desaturate(
wpi::units::meters_per_second_t attainableMaxSpeed) {
auto realMaxSpeed = wpi::units::math::max(wpi::units::math::abs(left),
wpi::units::math::abs(right));
if (realMaxSpeed > attainableMaxSpeed) {
left = left / realMaxSpeed * attainableMaxSpeed;
right = right / realMaxSpeed * attainableMaxSpeed;
}
}
/**
* Adds two DifferentialDriveWheelSpeeds and returns the sum.
*
* <p>For example, DifferentialDriveWheelSpeeds{1.0, 0.5} +
* DifferentialDriveWheelSpeeds{2.0, 1.5} =
* DifferentialDriveWheelSpeeds{3.0, 2.0}
*
* @param other The DifferentialDriveWheelSpeeds to add.
*
* @return The sum of the DifferentialDriveWheelSpeeds.
*/
constexpr DifferentialDriveWheelSpeeds operator+(
const DifferentialDriveWheelSpeeds& other) const {
return {left + other.left, right + other.right};
}
/**
* Subtracts the other DifferentialDriveWheelSpeeds from the current
* DifferentialDriveWheelSpeeds and returns the difference.
*
* <p>For example, DifferentialDriveWheelSpeeds{5.0, 4.0} -
* DifferentialDriveWheelSpeeds{1.0, 2.0} =
* DifferentialDriveWheelSpeeds{4.0, 2.0}
*
* @param other The DifferentialDriveWheelSpeeds to subtract.
*
* @return The difference between the two DifferentialDriveWheelSpeeds.
*/
constexpr DifferentialDriveWheelSpeeds operator-(
const DifferentialDriveWheelSpeeds& other) const {
return *this + -other;
}
/**
* Returns the inverse of the current DifferentialDriveWheelSpeeds.
* This is equivalent to negating all components of the
* DifferentialDriveWheelSpeeds.
*
* @return The inverse of the current DifferentialDriveWheelSpeeds.
*/
constexpr DifferentialDriveWheelSpeeds operator-() const {
return {-left, -right};
}
/**
* Multiplies the DifferentialDriveWheelSpeeds by a scalar and returns the new
* DifferentialDriveWheelSpeeds.
*
* <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} * 2
* = DifferentialDriveWheelSpeeds{4.0, 5.0}
*
* @param scalar The scalar to multiply by.
*
* @return The scaled DifferentialDriveWheelSpeeds.
*/
constexpr DifferentialDriveWheelSpeeds operator*(double scalar) const {
return {scalar * left, scalar * right};
}
/**
* Divides the DifferentialDriveWheelSpeeds by a scalar and returns the new
* DifferentialDriveWheelSpeeds.
*
* <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} / 2
* = DifferentialDriveWheelSpeeds{1.0, 1.25}
*
* @param scalar The scalar to divide by.
*
* @return The scaled DifferentialDriveWheelSpeeds.
*/
constexpr DifferentialDriveWheelSpeeds operator/(double scalar) const {
return operator*(1.0 / scalar);
}
};
} // namespace wpi::math
#include "wpi/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.hpp"
#include "wpi/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.hpp"

124
wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp Normal file
View File

@@ -0,0 +1,124 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "wpi/units/math.hpp"
#include "wpi/units/velocity.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace wpi::math {
/**
* Represents the wheel velocities for a differential drive drivetrain.
*/
struct WPILIB_DLLEXPORT DifferentialDriveWheelVelocities {
/**
* Velocity of the left side of the robot.
*/
wpi::units::meters_per_second_t left = 0_mps;
/**
* Velocity of the right side of the robot.
*/
wpi::units::meters_per_second_t right = 0_mps;
/**
* Renormalizes the wheel velocities if either side is above the specified
* maximum.
*
* Sometimes, after inverse kinematics, the requested velocity from one or
* more wheels may be above the max attainable velocity for the driving motor
* on that wheel. To fix this issue, one can reduce all the wheel velocities
* to make sure that all requested module velocities are at-or-below the
* absolute threshold, while maintaining the ratio of velocities between
* wheels.
*
* @param attainableMaxVelocity The absolute max velocity that a wheel can
* reach.
*/
constexpr void Desaturate(
wpi::units::meters_per_second_t attainableMaxVelocity) {
auto realMaxVelocity = wpi::units::math::max(wpi::units::math::abs(left),
wpi::units::math::abs(right));
if (realMaxVelocity > attainableMaxVelocity) {
left = left / realMaxVelocity * attainableMaxVelocity;
right = right / realMaxVelocity * attainableMaxVelocity;
}
}
/**
* Adds two DifferentialDriveWheelVelocities and returns the sum.
*
* For example, DifferentialDriveWheelVelocities{1.0, 0.5} +
* DifferentialDriveWheelVelocities{2.0, 1.5} =
* DifferentialDriveWheelVelocities{3.0, 2.0}
*
* @param other The DifferentialDriveWheelVelocities to add.
* @return The sum of the DifferentialDriveWheelVelocities.
*/
constexpr DifferentialDriveWheelVelocities operator+(
const DifferentialDriveWheelVelocities& other) const {
return {left + other.left, right + other.right};
}
/**
* Subtracts the other DifferentialDriveWheelVelocities from the current
* DifferentialDriveWheelVelocities and returns the difference.
*
* For example, DifferentialDriveWheelVelocities{5.0, 4.0} -
* DifferentialDriveWheelVelocities{1.0, 2.0} =
* DifferentialDriveWheelVelocities{4.0, 2.0}
*
* @param other The DifferentialDriveWheelVelocities to subtract.
* @return The difference between the two DifferentialDriveWheelVelocities.
*/
constexpr DifferentialDriveWheelVelocities operator-(
const DifferentialDriveWheelVelocities& other) const {
return *this + -other;
}
/**
* Returns the inverse of the current DifferentialDriveWheelVelocities.
* This is equivalent to negating all components of the
* DifferentialDriveWheelVelocities.
*
* @return The inverse of the current DifferentialDriveWheelVelocities.
*/
constexpr DifferentialDriveWheelVelocities operator-() const {
return {-left, -right};
}
/**
* Multiplies the DifferentialDriveWheelVelocities by a scalar and returns the
* new DifferentialDriveWheelVelocities.
*
* For example, DifferentialDriveWheelVelocities{2.0, 2.5} * 2
* = DifferentialDriveWheelVelocities{4.0, 5.0}
*
* @param scalar The scalar to multiply by.
* @return The scaled DifferentialDriveWheelVelocities.
*/
constexpr DifferentialDriveWheelVelocities operator*(double scalar) const {
return {scalar * left, scalar * right};
}
/**
* Divides the DifferentialDriveWheelVelocities by a scalar and returns the
* new DifferentialDriveWheelVelocities.
*
* For example, DifferentialDriveWheelVelocities{2.0, 2.5} / 2
* = DifferentialDriveWheelVelocities{1.0, 1.25}
*
* @param scalar The scalar to divide by.
* @return The scaled DifferentialDriveWheelVelocities.
*/
constexpr DifferentialDriveWheelVelocities operator/(double scalar) const {
return operator*(1.0 / scalar);
}
};
} // namespace wpi::math
#include "wpi/math/kinematics/proto/DifferentialDriveWheelVelocitiesProto.hpp"
#include "wpi/math/kinematics/struct/DifferentialDriveWheelVelocitiesStruct.hpp"

38
wpimath/src/main/native/include/wpi/math/kinematics/Kinematics.hpp
View File

@@ -6,20 +6,20 @@
#include "wpi/math/geometry/Twist2d.hpp"
#include "wpi/math/kinematics/ChassisAccelerations.hpp"
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace wpi::math {
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components)
* into individual wheel speeds. Robot code should not use this directly-
* into individual wheel velocities. Robot code should not use this directly-
* Instead, use the particular type for your drivetrain (e.g.,
* DifferentialDriveKinematics).
*
* Inverse kinematics converts a desired chassis speed into wheel speeds whereas
* forward kinematics converts wheel speeds into chassis speed.
* Inverse kinematics converts a desired chassis velocity into wheel velocities
* whereas forward kinematics converts wheel velocities into chassis velocity.
*/
template <typename WheelPositions, typename WheelSpeeds,
template <typename WheelPositions, typename WheelVelocities,
typename WheelAccelerations>
requires std::copy_constructible<WheelPositions> &&
std::assignable_from<WheelPositions&, const WheelPositions&>
@@ -28,27 +28,27 @@ class WPILIB_DLLEXPORT Kinematics {
virtual ~Kinematics() noexcept = default;
/**
* Performs forward kinematics to return the resulting chassis speed from the
* wheel speeds. This method is often used for odometry -- determining the
* robot's position on the field using data from the real-world speed of each
* wheel on the robot.
* Performs forward kinematics to return the resulting chassis velocity from
* the wheel velocities. This method is often used for odometry -- determining
* the robot's position on the field using data from the real-world velocity
* of each wheel on the robot.
*
* @param wheelSpeeds The speeds of the wheels.
* @return The chassis speed.
* @param wheelVelocities The velocities of the wheels.
* @return The chassis velocity.
*/
virtual ChassisSpeeds ToChassisSpeeds(
const WheelSpeeds& wheelSpeeds) const = 0;
virtual ChassisVelocities ToChassisVelocities(
const WheelVelocities& wheelVelocities) const = 0;
/**
* Performs inverse kinematics to return the wheel speeds from a desired
* Performs inverse kinematics to return the wheel velocities from a desired
* chassis velocity. This method is often used to convert joystick values into
* wheel speeds.
* wheel velocities.
*
* @param chassisSpeeds The desired chassis speed.
* @return The wheel speeds.
* @param chassisVelocities The desired chassis velocity.
* @return The wheel velocities.
*/
virtual WheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds) const = 0;
virtual WheelVelocities ToWheelVelocities(
const ChassisVelocities& chassisVelocities) const = 0;
/**
* Performs forward kinematics to return the resulting Twist2d from the given

88
wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveKinematics.hpp
View File

@@ -9,11 +9,11 @@
#include "wpi/math/geometry/Translation2d.hpp"
#include "wpi/math/geometry/Twist2d.hpp"
#include "wpi/math/kinematics/ChassisAccelerations.hpp"
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include "wpi/math/kinematics/Kinematics.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelPositions.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelVelocities.hpp"
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/math/util/MathShared.hpp"
#include "wpi/util/SymbolExports.hpp"
@@ -22,41 +22,42 @@ namespace wpi::math {
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components)
* into individual wheel speeds.
* into individual wheel velocities.
*
* The inverse kinematics (converting from a desired chassis velocity to
* individual wheel speeds) uses the relative locations of the wheels with
* individual wheel velocities) uses the relative locations of the wheels with
* respect to the center of rotation. The center of rotation for inverse
* kinematics is also variable. This means that you can set your set your center
* of rotation in a corner of the robot to perform special evasion maneuvers.
*
* Forward kinematics (converting an array of wheel speeds into the overall
* Forward kinematics (converting an array of wheel velocities into the overall
* chassis motion) is performs the exact opposite of what inverse kinematics
* does. Since this is an overdetermined system (more equations than variables),
* we use a least-squares approximation.
*
* The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds]
* We take the Moore-Penrose pseudoinverse of [wheelLocations] and then
* multiply by [wheelSpeeds] to get our chassis speeds.
* The inverse kinematics: [wheelVelocities] = [wheelLocations] *
* [chassisVelocities] We take the Moore-Penrose pseudoinverse of
* [wheelLocations] and then multiply by [wheelVelocities] to get our chassis
* velocities.
*
* Forward kinematics is also used for odometry -- determining the position of
* the robot on the field using encoders and a gyro.
*/
class WPILIB_DLLEXPORT MecanumDriveKinematics
: public Kinematics<MecanumDriveWheelPositions, MecanumDriveWheelSpeeds,
: public Kinematics<MecanumDriveWheelPositions, MecanumDriveWheelVelocities,
MecanumDriveWheelAccelerations> {
public:
/**
* Constructs a mecanum drive kinematics object.
*
* @param frontLeftWheel The location of the front-left wheel relative to the
* physical center of the robot.
* physical center of the robot.
* @param frontRightWheel The location of the front-right wheel relative to
* the physical center of the robot.
* the physical center of the robot.
* @param rearLeftWheel The location of the rear-left wheel relative to the
* physical center of the robot.
* physical center of the robot.
* @param rearRightWheel The location of the rear-right wheel relative to the
* physical center of the robot.
* physical center of the robot.
*/
explicit MecanumDriveKinematics(Translation2d frontLeftWheel,
Translation2d frontRightWheel,
@@ -75,9 +76,9 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics
MecanumDriveKinematics(const MecanumDriveKinematics&) = default;
/**
* Performs inverse kinematics to return the wheel speeds from a desired
* Performs inverse kinematics to return the wheel velocities from a desired
* chassis velocity. This method is often used to convert joystick values into
* wheel speeds.
* wheel velocities.
*
* This function also supports variable centers of rotation. During normal
* operations, the center of rotation is usually the same as the physical
@@ -85,45 +86,44 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics
* However, if you wish to change the center of rotation for evasive
* maneuvers, vision alignment, or for any other use case, you can do so.
*
* @param chassisSpeeds The desired chassis speed.
* @param chassisVelocities The desired chassis velocity.
* @param centerOfRotation The center of rotation. For example, if you set the
* center of rotation at one corner of the robot and
* provide a chassis speed that only has a dtheta
* component, the robot will rotate around that
* corner.
* center of rotation at one corner of the robot and provide a chassis
* velocity that only has a dtheta component, the robot will rotate around
* that corner.
*
* @return The wheel speeds. Use caution because they are not normalized.
* Sometimes, a user input may cause one of the wheel speeds to go
* above the attainable max velocity. Use the
* MecanumDriveWheelSpeeds::Normalize() function to rectify this
* issue. In addition, you can leverage the power of C++17 to directly
* assign the wheel speeds to variables:
* @return The wheel velocities. Use caution because they are not normalized.
* Sometimes, a user input may cause one of the wheel velocities to go
* above the attainable max velocity. Use the
* MecanumDriveWheelVelocities::Normalize() function to rectify this
* issue. In addition, you can leverage the power of C++17 to directly
* assign the wheel velocities to variables:
*
* @code{.cpp}
* auto [fl, fr, bl, br] = kinematics.ToWheelSpeeds(chassisSpeeds);
* auto [fl, fr, bl, br] = kinematics.ToWheelVelocities(chassisVelocities);
* @endcode
*/
MecanumDriveWheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds,
MecanumDriveWheelVelocities ToWheelVelocities(
const ChassisVelocities& chassisVelocities,
const Translation2d& centerOfRotation) const;
MecanumDriveWheelSpeeds ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds) const override {
return ToWheelSpeeds(chassisSpeeds, {});
MecanumDriveWheelVelocities ToWheelVelocities(
const ChassisVelocities& chassisVelocities) const override {
return ToWheelVelocities(chassisVelocities, {});
}
/**
* Performs forward kinematics to return the resulting chassis state from the
* given wheel speeds. This method is often used for odometry -- determining
* the robot's position on the field using data from the real-world speed of
* each wheel on the robot.
* given wheel velocities. This method is often used for odometry --
* determining the robot's position on the field using data from the
* real-world velocity of each wheel on the robot.
*
* @param wheelSpeeds The current mecanum drive wheel speeds.
* @param wheelVelocities The current mecanum drive wheel velocities.
*
* @return The resulting chassis speed.
* @return The resulting chassis velocity.
*/
ChassisSpeeds ToChassisSpeeds(
const MecanumDriveWheelSpeeds& wheelSpeeds) const override;
ChassisVelocities ToChassisVelocities(
const MecanumDriveWheelVelocities& wheelVelocities) const override;
Twist2d ToTwist2d(const MecanumDriveWheelPositions& start,
const MecanumDriveWheelPositions& end) const override;
@@ -136,7 +136,7 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics
*
* @param wheelDeltas The change in distance driven by each wheel.
*
* @return The resulting chassis speed.
* @return The resulting chassis velocity.
*/
Twist2d ToTwist2d(const MecanumDriveWheelPositions& wheelDeltas) const;
@@ -200,13 +200,13 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics
* Construct inverse kinematics matrix from wheel locations.
*
* @param fl The location of the front-left wheel relative to the physical
* center of the robot.
* center of the robot.
* @param fr The location of the front-right wheel relative to the physical
* center of the robot.
* center of the robot.
* @param rl The location of the rear-left wheel relative to the physical
* center of the robot.
* center of the robot.
* @param rr The location of the rear-right wheel relative to the physical
* center of the robot.
* center of the robot.
*/
void SetInverseKinematics(Translation2d fl, Translation2d fr,
Translation2d rl, Translation2d rr) const;

4
wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveOdometry.hpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Pose2d.hpp"
#include "wpi/math/kinematics/MecanumDriveKinematics.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelPositions.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelVelocities.hpp"
#include "wpi/math/kinematics/Odometry.hpp"
#include "wpi/util/SymbolExports.hpp"
@@ -23,7 +23,7 @@ namespace wpi::math {
* when using computer-vision systems.
*/
class WPILIB_DLLEXPORT MecanumDriveOdometry
: public Odometry<MecanumDriveWheelPositions, MecanumDriveWheelSpeeds,
: public Odometry<MecanumDriveWheelPositions, MecanumDriveWheelVelocities,
MecanumDriveWheelAccelerations> {
public:
/**

4
wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveOdometry3d.hpp
View File

@@ -7,7 +7,7 @@
#include "wpi/math/geometry/Pose3d.hpp"
#include "wpi/math/kinematics/MecanumDriveKinematics.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelPositions.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelVelocities.hpp"
#include "wpi/math/kinematics/Odometry3d.hpp"
#include "wpi/util/SymbolExports.hpp"
@@ -23,7 +23,7 @@ namespace wpi::math {
* when using computer-vision systems.
*/
class WPILIB_DLLEXPORT MecanumDriveOdometry3d
: public Odometry3d<MecanumDriveWheelPositions, MecanumDriveWheelSpeeds,
: public Odometry3d<MecanumDriveWheelPositions, MecanumDriveWheelVelocities,
MecanumDriveWheelAccelerations> {
public:
/**

8
wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp
View File

@@ -35,7 +35,7 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelAccelerations {
/**
* Adds two MecanumDriveWheelAccelerations and returns the sum.
*
* <p>For example, MecanumDriveWheelAccelerations{1.0, 0.5, 2.0, 1.5} +
* For example, MecanumDriveWheelAccelerations{1.0, 0.5, 2.0, 1.5} +
* MecanumDriveWheelAccelerations{2.0, 1.5, 0.5, 1.0} =
* MecanumDriveWheelAccelerations{3.0, 2.0, 2.5, 2.5}
*
@@ -52,7 +52,7 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelAccelerations {
* Subtracts the other MecanumDriveWheelAccelerations from the current
* MecanumDriveWheelAccelerations and returns the difference.
*
* <p>For example, MecanumDriveWheelAccelerations{5.0, 4.0, 6.0, 2.5} -
* For example, MecanumDriveWheelAccelerations{5.0, 4.0, 6.0, 2.5} -
* MecanumDriveWheelAccelerations{1.0, 2.0, 3.0, 0.5} =
* MecanumDriveWheelAccelerations{4.0, 2.0, 3.0, 2.0}
*
@@ -79,7 +79,7 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelAccelerations {
* Multiplies the MecanumDriveWheelAccelerations by a scalar and returns the
* new MecanumDriveWheelAccelerations.
*
* <p>For example, MecanumDriveWheelAccelerations{2.0, 2.5, 3.0, 3.5} * 2 =
* For example, MecanumDriveWheelAccelerations{2.0, 2.5, 3.0, 3.5} * 2 =
* MecanumDriveWheelAccelerations{4.0, 5.0, 6.0, 7.0}
*
* @param scalar The scalar to multiply by.
@@ -94,7 +94,7 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelAccelerations {
* Divides the MecanumDriveWheelAccelerations by a scalar and returns the new
* MecanumDriveWheelAccelerations.
*
* <p>For example, MecanumDriveWheelAccelerations{2.0, 2.5, 1.5, 1.0} / 2 =
* For example, MecanumDriveWheelAccelerations{2.0, 2.5, 1.5, 1.0} / 2 =
* MecanumDriveWheelAccelerations{1.0, 1.25, 0.75, 0.5}
*
* @param scalar The scalar to divide by.

149
wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp
View File

@@ -1,149 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <algorithm>
#include <array>
#include "wpi/units/math.hpp"
#include "wpi/units/velocity.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace wpi::math {
/**
* Represents the wheel speeds for a mecanum drive drivetrain.
*/
struct WPILIB_DLLEXPORT MecanumDriveWheelSpeeds {
/**
* Speed of the front-left wheel.
*/
wpi::units::meters_per_second_t frontLeft = 0_mps;
/**
* Speed of the front-right wheel.
*/
wpi::units::meters_per_second_t frontRight = 0_mps;
/**
* Speed of the rear-left wheel.
*/
wpi::units::meters_per_second_t rearLeft = 0_mps;
/**
* Speed of the rear-right wheel.
*/
wpi::units::meters_per_second_t rearRight = 0_mps;
/**
* Renormalizes the wheel speeds if any individual speed is above the
* specified maximum.
*
* Sometimes, after inverse kinematics, the requested speed from one or
* more wheels may be above the max attainable speed for the driving motor on
* that wheel. To fix this issue, one can reduce all the wheel speeds to make
* sure that all requested module speeds are at-or-below the absolute
* threshold, while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
* @return Desaturated MecanumDriveWheelSpeeds.
*/
constexpr MecanumDriveWheelSpeeds Desaturate(
wpi::units::meters_per_second_t attainableMaxSpeed) const {
std::array<wpi::units::meters_per_second_t, 4> wheelSpeeds{
frontLeft, frontRight, rearLeft, rearRight};
wpi::units::meters_per_second_t realMaxSpeed =
wpi::units::math::abs(*std::max_element(
wheelSpeeds.begin(), wheelSpeeds.end(),
[](const auto& a, const auto& b) {
return wpi::units::math::abs(a) < wpi::units::math::abs(b);
}));
if (realMaxSpeed > attainableMaxSpeed) {
for (int i = 0; i < 4; ++i) {
wheelSpeeds[i] = wheelSpeeds[i] / realMaxSpeed * attainableMaxSpeed;
}
return MecanumDriveWheelSpeeds{wheelSpeeds[0], wheelSpeeds[1],
wheelSpeeds[2], wheelSpeeds[3]};
}
return *this;
}
/**
* Adds two MecanumDriveWheelSpeeds and returns the sum.
*
* <p>For example, MecanumDriveWheelSpeeds{1.0, 0.5, 2.0, 1.5} +
* MecanumDriveWheelSpeeds{2.0, 1.5, 0.5, 1.0} =
* MecanumDriveWheelSpeeds{3.0, 2.0, 2.5, 2.5}
*
* @param other The MecanumDriveWheelSpeeds to add.
* @return The sum of the MecanumDriveWheelSpeeds.
*/
constexpr MecanumDriveWheelSpeeds operator+(
const MecanumDriveWheelSpeeds& other) const {
return {frontLeft + other.frontLeft, frontRight + other.frontRight,
rearLeft + other.rearLeft, rearRight + other.rearRight};
}
/**
* Subtracts the other MecanumDriveWheelSpeeds from the current
* MecanumDriveWheelSpeeds and returns the difference.
*
* <p>For example, MecanumDriveWheelSpeeds{5.0, 4.0, 6.0, 2.5} -
* MecanumDriveWheelSpeeds{1.0, 2.0, 3.0, 0.5} =
* MecanumDriveWheelSpeeds{4.0, 2.0, 3.0, 2.0}
*
* @param other The MecanumDriveWheelSpeeds to subtract.
* @return The difference between the two MecanumDriveWheelSpeeds.
*/
constexpr MecanumDriveWheelSpeeds operator-(
const MecanumDriveWheelSpeeds& other) const {
return *this + -other;
}
/**
* Returns the inverse of the current MecanumDriveWheelSpeeds.
* This is equivalent to negating all components of the
* MecanumDriveWheelSpeeds.
*
* @return The inverse of the current MecanumDriveWheelSpeeds.
*/
constexpr MecanumDriveWheelSpeeds operator-() const {
return {-frontLeft, -frontRight, -rearLeft, -rearRight};
}
/**
* Multiplies the MecanumDriveWheelSpeeds by a scalar and returns the new
* MecanumDriveWheelSpeeds.
*
* <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 3.0, 3.5} * 2 =
* MecanumDriveWheelSpeeds{4.0, 5.0, 6.0, 7.0}
*
* @param scalar The scalar to multiply by.
* @return The scaled MecanumDriveWheelSpeeds.
*/
constexpr MecanumDriveWheelSpeeds operator*(double scalar) const {
return {scalar * frontLeft, scalar * frontRight, scalar * rearLeft,
scalar * rearRight};
}
/**
* Divides the MecanumDriveWheelSpeeds by a scalar and returns the new
* MecanumDriveWheelSpeeds.
*
* <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 1.5, 1.0} / 2 =
* MecanumDriveWheelSpeeds{1.0, 1.25, 0.75, 0.5}
*
* @param scalar The scalar to divide by.
* @return The scaled MecanumDriveWheelSpeeds.
*/
constexpr MecanumDriveWheelSpeeds operator/(double scalar) const {
return operator*(1.0 / scalar);
}
};
} // namespace wpi::math
#include "wpi/math/kinematics/proto/MecanumDriveWheelSpeedsProto.hpp"
#include "wpi/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.hpp"

153
wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelVelocities.hpp Normal file
View File

@@ -0,0 +1,153 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <algorithm>
#include <array>
#include "wpi/units/math.hpp"
#include "wpi/units/velocity.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace wpi::math {
/**
* Represents the wheel velocities for a mecanum drive drivetrain.
*/
struct WPILIB_DLLEXPORT MecanumDriveWheelVelocities {
/**
* Velocity of the front-left wheel.
*/
wpi::units::meters_per_second_t frontLeft = 0_mps;
/**
* Velocity of the front-right wheel.
*/
wpi::units::meters_per_second_t frontRight = 0_mps;
/**
* Velocity of the rear-left wheel.
*/
wpi::units::meters_per_second_t rearLeft = 0_mps;
/**
* Velocity of the rear-right wheel.
*/
wpi::units::meters_per_second_t rearRight = 0_mps;
/**
* Renormalizes the wheel velocities if any individual velocity is above the
* specified maximum.
*
* Sometimes, after inverse kinematics, the requested velocity from one or
* more wheels may be above the max attainable velocity for the driving motor
* on that wheel. To fix this issue, one can reduce all the wheel velocities
* to make sure that all requested module velocities are at-or-below the
* absolute threshold, while maintaining the ratio of velocities between
* wheels.
*
* @param attainableMaxVelocity The absolute max velocity that a wheel can
* reach.
* @return Desaturated MecanumDriveWheelVelocities.
*/
constexpr MecanumDriveWheelVelocities Desaturate(
wpi::units::meters_per_second_t attainableMaxVelocity) const {
std::array<wpi::units::meters_per_second_t, 4> wheelVelocities{
frontLeft, frontRight, rearLeft, rearRight};
wpi::units::meters_per_second_t realMaxVelocity =
wpi::units::math::abs(*std::max_element(
wheelVelocities.begin(), wheelVelocities.end(),
[](const auto& a, const auto& b) {
return wpi::units::math::abs(a) < wpi::units::math::abs(b);
}));
if (realMaxVelocity > attainableMaxVelocity) {
for (int i = 0; i < 4; ++i) {
wheelVelocities[i] =
wheelVelocities[i] / realMaxVelocity * attainableMaxVelocity;
}
return MecanumDriveWheelVelocities{wheelVelocities[0], wheelVelocities[1],
wheelVelocities[2],
wheelVelocities[3]};
}
return *this;
}
/**
* Adds two MecanumDriveWheelVelocities and returns the sum.
*
* For example, MecanumDriveWheelVelocities{1.0, 0.5, 2.0, 1.5} +
* MecanumDriveWheelVelocities{2.0, 1.5, 0.5, 1.0} =
* MecanumDriveWheelVelocities{3.0, 2.0, 2.5, 2.5}
*
* @param other The MecanumDriveWheelVelocities to add.
* @return The sum of the MecanumDriveWheelVelocities.
*/
constexpr MecanumDriveWheelVelocities operator+(
const MecanumDriveWheelVelocities& other) const {
return {frontLeft + other.frontLeft, frontRight + other.frontRight,
rearLeft + other.rearLeft, rearRight + other.rearRight};
}
/**
* Subtracts the other MecanumDriveWheelVelocities from the current
* MecanumDriveWheelVelocities and returns the difference.
*
* For example, MecanumDriveWheelVelocities{5.0, 4.0, 6.0, 2.5} -
* MecanumDriveWheelVelocities{1.0, 2.0, 3.0, 0.5} =
* MecanumDriveWheelVelocities{4.0, 2.0, 3.0, 2.0}
*
* @param other The MecanumDriveWheelVelocities to subtract.
* @return The difference between the two MecanumDriveWheelVelocities.
*/
constexpr MecanumDriveWheelVelocities operator-(
const MecanumDriveWheelVelocities& other) const {
return *this + -other;
}
/**
* Returns the inverse of the current MecanumDriveWheelVelocities.
* This is equivalent to negating all components of the
* MecanumDriveWheelVelocities.
*
* @return The inverse of the current MecanumDriveWheelVelocities.
*/
constexpr MecanumDriveWheelVelocities operator-() const {
return {-frontLeft, -frontRight, -rearLeft, -rearRight};
}
/**
* Multiplies the MecanumDriveWheelVelocities by a scalar and returns the new
* MecanumDriveWheelVelocities.
*
* For example, MecanumDriveWheelVelocities{2.0, 2.5, 3.0, 3.5} * 2 =
* MecanumDriveWheelVelocities{4.0, 5.0, 6.0, 7.0}
*
* @param scalar The scalar to multiply by.
* @return The scaled MecanumDriveWheelVelocities.
*/
constexpr MecanumDriveWheelVelocities operator*(double scalar) const {
return {scalar * frontLeft, scalar * frontRight, scalar * rearLeft,
scalar * rearRight};
}
/**
* Divides the MecanumDriveWheelVelocities by a scalar and returns the new
* MecanumDriveWheelVelocities.
*
* For example, MecanumDriveWheelVelocities{2.0, 2.5, 1.5, 1.0} / 2 =
* MecanumDriveWheelVelocities{1.0, 1.25, 0.75, 0.5}
*
* @param scalar The scalar to divide by.
* @return The scaled MecanumDriveWheelVelocities.
*/
constexpr MecanumDriveWheelVelocities operator/(double scalar) const {
return operator*(1.0 / scalar);
}
};
} // namespace wpi::math
#include "wpi/math/kinematics/proto/MecanumDriveWheelVelocitiesProto.hpp"
#include "wpi/math/kinematics/struct/MecanumDriveWheelVelocitiesStruct.hpp"

8
wpimath/src/main/native/include/wpi/math/kinematics/Odometry.hpp
View File

@@ -23,10 +23,10 @@ namespace wpi::math {
* when using computer-vision systems.
*
* @tparam WheelPositions Wheel positions type.
* @tparam WheelSpeeds Wheel speeds type.
* @tparam WheelVelocities Wheel velocities type.
* @tparam WheelAccelerations Wheel accelerations type.
*/
template <typename WheelPositions, typename WheelSpeeds,
template <typename WheelPositions, typename WheelVelocities,
typename WheelAccelerations>
class WPILIB_DLLEXPORT Odometry {
public:
@@ -38,7 +38,7 @@ class WPILIB_DLLEXPORT Odometry {
* @param wheelPositions The current distances measured by each wheel.
* @param initialPose The starting position of the robot on the field.
*/
explicit Odometry(const Kinematics<WheelPositions, WheelSpeeds,
explicit Odometry(const Kinematics<WheelPositions, WheelVelocities,
WheelAccelerations>& kinematics,
const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions,
@@ -135,7 +135,7 @@ class WPILIB_DLLEXPORT Odometry {
}
private:
const Kinematics<WheelPositions, WheelSpeeds, WheelAccelerations>&
const Kinematics<WheelPositions, WheelVelocities, WheelAccelerations>&
m_kinematics;
Pose2d m_pose;

8
wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp
View File

@@ -23,10 +23,10 @@ namespace wpi::math {
* when using computer-vision systems.
*
* @tparam WheelPositions Wheel positions type.
* @tparam WheelSpeeds Wheel speeds type.
* @tparam WheelVelocities Wheel velocities type.
* @tparam WheelAccelerations Wheel accelerations type.
*/
template <typename WheelPositions, typename WheelSpeeds,
template <typename WheelPositions, typename WheelVelocities,
typename WheelAccelerations>
class WPILIB_DLLEXPORT Odometry3d {
public:
@@ -38,7 +38,7 @@ class WPILIB_DLLEXPORT Odometry3d {
* @param wheelPositions The current distances measured by each wheel.
* @param initialPose The starting position of the robot on the field.
*/
explicit Odometry3d(const Kinematics<WheelPositions, WheelSpeeds,
explicit Odometry3d(const Kinematics<WheelPositions, WheelVelocities,
WheelAccelerations>& kinematics,
const Rotation3d& gyroAngle,
const WheelPositions& wheelPositions,
@@ -147,7 +147,7 @@ class WPILIB_DLLEXPORT Odometry3d {
}
private:
const Kinematics<WheelPositions, WheelSpeeds, WheelAccelerations>&
const Kinematics<WheelPositions, WheelVelocities, WheelAccelerations>&
m_kinematics;
Pose3d m_pose;

327
wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp
View File

@@ -14,11 +14,11 @@
#include "wpi/math/geometry/Translation2d.hpp"
#include "wpi/math/geometry/Twist2d.hpp"
#include "wpi/math/kinematics/ChassisAccelerations.hpp"
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include "wpi/math/kinematics/Kinematics.hpp"
#include "wpi/math/kinematics/SwerveModuleAcceleration.hpp"
#include "wpi/math/kinematics/SwerveModulePosition.hpp"
#include "wpi/math/kinematics/SwerveModuleState.hpp"
#include "wpi/math/kinematics/SwerveModuleVelocity.hpp"
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/math/util/MathShared.hpp"
#include "wpi/units/math.hpp"
@@ -30,7 +30,7 @@ namespace wpi::math {
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components)
* into individual module states (speed and angle).
* into individual module states (velocity and angle).
*
* The inverse kinematics (converting from a desired chassis velocity to
* individual module states) uses the relative locations of the modules with
@@ -43,9 +43,10 @@ namespace wpi::math {
* does. Since this is an overdetermined system (more equations than variables),
* we use a least-squares approximation.
*
* The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds]
* We take the Moore-Penrose pseudoinverse of [moduleLocations] and then
* multiply by [moduleStates] to get our chassis speeds.
* The inverse kinematics: [moduleVelocities] = [moduleLocations] *
* [chassisVelocities] We take the Moore-Penrose pseudoinverse of
* [moduleLocations] and then multiply by [moduleVelocities] to get our chassis
* velocities.
*
* Forward kinematics is also used for odometry -- determining the position of
* the robot on the field using encoders and a gyro.
@@ -54,7 +55,7 @@ template <size_t NumModules>
class SwerveDriveKinematics
: public Kinematics<
wpi::util::array<SwerveModulePosition, NumModules>,
wpi::util::array<SwerveModuleState, NumModules>,
wpi::util::array<SwerveModuleVelocity, NumModules>,
wpi::util::array<SwerveModuleAcceleration, NumModules>> {
public:
/**
@@ -66,7 +67,7 @@ class SwerveDriveKinematics
* kinematics methods.
*
* @param moduleTranslations The locations of the modules relative to the
* physical center of the robot.
* physical center of the robot.
*/
template <std::convertible_to<Translation2d>... ModuleTranslations>
requires(sizeof...(ModuleTranslations) == NumModules)
@@ -120,8 +121,9 @@ class SwerveDriveKinematics
/**
* Reset the internal swerve module headings.
*
* @param moduleHeadings The swerve module headings. The order of the module
* headings should be same as passed into the constructor of this class.
* headings should be same as passed into the constructor of this class.
*/
template <std::convertible_to<Rotation2d>... ModuleHeadings>
requires(sizeof...(ModuleHeadings) == NumModules)
@@ -132,8 +134,9 @@ class SwerveDriveKinematics
/**
* Reset the internal swerve module headings.
*
* @param moduleHeadings The swerve module headings. The order of the module
* headings should be same as passed into the constructor of this class.
* headings should be same as passed into the constructor of this class.
*/
void ResetHeadings(wpi::util::array<Rotation2d, NumModules> moduleHeadings) {
for (size_t i = 0; i < NumModules; i++) {
@@ -144,7 +147,7 @@ class SwerveDriveKinematics
/**
* Performs inverse kinematics to return the module states from a desired
* chassis velocity. This method is often used to convert joystick values into
* module speeds and angles.
* module velocities and angles.
*
* This function also supports variable centers of rotation. During normal
* operations, the center of rotation is usually the same as the physical
@@ -152,39 +155,41 @@ class SwerveDriveKinematics
* However, if you wish to change the center of rotation for evasive
* maneuvers, vision alignment, or for any other use case, you can do so.
*
* In the case that the desired chassis speeds are zero (i.e. the robot will
* be stationary), the previously calculated module angle will be maintained.
* In the case that the desired chassis velocities are zero (i.e. the robot
* will be stationary), the previously calculated module angle will be
* maintained.
*
* @param chassisSpeeds The desired chassis speed.
* @param chassisVelocities The desired chassis velocity.
* @param centerOfRotation The center of rotation. For example, if you set the
* center of rotation at one corner of the robot and provide a chassis speed
* that only has a dtheta component, the robot will rotate around that corner.
*
* center of rotation at one corner of the robot and provide a chassis
* velocity that only has a dtheta component, the robot will rotate around
* that corner.
* @return An array containing the module states. Use caution because these
* module states are not normalized. Sometimes, a user input may cause one of
* the module speeds to go above the attainable max velocity. Use the
* DesaturateWheelSpeeds(wpi::util::array<SwerveModuleState, NumModules>*,
* wpi::units::meters_per_second_t) function to rectify this issue. In
* addition, you can leverage the power of C++17 to directly assign the module
* states to variables:
* module states are not normalized. Sometimes, a user input may cause one
* of the module velocities to go above the attainable max velocity. Use
* the DesaturateWheelVelocities(wpi::util::array<SwerveModuleVelocity,
* NumModules>*, wpi::units::meters_per_second_t) function to rectify this
* issue. In addition, you can leverage the power of C++17 to directly
* assign the module states to variables:
*
* @code{.cpp}
* auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(chassisSpeeds);
* auto [fl, fr, bl, br] =
* kinematics.ToSwerveModuleVelocities(chassisVelocities);
* @endcode
*/
wpi::util::array<SwerveModuleState, NumModules> ToSwerveModuleStates(
const ChassisSpeeds& chassisSpeeds,
wpi::util::array<SwerveModuleVelocity, NumModules> ToSwerveModuleVelocities(
const ChassisVelocities& chassisVelocities,
const Translation2d& centerOfRotation = Translation2d{}) const {
wpi::util::array<SwerveModuleState, NumModules> moduleStates(
wpi::util::array<SwerveModuleVelocity, NumModules> moduleVelocities(
wpi::util::empty_array);
if (chassisSpeeds.vx == 0_mps && chassisSpeeds.vy == 0_mps &&
chassisSpeeds.omega == 0_rad_per_s) {
if (chassisVelocities.vx == 0_mps && chassisVelocities.vy == 0_mps &&
chassisVelocities.omega == 0_rad_per_s) {
for (size_t i = 0; i < NumModules; i++) {
moduleStates[i] = {0_mps, m_moduleHeadings[i]};
moduleVelocities[i] = {0_mps, m_moduleHeadings[i]};
}
return moduleStates;
return moduleVelocities;
}
// We have a new center of rotation. We need to compute the matrix again.
@@ -192,83 +197,85 @@ class SwerveDriveKinematics
SetInverseKinematics(centerOfRotation);
}
Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.value(),
chassisSpeeds.vy.value(),
chassisSpeeds.omega.value()};
Eigen::Vector3d chassisVelocitiesVector{chassisVelocities.vx.value(),
chassisVelocities.vy.value(),
chassisVelocities.omega.value()};
Matrixd<NumModules * 2, 1> moduleStateMatrix =
m_firstOrderInverseKinematics * chassisSpeedsVector;
Matrixd<NumModules * 2, 1> moduleVelocityMatrix =
m_firstOrderInverseKinematics * chassisVelocitiesVector;
for (size_t i = 0; i < NumModules; i++) {
wpi::units::meters_per_second_t x{moduleStateMatrix(i * 2, 0)};
wpi::units::meters_per_second_t y{moduleStateMatrix(i * 2 + 1, 0)};
wpi::units::meters_per_second_t x{moduleVelocityMatrix(i * 2, 0)};
wpi::units::meters_per_second_t y{moduleVelocityMatrix(i * 2 + 1, 0)};
auto speed = wpi::units::math::hypot(x, y);
auto rotation = speed > 1e-6_mps ? Rotation2d{x.value(), y.value()}
: m_moduleHeadings[i];
auto velocity = wpi::units::math::hypot(x, y);
auto rotation = velocity > 1e-6_mps ? Rotation2d{x.value(), y.value()}
: m_moduleHeadings[i];
moduleStates[i] = {speed, rotation};
moduleVelocities[i] = {velocity, rotation};
m_moduleHeadings[i] = rotation;
}
return moduleStates;
return moduleVelocities;
}
wpi::util::array<SwerveModuleState, NumModules> ToWheelSpeeds(
const ChassisSpeeds& chassisSpeeds) const override {
return ToSwerveModuleStates(chassisSpeeds);
wpi::util::array<SwerveModuleVelocity, NumModules> ToWheelVelocities(
const ChassisVelocities& chassisVelocities) const override {
return ToSwerveModuleVelocities(chassisVelocities);
}
/**
* Performs forward kinematics to return the resulting chassis state from the
* given module states. This method is often used for odometry -- determining
* the robot's position on the field using data from the real-world speed and
* angle of each module on the robot.
* the robot's position on the field using data from the real-world velocity
* and angle of each module on the robot.
*
* @param moduleStates The state of the modules (as a SwerveModuleState type)
* as measured from respective encoders and gyros. The order of the swerve
* module states should be same as passed into the constructor of this class.
*
* @return The resulting chassis speed.
* @param moduleVelocities The state of the modules (as a SwerveModuleVelocity
* type) as measured from respective encoders and gyros. The order of the
* swerve module states should be same as passed into the constructor of
* this class.
* @return The resulting chassis velocity.
*/
template <std::convertible_to<SwerveModuleState>... ModuleStates>
requires(sizeof...(ModuleStates) == NumModules)
ChassisSpeeds ToChassisSpeeds(ModuleStates&&... moduleStates) const {
return this->ToChassisSpeeds(
wpi::util::array<SwerveModuleState, NumModules>{moduleStates...});
template <std::convertible_to<SwerveModuleVelocity>... ModuleVelocities>
requires(sizeof...(ModuleVelocities) == NumModules)
ChassisVelocities ToChassisVelocities(
ModuleVelocities&&... moduleVelocities) const {
return this->ToChassisVelocities(
wpi::util::array<SwerveModuleVelocity, NumModules>{
moduleVelocities...});
}
/**
* Performs forward kinematics to return the resulting chassis state from the
* given module states. This method is often used for odometry -- determining
* the robot's position on the field using data from the real-world speed and
* angle of each module on the robot.
* the robot's position on the field using data from the real-world velocity
* and angle of each module on the robot.
*
* @param moduleStates The state of the modules as an wpi::util::array of type
* SwerveModuleState, NumModules long as measured from respective encoders
* and gyros. The order of the swerve module states should be same as passed
* into the constructor of this class.
*
* @return The resulting chassis speed.
* @param moduleVelocities The state of the modules as an wpi::util::array of
* type SwerveModuleVelocity, NumModules long as measured from respective
* encoders and gyros. The order of the swerve module states should be
* same as passed into the constructor of this class.
* @return The resulting chassis velocity.
*/
ChassisSpeeds ToChassisSpeeds(
const wpi::util::array<SwerveModuleState, NumModules>& moduleStates)
const override {
Matrixd<NumModules * 2, 1> moduleStateMatrix;
ChassisVelocities ToChassisVelocities(
const wpi::util::array<SwerveModuleVelocity, NumModules>&
moduleVelocities) const override {
Matrixd<NumModules * 2, 1> moduleVelocityMatrix;
for (size_t i = 0; i < NumModules; ++i) {
SwerveModuleState module = moduleStates[i];
moduleStateMatrix(i * 2, 0) = module.speed.value() * module.angle.Cos();
moduleStateMatrix(i * 2 + 1, 0) =
module.speed.value() * module.angle.Sin();
SwerveModuleVelocity module = moduleVelocities[i];
moduleVelocityMatrix(i * 2, 0) =
module.velocity.value() * module.angle.Cos();
moduleVelocityMatrix(i * 2 + 1, 0) =
module.velocity.value() * module.angle.Sin();
}
Eigen::Vector3d chassisSpeedsVector =
m_firstOrderForwardKinematics.solve(moduleStateMatrix);
Eigen::Vector3d chassisVelocitiesVector =
m_firstOrderForwardKinematics.solve(moduleVelocityMatrix);
return {wpi::units::meters_per_second_t{chassisSpeedsVector(0)},
wpi::units::meters_per_second_t{chassisSpeedsVector(1)},
wpi::units::radians_per_second_t{chassisSpeedsVector(2)}};
return {wpi::units::meters_per_second_t{chassisVelocitiesVector(0)},
wpi::units::meters_per_second_t{chassisVelocitiesVector(1)},
wpi::units::radians_per_second_t{chassisVelocitiesVector(2)}};
}
/**
@@ -278,10 +285,9 @@ class SwerveDriveKinematics
* real-world position delta and angle of each module on the robot.
*
* @param moduleDeltas The latest change in position of the modules (as a
* SwerveModulePosition type) as measured from respective encoders and gyros.
* The order of the swerve module states should be same as passed into the
* constructor of this class.
*
* SwerveModulePosition type) as measured from respective encoders and
* gyros. The order of the swerve module states should be same as passed
* into the constructor of this class.
* @return The resulting Twist2d.
*/
template <std::convertible_to<SwerveModulePosition>... ModuleDeltas>
@@ -298,10 +304,9 @@ class SwerveDriveKinematics
* real-world position delta and angle of each module on the robot.
*
* @param moduleDeltas The latest change in position of the modules (as a
* SwerveModulePosition type) as measured from respective encoders and gyros.
* The order of the swerve module states should be same as passed into the
* constructor of this class.
*
* SwerveModulePosition type) as measured from respective encoders and
* gyros. The order of the swerve module states should be same as passed
* into the constructor of this class.
* @return The resulting Twist2d.
*/
Twist2d ToTwist2d(
@@ -337,105 +342,109 @@ class SwerveDriveKinematics
}
/**
* Renormalizes the wheel speeds if any individual speed is above the
* Renormalizes the wheel velocities if any individual velocity is above the
* specified maximum.
*
* Sometimes, after inverse kinematics, the requested speed
* from one or more modules may be above the max attainable speed for the
* Sometimes, after inverse kinematics, the requested velocity
* from one or more modules may be above the max attainable velocity for the
* driving motor on that module. To fix this issue, one can reduce all the
* wheel speeds to make sure that all requested module speeds are at-or-below
* the absolute threshold, while maintaining the ratio of speeds between
* modules.
* wheel velocities to make sure that all requested module velocities are
* at-or-below the absolute threshold, while maintaining the ratio of
* velocities between modules.
*
* Scaling down the module speeds rotates the direction of net motion in the
* opposite direction of rotational velocity, which makes discretizing the
* chassis speeds inaccurate because the discretization did not account for
* this translational skew.
* Scaling down the module velocities rotates the direction of net motion in
* the opposite direction of rotational velocity, which makes discretizing the
* chassis velocities inaccurate because the discretization did not account
* for this translational skew.
*
* @param moduleStates Reference to array of module states. The array will be
* mutated with the normalized speeds!
* @param attainableMaxSpeed The absolute max speed that a module can reach.
* @param moduleVelocities Reference to array of module states. The array will
* be mutated with the normalized velocities!
* @param attainableMaxVelocity The absolute max velocity that a module can
* reach.
*/
static void DesaturateWheelSpeeds(
wpi::util::array<SwerveModuleState, NumModules>* moduleStates,
wpi::units::meters_per_second_t attainableMaxSpeed) {
auto& states = *moduleStates;
auto realMaxSpeed = wpi::units::math::abs(
static void DesaturateWheelVelocities(
wpi::util::array<SwerveModuleVelocity, NumModules>* moduleVelocities,
wpi::units::meters_per_second_t attainableMaxVelocity) {
auto& states = *moduleVelocities;
auto realMaxVelocity = wpi::units::math::abs(
std::max_element(states.begin(), states.end(),
[](const auto& a, const auto& b) {
return wpi::units::math::abs(a.speed) <
wpi::units::math::abs(b.speed);
return wpi::units::math::abs(a.velocity) <
wpi::units::math::abs(b.velocity);
})
->speed);
->velocity);
if (realMaxSpeed > attainableMaxSpeed) {
if (realMaxVelocity > attainableMaxVelocity) {
for (auto& module : states) {
module.speed = module.speed / realMaxSpeed * attainableMaxSpeed;
module.velocity =
module.velocity / realMaxVelocity * attainableMaxVelocity;
}
}
}
/**
* Renormalizes the wheel speeds if any individual speed is above the
* Renormalizes the wheel velocities if any individual velocity is above the
* specified maximum, as well as getting rid of joystick saturation at edges
* of joystick.
*
* Sometimes, after inverse kinematics, the requested speed
* from one or more modules may be above the max attainable speed for the
* Sometimes, after inverse kinematics, the requested velocity
* from one or more modules may be above the max attainable velocity for the
* driving motor on that module. To fix this issue, one can reduce all the
* wheel speeds to make sure that all requested module speeds are at-or-below
* the absolute threshold, while maintaining the ratio of speeds between
* modules.
* wheel velocities to make sure that all requested module velocities are
* at-or-below the absolute threshold, while maintaining the ratio of
* velocities between modules.
*
* Scaling down the module speeds rotates the direction of net motion in the
* opposite direction of rotational velocity, which makes discretizing the
* chassis speeds inaccurate because the discretization did not account for
* this translational skew.
* Scaling down the module velocities rotates the direction of net motion in
* the opposite direction of rotational velocity, which makes discretizing the
* chassis velocities inaccurate because the discretization did not account
* for this translational skew.
*
* @param moduleStates Reference to array of module states. The array will be
* mutated with the normalized speeds!
* @param desiredChassisSpeed The desired speed of the robot
* @param attainableMaxModuleSpeed The absolute max speed a module can reach
* @param attainableMaxRobotTranslationSpeed The absolute max speed the robot
* can reach while translating
* @param attainableMaxRobotRotationSpeed The absolute max speed the robot can
* reach while rotating
* @param moduleVelocities Reference to array of module states. The array will
* be mutated with the normalized velocities!
* @param desiredChassisVelocity The desired velocity of the robot
* @param attainableMaxModuleVelocity The absolute max velocity a module can
* reach
* @param attainableMaxRobotTranslationVelocity The absolute max velocity the
* robot can reach while translating
* @param attainableMaxRobotRotationVelocity The absolute max velocity the
* robot can reach while rotating
*/
static void DesaturateWheelSpeeds(
wpi::util::array<SwerveModuleState, NumModules>* moduleStates,
ChassisSpeeds desiredChassisSpeed,
wpi::units::meters_per_second_t attainableMaxModuleSpeed,
wpi::units::meters_per_second_t attainableMaxRobotTranslationSpeed,
wpi::units::radians_per_second_t attainableMaxRobotRotationSpeed) {
auto& states = *moduleStates;
static void DesaturateWheelVelocities(
wpi::util::array<SwerveModuleVelocity, NumModules>* moduleVelocities,
ChassisVelocities desiredChassisVelocity,
wpi::units::meters_per_second_t attainableMaxModuleVelocity,
wpi::units::meters_per_second_t attainableMaxRobotTranslationVelocity,
wpi::units::radians_per_second_t attainableMaxRobotRotationVelocity) {
auto& states = *moduleVelocities;
auto realMaxSpeed = wpi::units::math::abs(
auto realMaxVelocity = wpi::units::math::abs(
std::max_element(states.begin(), states.end(),
[](const auto& a, const auto& b) {
return wpi::units::math::abs(a.speed) <
wpi::units::math::abs(b.speed);
return wpi::units::math::abs(a.velocity) <
wpi::units::math::abs(b.velocity);
})
->speed);
->velocity);
if (attainableMaxRobotTranslationSpeed == 0_mps ||
attainableMaxRobotRotationSpeed == 0_rad_per_s ||
realMaxSpeed == 0_mps) {
if (attainableMaxRobotTranslationVelocity == 0_mps ||
attainableMaxRobotRotationVelocity == 0_rad_per_s ||
realMaxVelocity == 0_mps) {
return;
}
auto translationalK = wpi::units::math::hypot(desiredChassisSpeed.vx,
desiredChassisSpeed.vy) /
attainableMaxRobotTranslationSpeed;
auto translationalK = wpi::units::math::hypot(desiredChassisVelocity.vx,
desiredChassisVelocity.vy) /
attainableMaxRobotTranslationVelocity;
auto rotationalK = wpi::units::math::abs(desiredChassisSpeed.omega) /
attainableMaxRobotRotationSpeed;
auto rotationalK = wpi::units::math::abs(desiredChassisVelocity.omega) /
attainableMaxRobotRotationVelocity;
auto k = wpi::units::math::max(translationalK, rotationalK);
auto scale = wpi::units::math::min(
k * attainableMaxModuleSpeed / realMaxSpeed, wpi::units::scalar_t{1});
auto scale =
wpi::units::math::min(k * attainableMaxModuleVelocity / realMaxVelocity,
wpi::units::scalar_t{1});
for (auto& module : states) {
module.speed = module.speed * scale;
module.velocity = module.velocity * scale;
}
}
@@ -461,7 +470,7 @@ class SwerveDriveKinematics
* calculations -- converting desired robot accelerations into individual
* module accelerations.
*
* <p>This function also supports variable centers of rotation. During normal
* This function also supports variable centers of rotation. During normal
* operations, the center of rotation is usually the same as the physical
* center of the robot; therefore, the argument is defaulted to that use case.
* However, if you wish to change the center of rotation for evasive
@@ -470,9 +479,9 @@ class SwerveDriveKinematics
* @param chassisAccelerations The desired chassis accelerations.
* @param angularVelocity The desired robot angular velocity.
* @param centerOfRotation The center of rotation. For example, if you set the
* center of rotation at one corner of the robot and provide a chassis
* acceleration that only has a dtheta component, the robot will rotate around
* that corner.
* center of rotation at one corner of the robot and provide a chassis
* acceleration that only has a dtheta component, the robot will rotate
* around that corner.
* @return An array containing the module accelerations.
*/
wpi::util::array<SwerveModuleAcceleration, NumModules>
@@ -551,8 +560,9 @@ class SwerveDriveKinematics
* data from the real-world acceleration of each module on the robot.
*
* @param moduleAccelerations The accelerations of the modules as measured
* from respective encoders and gyros. The order of the swerve module
* accelerations should be same as passed into the constructor of this class.
* from respective encoders and gyros. The order of the swerve module
* accelerations should be same as passed into the constructor of this
* class.
* @return The resulting chassis accelerations.
*/
template <
@@ -572,8 +582,9 @@ class SwerveDriveKinematics
* data from the real-world acceleration of each module on the robot.
*
* @param moduleAccelerations The accelerations of the modules as measured
* from respective encoders and gyros. The order of the swerve module
* accelerations should be same as passed into the constructor of this class.
* from respective encoders and gyros. The order of the swerve module
* accelerations should be same as passed into the constructor of this
* class.
* @return The resulting chassis accelerations.
*/
ChassisAccelerations ToChassisAccelerations(

4
wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry.hpp
View File

@@ -10,7 +10,7 @@
#include "wpi/math/kinematics/Odometry.hpp"
#include "wpi/math/kinematics/SwerveDriveKinematics.hpp"
#include "wpi/math/kinematics/SwerveModulePosition.hpp"
#include "wpi/math/kinematics/SwerveModuleState.hpp"
#include "wpi/math/kinematics/SwerveModuleVelocity.hpp"
#include "wpi/math/util/MathShared.hpp"
#include "wpi/util/SymbolExports.hpp"
@@ -28,7 +28,7 @@ namespace wpi::math {
template <size_t NumModules>
class SwerveDriveOdometry
: public Odometry<wpi::util::array<SwerveModulePosition, NumModules>,
wpi::util::array<SwerveModuleState, NumModules>,
wpi::util::array<SwerveModuleVelocity, NumModules>,
wpi::util::array<SwerveModuleAcceleration, NumModules>> {
public:
/**

4
wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry3d.hpp
View File

@@ -10,7 +10,7 @@
#include "wpi/math/kinematics/Odometry3d.hpp"
#include "wpi/math/kinematics/SwerveDriveKinematics.hpp"
#include "wpi/math/kinematics/SwerveModulePosition.hpp"
#include "wpi/math/kinematics/SwerveModuleState.hpp"
#include "wpi/math/kinematics/SwerveModuleVelocity.hpp"
#include "wpi/util/SymbolExports.hpp"
namespace wpi::math {
@@ -28,7 +28,7 @@ template <size_t NumModules>
class SwerveDriveOdometry3d
: public Odometry3d<
wpi::util::array<SwerveModulePosition, NumModules>,
wpi::util::array<SwerveModuleState, NumModules>,
wpi::util::array<SwerveModuleVelocity, NumModules>,
wpi::util::array<SwerveModuleAcceleration, NumModules>> {
public:
/**

42
wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleState.hpp → wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleVelocity.hpp
View File

@@ -12,13 +12,13 @@
namespace wpi::math {
/**
* Represents the state of one swerve module.
* Represents the velocity of one swerve module.
*/
struct WPILIB_DLLEXPORT SwerveModuleState {
struct WPILIB_DLLEXPORT SwerveModuleVelocity {
/**
* Speed of the wheel of the module.
* Velocity of the wheel of the module.
*/
wpi::units::meters_per_second_t speed = 0_mps;
wpi::units::meters_per_second_t velocity = 0_mps;
/**
* Angle of the module.
@@ -26,18 +26,18 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
Rotation2d angle;
/**
* Checks equality between this SwerveModuleState and another object.
* Checks equality between this SwerveModuleVelocity and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
constexpr bool operator==(const SwerveModuleState& other) const {
return wpi::units::math::abs(speed - other.speed) < 1E-9_mps &&
constexpr bool operator==(const SwerveModuleVelocity& other) const {
return wpi::units::math::abs(velocity - other.velocity) < 1E-9_mps &&
angle == other.angle;
}
/**
* Minimize the change in the heading this swerve module state would
* Minimize the change in the heading this swerve module velocity would
* require by potentially reversing the direction the wheel spins. If this is
* used with the PIDController class's continuous input functionality, the
* furthest a wheel will ever rotate is 90 degrees.
@@ -47,43 +47,45 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
constexpr void Optimize(const Rotation2d& currentAngle) {
auto delta = angle - currentAngle;
if (wpi::units::math::abs(delta.Degrees()) > 90_deg) {
speed *= -1;
velocity *= -1;
angle = angle + Rotation2d{180_deg};
}
}
/**
* Minimize the change in heading the desired swerve module state would
* Minimize the change in heading the desired swerve module velocity would
* require by potentially reversing the direction the wheel spins. If this is
* used with the PIDController class's continuous input functionality, the
* furthest a wheel will ever rotate is 90 degrees.
*
* @param desiredState The desired state.
* @param desiredVelocity The desired velocity.
* @param currentAngle The current module angle.
*/
[[deprecated("Use instance method instead.")]]
constexpr static SwerveModuleState Optimize(
const SwerveModuleState& desiredState, const Rotation2d& currentAngle) {
auto delta = desiredState.angle - currentAngle;
constexpr static SwerveModuleVelocity Optimize(
const SwerveModuleVelocity& desiredVelocity,
const Rotation2d& currentAngle) {
auto delta = desiredVelocity.angle - currentAngle;
if (wpi::units::math::abs(delta.Degrees()) > 90_deg) {
return {-desiredState.speed, desiredState.angle + Rotation2d{180_deg}};
return {-desiredVelocity.velocity,
desiredVelocity.angle + Rotation2d{180_deg}};
} else {
return {desiredState.speed, desiredState.angle};
return {desiredVelocity.velocity, desiredVelocity.angle};
}
}
/**
* Scales speed by cosine of angle error. This scales down movement
* Scales velocity by cosine of angle error. This scales down movement
* perpendicular to the desired direction of travel that can occur when
* modules change directions. This results in smoother driving.
*
* @param currentAngle The current module angle.
*/
constexpr void CosineScale(const Rotation2d& currentAngle) {
speed *= (angle - currentAngle).Cos();
velocity *= (angle - currentAngle).Cos();
}
};
} // namespace wpi::math
#include "wpi/math/kinematics/proto/SwerveModuleStateProto.hpp"
#include "wpi/math/kinematics/struct/SwerveModuleStateStruct.hpp"
#include "wpi/math/kinematics/proto/SwerveModuleVelocityProto.hpp"
#include "wpi/math/kinematics/struct/SwerveModuleVelocityStruct.hpp"

19
wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisSpeedsProto.hpp
View File

@@ -1,19 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/protobuf/Protobuf.hpp"
#include "wpimath/protobuf/kinematics.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::ChassisSpeeds> {
using MessageStruct = wpi_proto_ProtobufChassisSpeeds;
using InputStream = wpi::util::ProtoInputStream<wpi::math::ChassisSpeeds>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::ChassisSpeeds>;
static std::optional<wpi::math::ChassisSpeeds> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::ChassisSpeeds& value);
};

22
wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisVelocitiesProto.hpp Normal file
View File

@@ -0,0 +1,22 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/protobuf/Protobuf.hpp"
#include "wpimath/protobuf/kinematics.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::ChassisVelocities> {
using MessageStruct = wpi_proto_ProtobufChassisVelocities;
using InputStream = wpi::util::ProtoInputStream<wpi::math::ChassisVelocities>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::ChassisVelocities>;
static std::optional<wpi::math::ChassisVelocities> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const wpi::math::ChassisVelocities& value);
};

14
wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.hpp → wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelVelocitiesProto.hpp
View File

@@ -4,21 +4,21 @@
#pragma once
#include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/protobuf/Protobuf.hpp"
#include "wpimath/protobuf/kinematics.npb.h"
template <>
struct WPILIB_DLLEXPORT
wpi::util::Protobuf<wpi::math::DifferentialDriveWheelSpeeds> {
using MessageStruct = wpi_proto_ProtobufDifferentialDriveWheelSpeeds;
wpi::util::Protobuf<wpi::math::DifferentialDriveWheelVelocities> {
using MessageStruct = wpi_proto_ProtobufDifferentialDriveWheelVelocities;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::DifferentialDriveWheelSpeeds>;
wpi::util::ProtoInputStream<wpi::math::DifferentialDriveWheelVelocities>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::DifferentialDriveWheelSpeeds>;
static std::optional<wpi::math::DifferentialDriveWheelSpeeds> Unpack(
wpi::util::ProtoOutputStream<wpi::math::DifferentialDriveWheelVelocities>;
static std::optional<wpi::math::DifferentialDriveWheelVelocities> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const wpi::math::DifferentialDriveWheelSpeeds& value);
const wpi::math::DifferentialDriveWheelVelocities& value);
};

24
wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelSpeedsProto.hpp
View File

@@ -1,24 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/protobuf/Protobuf.hpp"
#include "wpimath/protobuf/kinematics.npb.h"
template <>
struct WPILIB_DLLEXPORT
wpi::util::Protobuf<wpi::math::MecanumDriveWheelSpeeds> {
using MessageStruct = wpi_proto_ProtobufMecanumDriveWheelSpeeds;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::MecanumDriveWheelSpeeds>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::MecanumDriveWheelSpeeds>;
static std::optional<wpi::math::MecanumDriveWheelSpeeds> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const wpi::math::MecanumDriveWheelSpeeds& value);
};

24
wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelVelocitiesProto.hpp Normal file
View File

@@ -0,0 +1,24 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "wpi/math/kinematics/MecanumDriveWheelVelocities.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/protobuf/Protobuf.hpp"
#include "wpimath/protobuf/kinematics.npb.h"
template <>
struct WPILIB_DLLEXPORT
wpi::util::Protobuf<wpi::math::MecanumDriveWheelVelocities> {
using MessageStruct = wpi_proto_ProtobufMecanumDriveWheelVelocities;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::MecanumDriveWheelVelocities>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::MecanumDriveWheelVelocities>;
static std::optional<wpi::math::MecanumDriveWheelVelocities> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const wpi::math::MecanumDriveWheelVelocities& value);
};

15
wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleStateProto.hpp → wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleVelocityProto.hpp
View File

@@ -4,19 +4,20 @@
#pragma once
#include "wpi/math/kinematics/SwerveModuleState.hpp"
#include "wpi/math/kinematics/SwerveModuleVelocity.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/protobuf/Protobuf.hpp"
#include "wpimath/protobuf/kinematics.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::SwerveModuleState> {
using MessageStruct = wpi_proto_ProtobufSwerveModuleState;
using InputStream = wpi::util::ProtoInputStream<wpi::math::SwerveModuleState>;
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::SwerveModuleVelocity> {
using MessageStruct = wpi_proto_ProtobufSwerveModuleVelocity;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::SwerveModuleVelocity>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::SwerveModuleState>;
static std::optional<wpi::math::SwerveModuleState> Unpack(
wpi::util::ProtoOutputStream<wpi::math::SwerveModuleVelocity>;
static std::optional<wpi::math::SwerveModuleVelocity> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const wpi::math::SwerveModuleState& value);
const wpi::math::SwerveModuleVelocity& value);
};

14
wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisSpeedsStruct.hpp → wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisVelocitiesStruct.hpp
View File

@@ -4,21 +4,23 @@
#pragma once
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::ChassisSpeeds> {
static constexpr std::string_view GetTypeName() { return "ChassisSpeeds"; }
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::ChassisVelocities> {
static constexpr std::string_view GetTypeName() {
return "ChassisVelocities";
}
static constexpr size_t GetSize() { return 24; }
static constexpr std::string_view GetSchema() {
return "double vx;double vy;double omega";
}
static wpi::math::ChassisSpeeds Unpack(std::span<const uint8_t> data);
static wpi::math::ChassisVelocities Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const wpi::math::ChassisSpeeds& value);
const wpi::math::ChassisVelocities& value);
};
static_assert(wpi::util::StructSerializable<wpi::math::ChassisSpeeds>);
static_assert(wpi::util::StructSerializable<wpi::math::ChassisVelocities>);

12
wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.hpp → wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelVelocitiesStruct.hpp
View File

@@ -4,26 +4,26 @@
#pragma once
#include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT
wpi::util::Struct<wpi::math::DifferentialDriveWheelSpeeds> {
wpi::util::Struct<wpi::math::DifferentialDriveWheelVelocities> {
static constexpr std::string_view GetTypeName() {
return "DifferentialDriveWheelSpeeds";
return "DifferentialDriveWheelVelocities";
}
static constexpr size_t GetSize() { return 16; }
static constexpr std::string_view GetSchema() {
return "double left;double right";
}
static wpi::math::DifferentialDriveWheelSpeeds Unpack(
static wpi::math::DifferentialDriveWheelVelocities Unpack(
std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const wpi::math::DifferentialDriveWheelSpeeds& value);
const wpi::math::DifferentialDriveWheelVelocities& value);
};
static_assert(
wpi::util::StructSerializable<wpi::math::DifferentialDriveWheelSpeeds>);
wpi::util::StructSerializable<wpi::math::DifferentialDriveWheelVelocities>);

13
wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.hpp → wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelVelocitiesStruct.hpp
View File

@@ -4,14 +4,15 @@
#pragma once
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelVelocities.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::MecanumDriveWheelSpeeds> {
struct WPILIB_DLLEXPORT
wpi::util::Struct<wpi::math::MecanumDriveWheelVelocities> {
static constexpr std::string_view GetTypeName() {
return "MecanumDriveWheelSpeeds";
return "MecanumDriveWheelVelocities";
}
static constexpr size_t GetSize() { return 32; }
static constexpr std::string_view GetSchema() {
@@ -19,11 +20,11 @@ struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::MecanumDriveWheelSpeeds> {
"double rear_right";
}
static wpi::math::MecanumDriveWheelSpeeds Unpack(
static wpi::math::MecanumDriveWheelVelocities Unpack(
std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const wpi::math::MecanumDriveWheelSpeeds& value);
const wpi::math::MecanumDriveWheelVelocities& value);
};
static_assert(
wpi::util::StructSerializable<wpi::math::MecanumDriveWheelSpeeds>);
wpi::util::StructSerializable<wpi::math::MecanumDriveWheelVelocities>);

16
wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleStateStruct.hpp → wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleVelocityStruct.hpp
View File

@@ -4,30 +4,30 @@
#pragma once
#include "wpi/math/kinematics/SwerveModuleState.hpp"
#include "wpi/math/kinematics/SwerveModuleVelocity.hpp"
#include "wpi/util/SymbolExports.hpp"
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::SwerveModuleState> {
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::SwerveModuleVelocity> {
static constexpr std::string_view GetTypeName() {
return "SwerveModuleState";
return "SwerveModuleVelocity";
}
static constexpr size_t GetSize() {
return 8 + wpi::util::GetStructSize<wpi::math::Rotation2d>();
}
static constexpr std::string_view GetSchema() {
return "double speed;Rotation2d angle";
return "double velocity;Rotation2d angle";
}
static wpi::math::SwerveModuleState Unpack(std::span<const uint8_t> data);
static wpi::math::SwerveModuleVelocity Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const wpi::math::SwerveModuleState& value);
const wpi::math::SwerveModuleVelocity& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::util::ForEachStructSchema<wpi::math::Rotation2d>(fn);
}
};
static_assert(wpi::util::StructSerializable<wpi::math::SwerveModuleState>);
static_assert(wpi::util::HasNestedStruct<wpi::math::SwerveModuleState>);
static_assert(wpi::util::StructSerializable<wpi::math::SwerveModuleVelocity>);
static_assert(wpi::util::HasNestedStruct<wpi::math::SwerveModuleVelocity>);

40
wpimath/src/main/native/include/wpi/math/system/DCMotor.hpp
View File

@@ -53,11 +53,11 @@ class WPILIB_DLLEXPORT DCMotor {
* Constructs a DC motor.
*
* @param nominalVoltage Voltage at which the motor constants were measured.
* @param stallTorque Torque when stalled.
* @param stallCurrent Current draw when stalled.
* @param freeCurrent Current draw under no load.
* @param freeSpeed Angular velocity under no load.
* @param numMotors Number of motors in a gearbox.
* @param stallTorque Torque when stalled.
* @param stallCurrent Current draw when stalled.
* @param freeCurrent Current draw under no load.
* @param freeSpeed Angular velocity under no load.
* @param numMotors Number of motors in a gearbox.
*/
constexpr DCMotor(wpi::units::volt_t nominalVoltage,
wpi::units::newton_meter_t stallTorque,
@@ -75,15 +75,15 @@ class WPILIB_DLLEXPORT DCMotor {
Kt(this->stallTorque / this->stallCurrent) {}
/**
* Returns current drawn by motor with given speed and input voltage.
* Returns current drawn by motor with given velocity and input voltage.
*
* @param speed The current angular velocity of the motor.
* @param velocity The current angular velocity of the motor.
* @param inputVoltage The voltage being applied to the motor.
*/
constexpr wpi::units::ampere_t Current(
wpi::units::radians_per_second_t speed,
wpi::units::radians_per_second_t velocity,
wpi::units::volt_t inputVoltage) const {
return -1.0 / Kv / R * speed + 1.0 / R * inputVoltage;
return -1.0 / Kv / R * velocity + 1.0 / R * inputVoltage;
}
/**
@@ -99,7 +99,7 @@ class WPILIB_DLLEXPORT DCMotor {
/**
* Returns torque produced by the motor with a given current.
*
* @param current The current drawn by the motor.
* @param current The current drawn by the motor.
*/
constexpr wpi::units::newton_meter_t Torque(
wpi::units::ampere_t current) const {
@@ -110,23 +110,23 @@ class WPILIB_DLLEXPORT DCMotor {
* Returns the voltage provided to the motor for a given torque and
* angular velocity.
*
* @param torque The torque produced by the motor.
* @param speed The current angular velocity of the motor.
* @param torque The torque produced by the motor.
* @param velocity The current angular velocity of the motor.
*/
constexpr wpi::units::volt_t Voltage(
wpi::units::newton_meter_t torque,
wpi::units::radians_per_second_t speed) const {
return 1.0 / Kv * speed + 1.0 / Kt * R * torque;
wpi::units::radians_per_second_t velocity) const {
return 1.0 / Kv * velocity + 1.0 / Kt * R * torque;
}
/**
* Returns the angular speed produced by the motor at a given torque and input
* voltage.
* Returns the angular velocity produced by the motor at a given torque and
* input voltage.
*
* @param torque The torque produced by the motor.
* @param inputVoltage The input voltage provided to the motor.
* @param torque The torque produced by the motor.
* @param inputVoltage The input voltage provided to the motor.
*/
constexpr wpi::units::radians_per_second_t Speed(
constexpr wpi::units::radians_per_second_t Velocity(
wpi::units::newton_meter_t torque,
wpi::units::volt_t inputVoltage) const {
return inputVoltage * Kv - 1.0 / Kt * torque * R * Kv;
@@ -135,7 +135,7 @@ class WPILIB_DLLEXPORT DCMotor {
/**
* Returns a copy of this motor with the given gearbox reduction applied.
*
* @param gearboxReduction The gearbox reduction.
* @param gearboxReduction The gearbox reduction.
*/
constexpr DCMotor WithReduction(double gearboxReduction) {
return DCMotor(nominalVoltage, stallTorque * gearboxReduction, stallCurrent,

2
wpimath/src/main/native/include/wpi/math/trajectory/Trajectory.hpp
View File

@@ -34,7 +34,7 @@ class WPILIB_DLLEXPORT Trajectory {
/// The time elapsed since the beginning of the trajectory.
wpi::units::second_t t = 0_s;
/// The speed at that point of the trajectory.
/// The velocity at that point of the trajectory.
wpi::units::meters_per_second_t velocity = 0_mps;
/// The acceleration at that point of the trajectory.

40
wpimath/src/main/native/include/wpi/math/trajectory/TrapezoidProfile.hpp
View File

@@ -161,27 +161,28 @@ class TrapezoidProfile {
wpi::units::second_t accelerationTime =
m_constraints.maxVelocity / m_constraints.maxAcceleration;
Distance_t fullSpeedDist =
Distance_t fullVelocityDist =
fullTrapezoidDist -
accelerationTime * accelerationTime * m_constraints.maxAcceleration;
// Handle the case where the profile never reaches full speed
if (fullSpeedDist < Distance_t{0}) {
// Handle the case where the profile never reaches full velocity
if (fullVelocityDist < Distance_t{0}) {
accelerationTime = wpi::units::math::sqrt(fullTrapezoidDist /
m_constraints.maxAcceleration);
fullSpeedDist = Distance_t{0};
fullVelocityDist = Distance_t{0};
}
m_endAccel = accelerationTime - cutoffBegin;
m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
m_endDecel = m_endFullSpeed + accelerationTime - cutoffEnd;
m_endFullVelocity =
m_endAccel + fullVelocityDist / m_constraints.maxVelocity;
m_endDecel = m_endFullVelocity + accelerationTime - cutoffEnd;
State result = m_current;
if (t < m_endAccel) {
result.velocity += t * m_constraints.maxAcceleration;
result.position +=
(m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
} else if (t < m_endFullSpeed) {
} else if (t < m_endFullVelocity) {
result.velocity = m_constraints.maxVelocity;
result.position += (m_current.velocity +
m_endAccel * m_constraints.maxAcceleration / 2.0) *
@@ -214,16 +215,17 @@ class TrapezoidProfile {
Velocity_t velocity = m_current.velocity * m_direction;
wpi::units::second_t endAccel = m_endAccel * m_direction;
wpi::units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;
wpi::units::second_t endFullVelocity =
m_endFullVelocity * m_direction - endAccel;
if (target < position) {
endAccel *= -1.0;
endFullSpeed *= -1.0;
endFullVelocity *= -1.0;
velocity *= -1.0;
}
endAccel = wpi::units::math::max(endAccel, 0_s);
endFullSpeed = wpi::units::math::max(endFullSpeed, 0_s);
endFullVelocity = wpi::units::math::max(endFullVelocity, 0_s);
const Acceleration_t acceleration = m_constraints.maxAcceleration;
const Acceleration_t deceleration = -m_constraints.maxAcceleration;
@@ -245,18 +247,18 @@ class TrapezoidProfile {
decelVelocity = velocity;
}
Distance_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
Distance_t fullVelocityDist = m_constraints.maxVelocity * endFullVelocity;
Distance_t decelDist;
if (accelDist > distToTarget) {
accelDist = distToTarget;
fullSpeedDist = Distance_t{0};
fullVelocityDist = Distance_t{0};
decelDist = Distance_t{0};
} else if (accelDist + fullSpeedDist > distToTarget) {
fullSpeedDist = distToTarget - accelDist;
} else if (accelDist + fullVelocityDist > distToTarget) {
fullVelocityDist = distToTarget - accelDist;
decelDist = Distance_t{0};
} else {
decelDist = distToTarget - fullSpeedDist - accelDist;
decelDist = distToTarget - fullVelocityDist - accelDist;
}
wpi::units::second_t accelTime =
@@ -270,10 +272,10 @@ class TrapezoidProfile {
decelVelocity * decelVelocity + 2 * deceleration * decelDist))) /
deceleration;
wpi::units::second_t fullSpeedTime =
fullSpeedDist / m_constraints.maxVelocity;
wpi::units::second_t fullVelocityTime =
fullVelocityDist / m_constraints.maxVelocity;
return accelTime + fullSpeedTime + decelTime;
return accelTime + fullVelocityTime + decelTime;
}
/**
@@ -326,7 +328,7 @@ class TrapezoidProfile {
State m_current;
wpi::units::second_t m_endAccel = 0_s;
wpi::units::second_t m_endFullSpeed = 0_s;
wpi::units::second_t m_endFullVelocity = 0_s;
wpi::units::second_t m_endDecel = 0_s;
};

2
wpimath/src/main/native/include/wpi/math/trajectory/constraint/CentripetalAccelerationConstraint.hpp
View File

@@ -48,7 +48,7 @@ class WPILIB_DLLEXPORT CentripetalAccelerationConstraint
constexpr MinMax MinMaxAcceleration(
const Pose2d& pose, wpi::units::curvature_t curvature,
wpi::units::meters_per_second_t speed) const override {
wpi::units::meters_per_second_t velocity) const override {
// The acceleration of the robot has no impact on the centripetal
// acceleration of the robot.
return {};

16
wpimath/src/main/native/include/wpi/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.hpp
View File

@@ -23,27 +23,27 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematicsConstraint
public:
constexpr DifferentialDriveKinematicsConstraint(
DifferentialDriveKinematics kinematics,
wpi::units::meters_per_second_t maxSpeed)
: m_kinematics(std::move(kinematics)), m_maxSpeed(maxSpeed) {}
wpi::units::meters_per_second_t maxVelocity)
: m_kinematics(std::move(kinematics)), m_maxVelocity(maxVelocity) {}
constexpr wpi::units::meters_per_second_t MaxVelocity(
const Pose2d& pose, wpi::units::curvature_t curvature,
wpi::units::meters_per_second_t velocity) const override {
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds({velocity, 0_mps, velocity * curvature});
wheelSpeeds.Desaturate(m_maxSpeed);
auto wheelVelocities =
m_kinematics.ToWheelVelocities({velocity, 0_mps, velocity * curvature});
wheelVelocities.Desaturate(m_maxVelocity);
return m_kinematics.ToChassisSpeeds(wheelSpeeds).vx;
return m_kinematics.ToChassisVelocities(wheelVelocities).vx;
}
constexpr MinMax MinMaxAcceleration(
const Pose2d& pose, wpi::units::curvature_t curvature,
wpi::units::meters_per_second_t speed) const override {
wpi::units::meters_per_second_t velocity) const override {
return {};
}
private:
DifferentialDriveKinematics m_kinematics;
wpi::units::meters_per_second_t m_maxSpeed;
wpi::units::meters_per_second_t m_maxVelocity;
};
} // namespace wpi::math

30
wpimath/src/main/native/include/wpi/math/trajectory/constraint/DifferentialDriveVoltageConstraint.hpp
View File

@@ -52,19 +52,21 @@ class WPILIB_DLLEXPORT DifferentialDriveVoltageConstraint
constexpr MinMax MinMaxAcceleration(
const Pose2d& pose, wpi::units::curvature_t curvature,
wpi::units::meters_per_second_t speed) const override {
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds({speed, 0_mps, speed * curvature});
wpi::units::meters_per_second_t velocity) const override {
auto wheelVelocities =
m_kinematics.ToWheelVelocities({velocity, 0_mps, velocity * curvature});
auto maxWheelSpeed = (std::max)(wheelSpeeds.left, wheelSpeeds.right);
auto minWheelSpeed = (std::min)(wheelSpeeds.left, wheelSpeeds.right);
auto maxWheelVelocity =
(std::max)(wheelVelocities.left, wheelVelocities.right);
auto minWheelVelocity =
(std::min)(wheelVelocities.left, wheelVelocities.right);
// Calculate maximum/minimum possible accelerations from motor dynamics
// and max/min wheel speeds
// and max/min wheel velocities
auto maxWheelAcceleration =
m_feedforward.MaxAchievableAcceleration(m_maxVoltage, maxWheelSpeed);
m_feedforward.MaxAchievableAcceleration(m_maxVoltage, maxWheelVelocity);
auto minWheelAcceleration =
m_feedforward.MinAchievableAcceleration(m_maxVoltage, minWheelSpeed);
m_feedforward.MinAchievableAcceleration(m_maxVoltage, minWheelVelocity);
// Robot chassis turning on radius = 1/|curvature|. Outer wheel has radius
// increased by half of the trackwidth T. Inner wheel has radius decreased
@@ -72,7 +74,7 @@ class WPILIB_DLLEXPORT DifferentialDriveVoltageConstraint
// so Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 +
// |curvature|T/2). Inner wheel is similar.
// sgn(speed) term added to correctly account for which wheel is on
// sgn(velocity) term added to correctly account for which wheel is on
// outside of turn:
// If moving forward, max acceleration constraint corresponds to wheel on
// outside of turn If moving backward, max acceleration constraint
@@ -86,7 +88,7 @@ class WPILIB_DLLEXPORT DifferentialDriveVoltageConstraint
wpi::units::meters_per_second_squared_t maxChassisAcceleration;
wpi::units::meters_per_second_squared_t minChassisAcceleration;
if (speed == 0_mps) {
if (velocity == 0_mps) {
maxChassisAcceleration =
maxWheelAcceleration /
(1 + m_kinematics.trackwidth * wpi::units::math::abs(curvature) /
@@ -99,11 +101,11 @@ class WPILIB_DLLEXPORT DifferentialDriveVoltageConstraint
maxChassisAcceleration =
maxWheelAcceleration /
(1 + m_kinematics.trackwidth * wpi::units::math::abs(curvature) *
wpi::util::sgn(speed) / (2_rad));
wpi::util::sgn(velocity) / (2_rad));
minChassisAcceleration =
minWheelAcceleration /
(1 - m_kinematics.trackwidth * wpi::units::math::abs(curvature) *
wpi::util::sgn(speed) / (2_rad));
wpi::util::sgn(velocity) / (2_rad));
}
// When turning about a point inside of the wheelbase (i.e. radius less than
@@ -114,9 +116,9 @@ class WPILIB_DLLEXPORT DifferentialDriveVoltageConstraint
if ((m_kinematics.trackwidth / 2) >
1_rad / wpi::units::math::abs(curvature)) {
if (speed > 0_mps) {
if (velocity > 0_mps) {
minChassisAcceleration = -minChassisAcceleration;
} else if (speed < 0_mps) {
} else if (velocity < 0_mps) {
maxChassisAcceleration = -maxChassisAcceleration;
}
}

4
wpimath/src/main/native/include/wpi/math/trajectory/constraint/EllipticalRegionConstraint.hpp
View File

@@ -65,9 +65,9 @@ class EllipticalRegionConstraint : public TrajectoryConstraint {
constexpr MinMax MinMaxAcceleration(
const Pose2d& pose, wpi::units::curvature_t curvature,
wpi::units::meters_per_second_t speed) const override {
wpi::units::meters_per_second_t velocity) const override {
if (m_ellipse.Contains(pose.Translation())) {
return m_constraint.MinMaxAcceleration(pose, curvature, speed);
return m_constraint.MinMaxAcceleration(pose, curvature, velocity);
} else {
return {};
}

2
wpimath/src/main/native/include/wpi/math/trajectory/constraint/MaxVelocityConstraint.hpp
View File

@@ -35,7 +35,7 @@ class WPILIB_DLLEXPORT MaxVelocityConstraint : public TrajectoryConstraint {
constexpr MinMax MinMaxAcceleration(
const Pose2d& pose, wpi::units::curvature_t curvature,
wpi::units::meters_per_second_t speed) const override {
wpi::units::meters_per_second_t velocity) const override {
return {};
}

19
wpimath/src/main/native/include/wpi/math/trajectory/constraint/MecanumDriveKinematicsConstraint.hpp
View File

@@ -21,31 +21,32 @@ class WPILIB_DLLEXPORT MecanumDriveKinematicsConstraint
: public TrajectoryConstraint {
public:
MecanumDriveKinematicsConstraint(const MecanumDriveKinematics& kinematics,
wpi::units::meters_per_second_t maxSpeed)
: m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
wpi::units::meters_per_second_t maxVelocity)
: m_kinematics(kinematics), m_maxVelocity(maxVelocity) {}
wpi::units::meters_per_second_t MaxVelocity(
const Pose2d& pose, wpi::units::curvature_t curvature,
wpi::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})
.Desaturate(m_maxSpeed);
auto wheelVelocities =
m_kinematics
.ToWheelVelocities({xVelocity, yVelocity, velocity * curvature})
.Desaturate(m_maxVelocity);
auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
auto normVelocities = m_kinematics.ToChassisVelocities(wheelVelocities);
return wpi::units::math::hypot(normSpeeds.vx, normSpeeds.vy);
return wpi::units::math::hypot(normVelocities.vx, normVelocities.vy);
}
MinMax MinMaxAcceleration(
const Pose2d& pose, wpi::units::curvature_t curvature,
wpi::units::meters_per_second_t speed) const override {
wpi::units::meters_per_second_t velocity) const override {
return {};
}
private:
MecanumDriveKinematics m_kinematics;
wpi::units::meters_per_second_t m_maxSpeed;
wpi::units::meters_per_second_t m_maxVelocity;
};
} // namespace wpi::math

4
wpimath/src/main/native/include/wpi/math/trajectory/constraint/RectangularRegionConstraint.hpp
View File

@@ -60,9 +60,9 @@ class RectangularRegionConstraint : public TrajectoryConstraint {
constexpr MinMax MinMaxAcceleration(
const Pose2d& pose, wpi::units::curvature_t curvature,
wpi::units::meters_per_second_t speed) const override {
wpi::units::meters_per_second_t velocity) const override {
if (m_rectangle.Contains(pose.Translation())) {
return m_constraint.MinMaxAcceleration(pose, curvature, speed);
return m_constraint.MinMaxAcceleration(pose, curvature, velocity);
} else {
return {};
}

16
wpimath/src/main/native/include/wpi/math/trajectory/constraint/SwerveDriveKinematicsConstraint.hpp
View File

@@ -21,32 +21,32 @@ class SwerveDriveKinematicsConstraint : public TrajectoryConstraint {
public:
SwerveDriveKinematicsConstraint(
const wpi::math::SwerveDriveKinematics<NumModules>& kinematics,
wpi::units::meters_per_second_t maxSpeed)
: m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
wpi::units::meters_per_second_t maxVelocity)
: m_kinematics(kinematics), m_maxVelocity(maxVelocity) {}
wpi::units::meters_per_second_t MaxVelocity(
const Pose2d& pose, wpi::units::curvature_t curvature,
wpi::units::meters_per_second_t velocity) const override {
auto xVelocity = velocity * pose.Rotation().Cos();
auto yVelocity = velocity * pose.Rotation().Sin();
auto wheelSpeeds = m_kinematics.ToSwerveModuleStates(
auto wheelVelocities = m_kinematics.ToSwerveModuleVelocities(
{xVelocity, yVelocity, velocity * curvature});
m_kinematics.DesaturateWheelSpeeds(&wheelSpeeds, m_maxSpeed);
m_kinematics.DesaturateWheelVelocities(&wheelVelocities, m_maxVelocity);
auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
auto normVelocities = m_kinematics.ToChassisVelocities(wheelVelocities);
return wpi::units::math::hypot(normSpeeds.vx, normSpeeds.vy);
return wpi::units::math::hypot(normVelocities.vx, normVelocities.vy);
}
MinMax MinMaxAcceleration(
const Pose2d& pose, wpi::units::curvature_t curvature,
wpi::units::meters_per_second_t speed) const override {
wpi::units::meters_per_second_t velocity) const override {
return {};
}
private:
wpi::math::SwerveDriveKinematics<NumModules> m_kinematics;
wpi::units::meters_per_second_t m_maxSpeed;
wpi::units::meters_per_second_t m_maxVelocity;
};
} // namespace wpi::math

6
wpimath/src/main/native/include/wpi/math/trajectory/constraint/TrajectoryConstraint.hpp
View File

@@ -63,16 +63,16 @@ class WPILIB_DLLEXPORT TrajectoryConstraint {
/**
* Returns the minimum and maximum allowable acceleration for the trajectory
* given pose, curvature, and speed.
* given pose, curvature, and velocity.
*
* @param pose The pose at the current point in the trajectory.
* @param curvature The curvature at the current point in the trajectory.
* @param speed The speed at the current point in the trajectory.
* @param velocity The velocity at the current point in the trajectory.
*
* @return The min and max acceleration bounds.
*/
constexpr virtual MinMax MinMaxAcceleration(
const Pose2d& pose, wpi::units::curvature_t curvature,
wpi::units::meters_per_second_t speed) const = 0;
wpi::units::meters_per_second_t velocity) const = 0;
};
} // namespace wpi::math

2
wpimath/src/main/native/include/wpi/units/constants.hpp
View File

@@ -54,7 +54,7 @@ using PI = unit<std::ratio<1>, dimensionless::scalar, std::ratio<1>>;
static constexpr const unit_t<PI> pi(
1); ///< Ratio of a circle's circumference to its diameter.
static constexpr const velocity::meters_per_second_t c(
299792458.0); ///< Speed of light in vacuum.
299792458.0); ///< Velocity of light in vacuum.
static constexpr const unit_t<
compound_unit<cubed<length::meters>, inverse<mass::kilogram>,
inverse<squared<time::seconds>>>>