[wpimath] Move math functionality into new wpimath library (#2629)

The wpimath library is a new library designed to separate the reusable math functionality
from the common utility library (wpiutil) and the hardware-dependent library (wpilibc/j).

Package names / include file names were NOT changed to minimize breakage.  In a future year
it would be good to revamp these for a more uniform user experience and to reduce the risk
of accidental naming conflicts.

While theoretically all of this functionality could be placed into wpiutil, several pieces
of this library (e.g. DARE) are very time-consuming to compile, so it's nice to avoid this
expense for users who only want cscore or ntcore.  It also allows for easy future separation
of build tasks vs number of workers on memory-constrained machines.

This moves the following functionality from wpiutil into wpimath:
- Eigen
- ejml
- Drake
- DARE
- wpiutil.math package (Matrix etc)
- units

And the following functionality from wpilibc/j into wpimath:
- Geometry
- Kinematics
- Spline
- Trajectory
- LinearFilter
- MedianFilter
- Feed-forward controllers

wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp

@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
+
+#include "units/math.h"
+
+using namespace frc;
+
+CentripetalAccelerationConstraint::CentripetalAccelerationConstraint(
+    units::meters_per_second_squared_t maxCentripetalAcceleration)
+    : m_maxCentripetalAcceleration(maxCentripetalAcceleration) {}
+
+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
+
+  // therefore, ac = v^2 * k
+  // ac / k = v^2
+  // v = std::sqrt(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
+  // unitless.
+  return units::math::sqrt(m_maxCentripetalAcceleration /
+                           units::math::abs(curvature) * 1_rad);
+}
+
+TrajectoryConstraint::MinMax
+CentripetalAccelerationConstraint::MinMaxAcceleration(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t speed) const {
+  // The acceleration of the robot has no impact on the centripetal acceleration
+  // of the robot.
+  return {};
+}

wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+
+using namespace frc;
+
+DifferentialDriveKinematicsConstraint::DifferentialDriveKinematicsConstraint(
+    DifferentialDriveKinematics kinematics, units::meters_per_second_t maxSpeed)
+    : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
+
+units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t velocity) const {
+  auto wheelSpeeds =
+      m_kinematics.ToWheelSpeeds({velocity, 0_mps, velocity * curvature});
+  wheelSpeeds.Normalize(m_maxSpeed);
+
+  return m_kinematics.ToChassisSpeeds(wheelSpeeds).vx;
+}
+
+TrajectoryConstraint::MinMax
+DifferentialDriveKinematicsConstraint::MinMaxAcceleration(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t speed) const {
+  return {};
+}

wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp

@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h"
+
+#include <algorithm>
+#include <limits>
+
+#include <wpi/MathExtras.h>
+
+#include "units/math.h"
+
+using namespace frc;
+
+DifferentialDriveVoltageConstraint::DifferentialDriveVoltageConstraint(
+    SimpleMotorFeedforward<units::meter> feedforward,
+    DifferentialDriveKinematics kinematics, units::volt_t maxVoltage)
+    : m_feedforward(feedforward),
+      m_kinematics(kinematics),
+      m_maxVoltage(maxVoltage) {}
+
+units::meters_per_second_t DifferentialDriveVoltageConstraint::MaxVelocity(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t velocity) const {
+  return units::meters_per_second_t(std::numeric_limits<double>::max());
+}
+
+TrajectoryConstraint::MinMax
+DifferentialDriveVoltageConstraint::MinMaxAcceleration(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t speed) const {
+  auto wheelSpeeds =
+      m_kinematics.ToWheelSpeeds({speed, 0_mps, speed * curvature});
+
+  auto maxWheelSpeed = std::max(wheelSpeeds.left, wheelSpeeds.right);
+  auto minWheelSpeed = std::min(wheelSpeeds.left, wheelSpeeds.right);
+
+  // Calculate maximum/minimum possible accelerations from motor dynamics
+  // and max/min wheel speeds
+  auto maxWheelAcceleration =
+      m_feedforward.MaxAchievableAcceleration(m_maxVoltage, maxWheelSpeed);
+  auto minWheelAcceleration =
+      m_feedforward.MinAchievableAcceleration(m_maxVoltage, minWheelSpeed);
+
+  // Robot chassis turning on radius = 1/|curvature|.  Outer wheel has radius
+  // increased by half of the trackwidth T.  Inner wheel has radius decreased
+  // by half of the trackwidth.  Achassis / radius = Aouter / (radius + T/2), 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
+  // outside of turn:
+  // If moving forward, max acceleration constraint corresponds to wheel on
+  // outside of turn If moving backward, max acceleration constraint corresponds
+  // to wheel on inside of turn
+
+  // When velocity is zero, then wheel velocities are uniformly zero (robot
+  // cannot be turning on its center) - we have to treat this as a special case,
+  // as it breaks the signum function.  Both max and min acceleration are
+  // *reduced in magnitude* in this case.
+
+  units::meters_per_second_squared_t maxChassisAcceleration;
+  units::meters_per_second_squared_t minChassisAcceleration;
+
+  if (speed == 0_mps) {
+    maxChassisAcceleration =
+        maxWheelAcceleration /
+        (1 + m_kinematics.trackWidth * units::math::abs(curvature) / (2_rad));
+    minChassisAcceleration =
+        minWheelAcceleration /
+        (1 + m_kinematics.trackWidth * units::math::abs(curvature) / (2_rad));
+  } else {
+    maxChassisAcceleration =
+        maxWheelAcceleration /
+        (1 + m_kinematics.trackWidth * units::math::abs(curvature) *
+                 wpi::sgn(speed) / (2_rad));
+    minChassisAcceleration =
+        minWheelAcceleration /
+        (1 - m_kinematics.trackWidth * units::math::abs(curvature) *
+                 wpi::sgn(speed) / (2_rad));
+  }
+
+  // When turning about a point inside of the wheelbase (i.e. radius less than
+  // half the trackwidth), the inner wheel's direction changes, but the
+  // magnitude remains the same.  The formula above changes sign for the inner
+  // wheel when this happens. We can accurately account for this by simply
+  // negating the inner wheel.
+
+  if ((m_kinematics.trackWidth / 2) > 1_rad / units::math::abs(curvature)) {
+    if (speed > 0_mps) {
+      minChassisAcceleration = -minChassisAcceleration;
+    } else if (speed < 0_mps) {
+      maxChassisAcceleration = -maxChassisAcceleration;
+    }
+  }
+
+  return {minChassisAcceleration, maxChassisAcceleration};
+}

