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

[wpimath] 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

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