Add holonomic follower examples (#2052)

wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h

@@ -121,6 +121,22 @@ class SwerveDriveKinematics {
   template <typename... ModuleStates>
   ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates);
 
+  /**
+   * 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.
+   *
+   * @param moduleStates The state of the modules as an std::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.
+   */
+  ChassisSpeeds ToChassisSpeeds(
+      std::array<SwerveModuleState, NumModules> moduleStates);
+
   /**
    * Normalizes the wheel speeds using some max attainable speed. Sometimes,
    * after inverse kinematics, the requested speed from a/several modules may be

wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc

@@ -63,6 +63,13 @@ ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
                 "locations provided in constructor.");
 
   std::array<SwerveModuleState, NumModules> moduleStates{wheelStates...};
+
+  return this->ToChassisSpeeds(moduleStates);
+}
+
+template <size_t NumModules>
+ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
+    std::array<SwerveModuleState, NumModules> moduleStates) {
   Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
 
   for (size_t i = 0; i < NumModules; i++) {

wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h

@@ -14,7 +14,11 @@
 #include <units/units.h>
 
 #include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/SwerveDriveKinematics.h"
 #include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+#include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h"
+#include "frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h"
 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
 
 namespace frc {
@@ -88,6 +92,27 @@ class TrajectoryConfig {
         DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
   }
 
+  /**
+   * Adds a mecanum drive kinematics constraint to ensure that
+   * no wheel velocity of a mecanum drive goes above the max velocity.
+   *
+   * @param kinematics The mecanum drive kinematics.
+   */
+  void SetKinematics(MecanumDriveKinematics kinematics) {
+    AddConstraint(MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
+  }
+
+  /**
+   * Adds a swerve drive kinematics constraint to ensure that
+   * no wheel velocity of a swerve drive goes above the max velocity.
+   *
+   * @param kinematics The swerve drive kinematics.
+   */
+  template <size_t NumModules>
+  void SetKinematics(SwerveDriveKinematics<NumModules>& kinematics) {
+    AddConstraint(SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
+  }
+
   /**
    * Returns the starting velocity of the trajectory.
    * @return The starting velocity of the trajectory.

wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h

@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <cmath>
+
+#include <units/units.h>
+
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+/**
+ * A class that enforces constraints on the differential drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for both sides of the drivetrain stay below a certain
+ * limit.
+ */
+namespace frc {
+class MecanumDriveKinematicsConstraint : public TrajectoryConstraint {
+ public:
+  MecanumDriveKinematicsConstraint(MecanumDriveKinematics kinematics,
+                                   units::meters_per_second_t maxSpeed);
+
+  units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, curvature_t curvature,
+      units::meters_per_second_t velocity) override;
+
+  MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
+                            units::meters_per_second_t speed) override;
+
+ private:
+  MecanumDriveKinematics m_kinematics;
+  units::meters_per_second_t m_maxSpeed;
+};
+}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h

@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <cmath>
+
+#include <units/units.h>
+
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+/**
+ * A class that enforces constraints on the differential drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for both sides of the drivetrain stay below a certain
+ * limit.
+ */
+namespace frc {
+
+template <size_t NumModules>
+class SwerveDriveKinematicsConstraint : public TrajectoryConstraint {
+ public:
+  SwerveDriveKinematicsConstraint(
+      frc::SwerveDriveKinematics<NumModules> kinematics,
+      units::meters_per_second_t maxSpeed);
+
+  units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, curvature_t curvature,
+      units::meters_per_second_t velocity) override;
+
+  MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
+                            units::meters_per_second_t speed) override;
+
+ private:
+  frc::SwerveDriveKinematics<NumModules> m_kinematics;
+  units::meters_per_second_t m_maxSpeed;
+};
+}  // namespace frc
+
+#include "SwerveDriveKinematicsConstraint.inc"

wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc

@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+/**
+ * A class that enforces constraints on the differential drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for both sides of the drivetrain stay below a certain
+ * limit.
+ */
+
+namespace frc {
+
+template <size_t NumModules>
+SwerveDriveKinematicsConstraint<NumModules>::SwerveDriveKinematicsConstraint(
+    frc::SwerveDriveKinematics<NumModules> kinematics,
+    units::meters_per_second_t maxSpeed)
+    : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
+
+template <size_t NumModules>
+units::meters_per_second_t
+SwerveDriveKinematicsConstraint<NumModules>::MaxVelocity(
+    const Pose2d& pose, curvature_t curvature,
+    units::meters_per_second_t velocity) {
+  auto xVelocity = velocity * pose.Rotation().Cos();
+  auto yVelocity = velocity * pose.Rotation().Sin();
+  auto wheelSpeeds = m_kinematics.ToSwerveModuleStates(
+      {xVelocity, yVelocity, velocity * curvature});
+  m_kinematics.NormalizeWheelSpeeds(&wheelSpeeds, m_maxSpeed);
+
+  auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+
+  return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
+}
+
+template <size_t NumModules>
+TrajectoryConstraint::MinMax
+SwerveDriveKinematicsConstraint<NumModules>::MinMaxAcceleration(
+    const Pose2d& pose, curvature_t curvature,
+    units::meters_per_second_t speed) {
+  return {};
+}
+
+}  // namespace frc