Add holonomic follower examples (#2052)

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