wpimath/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp

@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/EllipticalRegionConstraint.h"
+
+#include <limits>
+
+#include "units/math.h"
+
+using namespace frc;
+
+EllipticalRegionConstraint::EllipticalRegionConstraint(
+    const Translation2d& center, units::meter_t xWidth, units::meter_t yWidth,
+    const Rotation2d& rotation, const TrajectoryConstraint& constraint)
+    : m_center(center),
+      m_radii(xWidth / 2.0, yWidth / 2.0),
+      m_constraint(constraint) {
+  m_radii = m_radii.RotateBy(rotation);
+}
+
+units::meters_per_second_t EllipticalRegionConstraint::MaxVelocity(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t velocity) const {
+  if (IsPoseInRegion(pose)) {
+    return m_constraint.MaxVelocity(pose, curvature, velocity);
+  } else {
+    return units::meters_per_second_t(std::numeric_limits<double>::infinity());
+  }
+}
+
+TrajectoryConstraint::MinMax EllipticalRegionConstraint::MinMaxAcceleration(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t speed) const {
+  if (IsPoseInRegion(pose)) {
+    return m_constraint.MinMaxAcceleration(pose, curvature, speed);
+  } else {
+    return {};
+  }
+}
+
+bool EllipticalRegionConstraint::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
+  // 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()) *
+                 units::math::pow<2>(m_radii.X()) <=
+         units::math::pow<2>(m_radii.X()) * units::math::pow<2>(m_radii.Y());
+}

wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h"
+
+#include "units/math.h"
+
+using namespace frc;
+
+MecanumDriveKinematicsConstraint::MecanumDriveKinematicsConstraint(
+    MecanumDriveKinematics kinematics, units::meters_per_second_t maxSpeed)
+    : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
+
+units::meters_per_second_t MecanumDriveKinematicsConstraint::MaxVelocity(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t velocity) const {
+  auto xVelocity = velocity * pose.Rotation().Cos();
+  auto yVelocity = velocity * pose.Rotation().Sin();
+  auto wheelSpeeds =
+      m_kinematics.ToWheelSpeeds({xVelocity, yVelocity, velocity * curvature});
+  wheelSpeeds.Normalize(m_maxSpeed);
+
+  auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+
+  return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
+}
+
+TrajectoryConstraint::MinMax
+MecanumDriveKinematicsConstraint::MinMaxAcceleration(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t speed) const {
+  return {};
+}

wpimath/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp

@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/RectangularRegionConstraint.h"
+
+#include <limits>
+
+using namespace frc;
+
+RectangularRegionConstraint::RectangularRegionConstraint(
+    const Translation2d& bottomLeftPoint, const Translation2d& topRightPoint,
+    const TrajectoryConstraint& constraint)
+    : m_bottomLeftPoint(bottomLeftPoint),
+      m_topRightPoint(topRightPoint),
+      m_constraint(constraint) {}
+
+units::meters_per_second_t RectangularRegionConstraint::MaxVelocity(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t velocity) const {
+  if (IsPoseInRegion(pose)) {
+    return m_constraint.MaxVelocity(pose, curvature, velocity);
+  } else {
+    return units::meters_per_second_t(std::numeric_limits<double>::infinity());
+  }
+}
+
+TrajectoryConstraint::MinMax RectangularRegionConstraint::MinMaxAcceleration(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t speed) const {
+  if (IsPoseInRegion(pose)) {
+    return m_constraint.MinMaxAcceleration(pose, curvature, speed);
+  } else {
+    return {};
+  }
+}
+
+bool RectangularRegionConstraint::IsPoseInRegion(const Pose2d& pose) const {
+  return pose.X() >= m_bottomLeftPoint.X() && pose.X() <= m_topRightPoint.X() &&
+         pose.Y() >= m_bottomLeftPoint.Y() && pose.Y() <= m_topRightPoint.Y();
+}