Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11: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

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()) *