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

[wpimath] Clean up math comments (#4252)

Browse Source
This commit is contained in:
Tyler Veness
2022-05-20 15:16:56 -07:00
committed by GitHub
parent fff4d1f44e
commit 5aa67f56e6
25 changed files with 141 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
View File

@@ -129,7 +129,7 @@ void DifferentialDrivetrainSim::SetPose(const frc::Pose2d& pose) {
Vectord<7> DifferentialDrivetrainSim::Dynamics(const Vectord<7>& x,
const Vectord<2>& u) {
// Because G^2 can be factored out of A, we can divide by the old ratio
// Because G² can be factored out of A, we can divide by the old ratio
// squared and multiply by the new ratio squared to get a new drivetrain
// model.
Matrixd<4, 2> B;

2
wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
View File

@@ -255,7 +255,7 @@ class DifferentialDrivetrainSim {
static DifferentialDrivetrainSim CreateKitbotSim(
frc::DCMotor motor, double gearing, units::meter_t wheelSize,
const std::array<double, 7>& measurementStdDevs = {}) {
// MOI estimation -- note that I = m r^2 for point masses
// MOI estimation -- note that I = mr² for point masses
units::kilogram_square_meter_t batteryMoi = 12.5_lb * 10_in * 10_in;
units::kilogram_square_meter_t gearboxMoi = (2.8_lb + 2.0_lb) *
2 // CIM plus toughbox per side

4
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
View File

@@ -323,7 +323,7 @@ public class DifferentialDrivetrainSim {
var B = new Matrix<>(Nat.N4(), Nat.N2());
B.assignBlock(0, 0, m_plant.getB().times(this.m_currentGearing / this.m_originalGearing));
// Because G^2 can be factored out of A, we can divide by the old ratio squared and multiply
// Because G² can be factored out of A, we can divide by the old ratio squared and multiply
// by the new ratio squared to get a new drivetrain model.
var A = new Matrix<>(Nat.N4(), Nat.N4());
A.assignBlock(
@@ -455,7 +455,7 @@ public class DifferentialDrivetrainSim {
KitbotGearing gearing,
KitbotWheelSize wheelSize,
Matrix<N7, N1> measurementStdDevs) {
// MOI estimation -- note that I = m r^2 for point masses
// MOI estimation -- note that I = mr² for point masses
var batteryMoi = 12.5 / 2.2 * Math.pow(Units.inchesToMeters(10), 2);
var gearboxMoi =
(2.8 /* CIM motor */ * 2 / 2.2 + 2.0 /* Toughbox Mini- ish */)

26
wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java
View File

@@ -81,25 +81,25 @@ public class CubicHermiteSpline extends Spline {
private SimpleMatrix makeHermiteBasis() {
if (hermiteBasis == null) {
// Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
// the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0.
// the coefficients of the spline P(t) = a₃t³ + a₂t² + a₁t + a₀.
//
// P(i) = P(0) = a0
// P'(i) = P'(0) = a1
// P(i+1) = P(1) = a3 + a2 + a1 + a0
// P'(i+1) = P'(1) = 3 * a3 + 2 * a2 + a1
// P(i) = P(0) = a₀
// P'(i) = P'(0) = a₁
// P(i+1) = P(1) = a₃ + a₂ + a₁ + a₀
// P'(i+1) = P'(1) = 3a₃ + 2a₂ + a₁
//
// [ P(i) ] = [ 0 0 0 1 ][ a3 ]
// [ P'(i) ] = [ 0 0 1 0 ][ a2 ]
// [ P(i+1) ] = [ 1 1 1 1 ][ a1 ]
// [ P'(i+1) ] = [ 3 2 1 0 ][ a0 ]
// [P(i) ] = [0 0 0 1][a₃]
// [P'(i) ] = [0 0 1 0][a₂]
// [P(i+1) ] = [1 1 1 1][a₁]
// [P'(i+1)] = [3 2 1 0][a₀]
//
// To solve for the coefficients, we can invert the 4x4 matrix and move it
// to the other side of the equation.
//
// [ a3 ] = [ 2 1 -2 1 ][ P(i) ]
// [ a2 ] = [ -3 -2 3 -1 ][ P'(i) ]
// [ a1 ] = [ 0 1 0 0 ][ P(i+1) ]
// [ a0 ] = [ 1 0 0 0 ][ P'(i+1) ]
// [a₃] = [ 2 1 -2 1][P(i) ]
// [a₂] = [-3 -2 3 -1][P'(i) ]
// [a₁] = [ 0 1 0 0][P(i+1) ]
// [a₀] = [ 1 0 0 0][P'(i+1)]
hermiteBasis =
new SimpleMatrix(
4,

42
wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java
View File

@@ -80,33 +80,33 @@ public class QuinticHermiteSpline extends Spline {
*/
private SimpleMatrix makeHermiteBasis() {
if (hermiteBasis == null) {
// Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control
// vectors, we want to find the coefficients of the spline
// P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0.
// Given P(i), P'(i), P"(i), P(i+1), P'(i+1), P"(i+1), the control vectors,
// we want to find the coefficients of the spline
// P(t) = a₅t⁵ + a₄t⁴ + a₃t³ + a₂t² + a₁t + a₀.
//
// P(i) = P(0) = a0
// P'(i) = P'(0) = a1
// P''(i) = P''(0) = 2 * a2
// P(i+1) = P(1) = a5 + a4 + a3 + a2 + a1 + a0
// P'(i+1) = P'(1) = 5 * a5 + 4 * a4 + 3 * a3 + 2 * a2 + a1
// P''(i+1) = P''(1) = 20 * a5 + 12 * a4 + 6 * a3 + 2 * a2
// P(i) = P(0) = a₀
// P'(i) = P'(0) = a₁
// P''(i) = P"(0) = 2a₂
// P(i+1) = P(1) = a₅ + a₄ + a₃ + a₂ + a₁ + a₀
// P'(i+1) = P'(1) = 5a₅ + 4a₄ + 3a₃ + 2a₂ + a₁
// P"(i+1) = P"(1) = 20a₅ + 12a₄ + 6a₃ + 2a₂
//
// [ P(i) ] = [ 0 0 0 0 0 1 ][ a5 ]
// [ P'(i) ] = [ 0 0 0 0 1 0 ][ a4 ]
// [ P''(i) ] = [ 0 0 0 2 0 0 ][ a3 ]
// [ P(i+1) ] = [ 1 1 1 1 1 1 ][ a2 ]
// [ P'(i+1) ] = [ 5 4 3 2 1 0 ][ a1 ]
// [ P''(i+1) ] = [ 20 12 6 2 0 0 ][ a0 ]
// [P(i) ] = [ 0 0 0 0 0 1][a₅]
// [P'(i) ] = [ 0 0 0 0 1 0][a₄]
// [P"(i) ] = [ 0 0 0 2 0 0][a₃]
// [P(i+1) ] = [ 1 1 1 1 1 1][a₂]
// [P'(i+1)] = [ 5 4 3 2 1 0][a₁]
// [P"(i+1)] = [20 12 6 2 0 0][a₀]
//
// To solve for the coefficients, we can invert the 6x6 matrix and move it
// to the other side of the equation.
//
// [ a5 ] = [ -6.0 -3.0 -0.5 6.0 -3.0 0.5 ][ P(i) ]
// [ a4 ] = [ 15.0 8.0 1.5 -15.0 7.0 -1.0 ][ P'(i) ]
// [ a3 ] = [ -10.0 -6.0 -1.5 10.0 -4.0 0.5 ][ P''(i) ]
// [ a2 ] = [ 0.0 0.0 0.5 0.0 0.0 0.0 ][ P(i+1) ]
// [ a1 ] = [ 0.0 1.0 0.0 0.0 0.0 0.0 ][ P'(i+1) ]
// [ a0 ] = [ 1.0 0.0 0.0 0.0 0.0 0.0 ][ P''(i+1) ]
// [a₅] = [ -6.0 -3.0 -0.5 6.0 -3.0 0.5][P(i) ]
// [a₄] = [ 15.0 8.0 1.5 -15.0 7.0 -1.0][P'(i) ]
// [a₃] = [-10.0 -6.0 -1.5 10.0 -4.0 0.5][P"(i) ]
// [a₂] = [ 0.0 0.0 0.5 0.0 0.0 0.0][P(i+1) ]
// [a₁] = [ 0.0 1.0 0.0 0.0 0.0 0.0][P'(i+1)]
// [a₀] = [ 1.0 0.0 0.0 0.0 0.0 0.0][P"(i+1)]
hermiteBasis =
new SimpleMatrix(
6,

12
wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
View File

@@ -211,7 +211,7 @@ public final class LinearSystemId {
}
/**
* Identify a velocity system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2). These
* Identify a velocity system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec²). These
* constants cam be found using SysId. The states of the system are [velocity], inputs are
* [voltage], and outputs are [velocity].
*
@@ -241,7 +241,7 @@ public final class LinearSystemId {
}
/**
* Identify a position system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2). These
* Identify a position system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec²). These
* constants cam be found using SysId. The states of the system are [position, velocity]ᵀ, inputs
* are [voltage], and outputs are [position].
*
@@ -272,8 +272,8 @@ public final class LinearSystemId {
/**
* Identify a standard differential drive drivetrain, given the drivetrain's kV and kA in both
* linear (volts/(meter/sec) and volts/(meter/sec^2)) and angular (volts/(meter/sec) and
* volts/(meter/sec^2)) cases. This can be found using SysId. The states of the system are [left
* linear (volts/(meter/sec) and volts/(meter/sec²)) and angular (volts/(meter/sec) and
* volts/(meter/sec²)) cases. This can be found using SysId. The states of the system are [left
* velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are [left
* velocity, right velocity]ᵀ.
*
@@ -316,8 +316,8 @@ public final class LinearSystemId {
/**
* Identify a standard differential drive drivetrain, given the drivetrain's kV and kA in both
* linear (volts/(meter/sec) and volts/(meter/sec^2)) and angular (volts/(radian/sec) and
* volts/(radian/sec^2)) cases. This can be found using SysId. The states of the system are [left
* linear (volts/(meter/sec) and volts/(meter/sec²)) and angular (volts/(radian/sec) and
* volts/(radian/sec²)) cases. This can be found using SysId. The states of the system are [left
* velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are [left
* velocity, right velocity]ᵀ.
*

2
wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
View File

@@ -332,7 +332,7 @@ public class Trajectory {
final double newV = velocityMetersPerSecond + (accelerationMetersPerSecondSq * deltaT);
// Calculate the change in position.
// delta_s = v_0 t + 0.5 at^2
// delta_s = v_0 t + 0.5at²
final double newS =
(velocityMetersPerSecond * deltaT
+ 0.5 * accelerationMetersPerSecondSq * Math.pow(deltaT, 2))

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

@@ -90,7 +90,7 @@ public final class TrajectoryParameterizer {
// acceleration, since acceleration limits may be a function of velocity.
while (true) {
// Enforce global max velocity and max reachable velocity by global
// acceleration limit. vf = std::sqrt(vi^2 + 2*a*d).
// acceleration limit. v_f = √(v_i² + 2ad).
constrainedState.maxVelocityMetersPerSecond =
Math.min(
maxVelocityMetersPerSecond,
@@ -164,7 +164,7 @@ public final class TrajectoryParameterizer {
while (true) {
// Enforce max velocity limit (reverse)
// vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
// v_f = √(v_i² + 2ad), where v_i = successor.
double newMaxVelocity =
Math.sqrt(
successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond

10
wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java
View File

@@ -38,12 +38,12 @@ public class CentripetalAccelerationConstraint implements TrajectoryConstraint {
@Override
public double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
// ac = v^2 / r
// k (curvature) = 1 / r
// ac = v²/r
// k (curvature) = 1/r
// therefore, ac = v^2 * k
// ac / k = v^2
// v = std::sqrt(ac / k)
// therefore, ac = v²k
// ac/k = v²
// v = √(ac/k)
return Math.sqrt(
m_maxCentripetalAccelerationMetersPerSecondSq / Math.abs(curvatureRadPerMeter));

11
wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java
View File

@@ -65,11 +65,16 @@ public class EllipticalRegionConstraint implements TrajectoryConstraint {
* @return Whether the robot pose is within the constraint region.
*/
public boolean isPoseInRegion(Pose2d robotPose) {
// The region (disk) bounded by the ellipse is given by the equation:
// ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1
// The region bounded by the ellipse is given by the equation:
//
// (x−h)²/Rx² + (y−k)²/Ry² ≤ 1
//
// Multiply by Rx²Ry² for efficiency reasons:
//
// (x−h)²Ry² + (y−k)²Rx² ≤ Rx²Ry²
//
// If the inequality is satisfied, then it is inside the ellipse; otherwise
// it is outside the ellipse.
// Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons.
return Math.pow(robotPose.getX() - m_center.getX(), 2) * Math.pow(m_radii.getY(), 2)
+ Math.pow(robotPose.getY() - m_center.getY(), 2) * Math.pow(m_radii.getX(), 2)
<= Math.pow(m_radii.getX(), 2) * Math.pow(m_radii.getY(), 2);

2
wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
View File

@@ -46,7 +46,7 @@ Trajectory::State Trajectory::State::Interpolate(State endValue,
const units::meters_per_second_t newV = velocity + (acceleration * deltaT);
// Calculate the change in position.
// delta_s = v_0 t + 0.5 at^2
// delta_s = v_0 t + 0.5at²
const units::meter_t newS =
(velocity * deltaT + 0.5 * acceleration * deltaT * deltaT) *
(reversing ? -1.0 : 1.0);

8
wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
View File

@@ -62,7 +62,7 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
// acceleration, since acceleration limits may be a function of velocity.
while (true) {
// Enforce global max velocity and max reachable velocity by global
// acceleration limit. vf = std::sqrt(vi^2 + 2*a*d).
// acceleration limit. v_f = √(v_i² + 2ad).
constrainedState.maxVelocity = units::math::min(
maxVelocity,
@@ -126,7 +126,7 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
while (true) {
// Enforce max velocity limit (reverse)
// vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
// v_f = √(v_i² + 2ad), where v_i = successor.
units::meters_per_second_t newMaxVelocity =
units::math::sqrt(successor.maxVelocity * successor.maxVelocity +
successor.minAcceleration * ds * 2.0);
@@ -187,10 +187,10 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
if (i > 0) {
states.at(i - 1).acceleration = reversed ? -accel : accel;
if (units::math::abs(accel) > 1E-6_mps_sq) {
// v_f = v_0 + a * t
// v_f = v_0 + at
dt = (state.maxVelocity - v) / accel;
} else if (units::math::abs(v) > 1E-6_mps) {
// delta_x = v * t
// delta_x = vt
dt = ds / v;
} else {
throw std::runtime_error(fmt::format(

10
wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
View File

@@ -15,12 +15,12 @@ CentripetalAccelerationConstraint::CentripetalAccelerationConstraint(
units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const {
// ac = v^2 / r
// k (curvature) = 1 / r
// ac = v²/r
// k (curvature) = 1/r
// therefore, ac = v^2 * k
// ac / k = v^2
// v = std::sqrt(ac / k)
// therefore, ac = v²k
// ac/k = v²
// v = √(ac/k)
// We have to multiply by 1_rad here to get the units to cancel out nicely.
// The units library defines a unit for radians although it is technically

4
wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
View File

@@ -37,7 +37,7 @@ class WPILIB_DLLEXPORT ArmFeedforward {
* @param kS The static gain, in volts.
* @param kG The gravity gain, in volts.
* @param kV The velocity gain, in volt seconds per radian.
* @param kA The acceleration gain, in volt seconds^2 per radian.
* @param kA The acceleration gain, in volt seconds² per radian.
*/
constexpr ArmFeedforward(
units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
@@ -49,7 +49,7 @@ class WPILIB_DLLEXPORT ArmFeedforward {
*
* @param angle The angle setpoint, in radians.
* @param velocity The velocity setpoint, in radians per second.
* @param acceleration The acceleration setpoint, in radians per second^2.
* @param acceleration The acceleration setpoint, in radians per second².
* @return The computed feedforward, in volts.
*/
units::volt_t Calculate(units::unit_t<Angle> angle,

4
wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
View File

@@ -33,7 +33,7 @@ class ElevatorFeedforward {
* @param kS The static gain, in volts.
* @param kG The gravity gain, in volts.
* @param kV The velocity gain, in volt seconds per distance.
* @param kA The acceleration gain, in volt seconds^2 per distance.
* @param kA The acceleration gain, in volt seconds² per distance.
*/
constexpr ElevatorFeedforward(
units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
@@ -44,7 +44,7 @@ class ElevatorFeedforward {
* Calculates the feedforward from the gains and setpoints.
*
* @param velocity The velocity setpoint, in distance per second.
* @param acceleration The acceleration setpoint, in distance per second^2.
* @param acceleration The acceleration setpoint, in distance per second².
* @return The computed feedforward, in volts.
*/
constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,

4
wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
View File

@@ -35,7 +35,7 @@ class SimpleMotorFeedforward {
*
* @param kS The static gain, in volts.
* @param kV The velocity gain, in volt seconds per distance.
* @param kA The acceleration gain, in volt seconds^2 per distance.
* @param kA The acceleration gain, in volt seconds² per distance.
*/
constexpr SimpleMotorFeedforward(
units::volt_t kS, units::unit_t<kv_unit> kV,
@@ -46,7 +46,7 @@ class SimpleMotorFeedforward {
* Calculates the feedforward from the gains and setpoints.
*
* @param velocity The velocity setpoint, in distance per second.
* @param acceleration The acceleration setpoint, in distance per second^2.
* @param acceleration The acceleration setpoint, in distance per second².
* @return The computed feedforward, in volts.
*/
constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,

26
wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
View File

@@ -51,25 +51,25 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
*/
static Matrixd<4, 4> MakeHermiteBasis() {
// Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
// the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0.
// the coefficients of the spline P(t) = a₃t³ + a₂t² + a₁t + a₀.
//
// P(i) = P(0) = a0
// P'(i) = P'(0) = a1
// P(i+1) = P(1) = a3 + a2 + a1 + a0
// P'(i+1) = P'(1) = 3 * a3 + 2 * a2 + a1
// P(i) = P(0) = a₀
// P'(i) = P'(0) = a₁
// P(i+1) = P(1) = a₃ + a₂ + a₁ + a₀
// P'(i+1) = P'(1) = 3a₃ + 2a₂ + a₁
//
// [ P(i) ] = [ 0 0 0 1 ][ a3 ]
// [ P'(i) ] = [ 0 0 1 0 ][ a2 ]
// [ P(i+1) ] = [ 1 1 1 1 ][ a1 ]
// [ P'(i+1) ] = [ 3 2 1 0 ][ a0 ]
// [P(i) ] = [0 0 0 1][a₃]
// [P'(i) ] = [0 0 1 0][a₂]
// [P(i+1) ] = [1 1 1 1][a₁]
// [P'(i+1)] = [3 2 1 0][a₀]
//
// To solve for the coefficients, we can invert the 4x4 matrix and move it
// to the other side of the equation.
//
// [ a3 ] = [ 2 1 -2 1 ][ P(i) ]
// [ a2 ] = [ -3 -2 3 -1 ][ P'(i) ]
// [ a1 ] = [ 0 1 0 0 ][ P(i+1) ]
// [ a0 ] = [ 1 0 0 0 ][ P'(i+1) ]
// [a₃] = [ 2 1 -2 1][P(i) ]
// [a₂] = [-3 -2 3 -1][P'(i) ]
// [a₁] = [ 0 1 0 0][P(i+1) ]
// [a₀] = [ 1 0 0 0][P'(i+1)]
static const Matrixd<4, 4> basis{{+2.0, +1.0, -2.0, +1.0},
{-3.0, -2.0, +3.0, -1.0},

42
wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
View File

@@ -50,33 +50,33 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
* @return The hermite basis matrix for quintic hermite spline interpolation.
*/
static Matrixd<6, 6> MakeHermiteBasis() {
// Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control
// vectors, we want to find the coefficients of the spline
// P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0.
// Given P(i), P'(i), P"(i), P(i+1), P'(i+1), P"(i+1), the control vectors,
// we want to find the coefficients of the spline
// P(t) = a₅t⁵ + a₄t⁴ + a₃t³ + a₂t² + a₁t + a₀.
//
// P(i) = P(0) = a0
// P'(i) = P'(0) = a1
// P''(i) = P''(0) = 2 * a2
// P(i+1) = P(1) = a5 + a4 + a3 + a2 + a1 + a0
// P'(i+1) = P'(1) = 5 * a5 + 4 * a4 + 3 * a3 + 2 * a2 + a1
// P''(i+1) = P''(1) = 20 * a5 + 12 * a4 + 6 * a3 + 2 * a2
// P(i) = P(0) = a₀
// P'(i) = P'(0) = a₁
// P''(i) = P"(0) = 2a₂
// P(i+1) = P(1) = a₅ + a₄ + a₃ + a₂ + a₁ + a₀
// P'(i+1) = P'(1) = 5a₅ + 4a₄ + 3a₃ + 2a₂ + a₁
// P"(i+1) = P"(1) = 20a₅ + 12a₄ + 6a₃ + 2a₂
//
// [ P(i) ] = [ 0 0 0 0 0 1 ][ a5 ]
// [ P'(i) ] = [ 0 0 0 0 1 0 ][ a4 ]
// [ P''(i) ] = [ 0 0 0 2 0 0 ][ a3 ]
// [ P(i+1) ] = [ 1 1 1 1 1 1 ][ a2 ]
// [ P'(i+1) ] = [ 5 4 3 2 1 0 ][ a1 ]
// [ P''(i+1) ] = [ 20 12 6 2 0 0 ][ a0 ]
// [P(i) ] = [ 0 0 0 0 0 1][a₅]
// [P'(i) ] = [ 0 0 0 0 1 0][a₄]
// [P"(i) ] = [ 0 0 0 2 0 0][a₃]
// [P(i+1) ] = [ 1 1 1 1 1 1][a₂]
// [P'(i+1)] = [ 5 4 3 2 1 0][a₁]
// [P"(i+1)] = [20 12 6 2 0 0][a₀]
//
// To solve for the coefficients, we can invert the 6x6 matrix and move it
// to the other side of the equation.
//
// [ a5 ] = [ -6.0 -3.0 -0.5 6.0 -3.0 0.5 ][ P(i) ]
// [ a4 ] = [ 15.0 8.0 1.5 -15.0 7.0 -1.0 ][ P'(i) ]
// [ a3 ] = [ -10.0 -6.0 -1.5 10.0 -4.0 0.5 ][ P''(i) ]
// [ a2 ] = [ 0.0 0.0 0.5 0.0 0.0 0.0 ][ P(i+1) ]
// [ a1 ] = [ 0.0 1.0 0.0 0.0 0.0 0.0 ][ P'(i+1) ]
// [ a0 ] = [ 1.0 0.0 0.0 0.0 0.0 0.0 ][ P''(i+1) ]
// [a₅] = [ -6.0 -3.0 -0.5 6.0 -3.0 0.5][P(i) ]
// [a₄] = [ 15.0 8.0 1.5 -15.0 7.0 -1.0][P'(i) ]
// [a₃] = [-10.0 -6.0 -1.5 10.0 -4.0 0.5][P"(i) ]
// [a₂] = [ 0.0 0.0 0.5 0.0 0.0 0.0][P(i+1) ]
// [a₁] = [ 0.0 1.0 0.0 0.0 0.0 0.0][P'(i+1)]
// [a₀] = [ 1.0 0.0 0.0 0.0 0.0 0.0][P"(i+1)]
static const Matrixd<6, 6> basis{
{-06.0, -03.0, -00.5, +06.0, -03.0, +00.5},

4
wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
View File

@@ -116,7 +116,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
* u = K_v v + K_a a
*
* @param kV The velocity gain, in volt seconds per distance.
* @param kA The acceleration gain, in volt seconds^2 per distance.
* @param kA The acceleration gain, in volt seconds² per distance.
* @throws std::domain_error if kV <= 0 or kA <= 0.
*/
template <typename Distance, typename = std::enable_if_t<
@@ -158,7 +158,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
* u = K_v v + K_a a
*
* @param kV The velocity gain, in volt seconds per distance.
* @param kA The acceleration gain, in volt seconds^2 per distance.
* @param kA The acceleration gain, in volt seconds² per distance.
* @throws std::domain_error if kV <= 0 or kA <= 0.
*/
template <typename Distance, typename = std::enable_if_t<

11
wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
View File

@@ -66,11 +66,16 @@ class EllipticalRegionConstraint : public TrajectoryConstraint {
* @return Whether the robot pose is within the constraint region.
*/
bool IsPoseInRegion(const Pose2d& pose) const {
// The region (disk) bounded by the ellipse is given by the equation:
// ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1
// The region bounded by the ellipse is given by the equation:
//
// (x−h)²/Rx² + (y−k)²/Ry² ≤ 1
//
// Multiply by Rx²Ry² for efficiency reasons:
//
// (x−h)²Ry² + (y−k)²Rx² ≤ Rx²Ry²
//
// If the inequality is satisfied, then it is inside the ellipse; otherwise
// it is outside the ellipse.
// Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons.
return units::math::pow<2>(pose.X() - m_center.X()) *
units::math::pow<2>(m_radii.Y()) +
units::math::pow<2>(pose.Y() - m_center.Y()) *

8
wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java
View File

@@ -141,7 +141,7 @@ class LinearFilterTest {
3,
// f(x) = ln(x)
(double x) -> Math.log(x),
// df/dx = 1 / x
// df/dx = 1/x
(double x) -> 1.0 / x,
h,
1.0,
@@ -174,7 +174,7 @@ class LinearFilterTest {
5,
// f(x) = ln(x)
(double x) -> Math.log(x),
// d²f/dx² = -1 / x²
// d²f/dx² = -1/x²
(double x) -> -1.0 / (x * x),
h,
1.0,
@@ -213,7 +213,7 @@ class LinearFilterTest {
2,
// f(x) = ln(x)
(double x) -> Math.log(x),
// df/dx = 1 / x
// df/dx = 1/x
(double x) -> 1.0 / x,
h,
1.0,
@@ -246,7 +246,7 @@ class LinearFilterTest {
4,
// f(x) = ln(x)
(double x) -> Math.log(x),
// d²f/dx² = -1 / x²
// d²f/dx² = -1/x²
(double x) -> -1.0 / (x * x),
h,
1.0,

10
wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
View File

@@ -102,8 +102,8 @@ class SwerveDriveKinematicsTest {
var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
/*
The circumference of the wheels about the COR is pi * diameter, or 2 * pi * radius
the radius is the sqrt(12^2in + 12^2in), or 16.9706in, so the circumference the wheels
The circumference of the wheels about the COR is π * diameter, or 2π * radius
the radius is the √(12²in + 12²in), or 16.9706in, so the circumference the wheels
trace out is 106.629190516in. since we want our robot to rotate at 1 rotation per second,
our wheels must trace out 1 rotation (or 106.63 inches) per second.
*/
@@ -143,7 +143,7 @@ class SwerveDriveKinematicsTest {
This one is a bit trickier. Because we are rotating about the front-left wheel,
it should be parked at 0 degrees and 0 speed. The front-right and back-left wheels both travel
an arc with radius 24 (and circumference 150.796), and the back-right wheel travels an arc with
radius sqrt(24^2 + 24^2) and circumference 213.2584. As for angles, the front-right wheel
radius √(24² + 24²) and circumference 213.2584. As for angles, the front-right wheel
should be pointing straight forward, the back-left wheel should be pointing straight right,
and the back-right wheel should be at a -45 degree angle
*/
@@ -169,12 +169,12 @@ class SwerveDriveKinematicsTest {
var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
/*
We already know that our omega should be 2pi from the previous test. Next, we need to determine
We already know that our omega should be 2π from the previous test. Next, we need to determine
the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center,
we know that vx and vy must be the same. Furthermore, we know that the center of mass makes
a full revolution about the center of revolution once every second. Therefore, the center of
mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triagle are
1:sqrt(2)/2:sqrt(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
*/
assertAll(

6
wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
View File

@@ -15,16 +15,16 @@ import org.junit.jupiter.api.Test;
class RungeKuttaTimeVaryingTest {
private static Matrix<N1, N1> rungeKuttaTimeVaryingSolution(double t) {
return new MatBuilder<>(Nat.N1(), Nat.N1())
.fill(12.0 * Math.exp(t) / (Math.pow(Math.exp(t) + 1.0, 2.0)));
.fill(12.0 * Math.exp(t) / Math.pow(Math.exp(t) + 1.0, 2.0));
}
// Tests RK4 with a time varying solution. From
// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
// x' = x (2 / (e^t + 1) - 1)
// x' = x (2/(eᵗ + 1) - 1)
//
// The true (analytical) solution is:
//
// x(t) = 12 * e^t / ((e^t + 1)^2)
// x(t) = 12eᵗ/(eᵗ + 1)²
@Test
void rungeKuttaTimeVaryingTest() {
final var y0 = rungeKuttaTimeVaryingSolution(5.0);

12
wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
View File

@@ -207,14 +207,14 @@ TEST(LinearFilterOutputTest, CentralFiniteDifference) {
return std::log(x);
},
[](double x) {
// df/dx = 1 / x
// df/dx = 1/x
return 1.0 / x;
},
h, 1.0, 20.0);
AssertCentralResults<2, 5>(
[](double x) {
// f(x) = x^2
// f(x) = x²
return x * x;
},
[](double x) {
@@ -240,7 +240,7 @@ TEST(LinearFilterOutputTest, CentralFiniteDifference) {
return std::log(x);
},
[](double x) {
// d²f/dx² = -1 / x²
// d²f/dx² = -1/x²
return -1.0 / (x * x);
},
h, 1.0, 20.0);
@@ -280,14 +280,14 @@ TEST(LinearFilterOutputTest, BackwardFiniteDifference) {
return std::log(x);
},
[](double x) {
// df/dx = 1 / x
// df/dx = 1/x
return 1.0 / x;
},
h, 1.0, 20.0);
AssertBackwardResults<2, 4>(
[](double x) {
// f(x) = x^2
// f(x) = x²
return x * x;
},
[](double x) {
@@ -313,7 +313,7 @@ TEST(LinearFilterOutputTest, BackwardFiniteDifference) {
return std::log(x);
},
[](double x) {
// d²f/dx² = -1 / x²
// d²f/dx² = -1/x²
return -1.0 / (x * x);
},
h, 1.0, 20.0);

7
wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
View File

@@ -10,18 +10,17 @@
namespace {
frc::Vectord<1> RungeKuttaTimeVaryingSolution(double t) {
return frc::Vectord<1>{12.0 * std::exp(t) /
(std::pow(std::exp(t) + 1.0, 2.0))};
return frc::Vectord<1>{12.0 * std::exp(t) / std::pow(std::exp(t) + 1.0, 2.0)};
}
} // namespace
// Tests RK4 with a time varying solution. From
// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
// x' = x (2 / (e^t + 1) - 1)
// x' = x (2/(eᵗ + 1) - 1)
//
// The true (analytical) solution is:
//
// x(t) = 12 * e^t / ((e^t + 1)^2)
// x(t) = 12eᵗ/((eᵗ + 1)²)
TEST(RungeKuttaTimeVaryingTest, RungeKuttaTimeVarying) {
frc::Vectord<1> y0 = RungeKuttaTimeVaryingSolution(5.0);