Add holonomic follower examples (#2052)

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java

@@ -0,0 +1,118 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.ArrayList;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class MecanumControllerCommandTest {
+  private final Timer m_timer = new Timer();
+  private Rotation2d m_angle = new Rotation2d(0);
+
+  private double m_frontLeftSpeed;
+  private double m_rearLeftSpeed;
+  private double m_frontRightSpeed;
+  private double m_rearRightSpeed;
+
+  private final ProfiledPIDController m_rotController = new ProfiledPIDController(1, 0, 0,
+      new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI));
+
+  private static final double kxTolerance = 1 / 12.0;
+  private static final double kyTolerance = 1 / 12.0;
+  private static final double kAngularTolerance = 1 / 12.0;
+
+  private static final double kWheelBase = 0.5;
+  private static final double kTrackWidth = 0.5;
+
+  private final MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics(
+      new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+      new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+      new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+      new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+
+  private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
+      new Rotation2d(0), new Pose2d(0, 0, new Rotation2d(0)));
+
+  public void setWheelSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
+    this.m_frontLeftSpeed = wheelSpeeds.frontLeftMetersPerSecond;
+    this.m_rearLeftSpeed = wheelSpeeds.rearLeftMetersPerSecond;
+    this.m_frontRightSpeed = wheelSpeeds.frontRightMetersPerSecond;
+    this.m_rearRightSpeed = wheelSpeeds.rearRightMetersPerSecond;
+  }
+
+  public MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
+    return new MecanumDriveWheelSpeeds(m_frontLeftSpeed,
+      m_frontRightSpeed, m_rearLeftSpeed, m_rearRightSpeed);
+  }
+
+  public Pose2d getRobotPose() {
+    m_odometry.updateWithTime(m_timer.get(), m_angle, getCurrentWheelSpeeds());
+    return m_odometry.getPoseMeters();
+  }
+
+  @Test
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  void testReachesReference() {
+    final var subsystem = new Subsystem() {};
+
+    final var waypoints = new ArrayList<Pose2d>();
+    waypoints.add(new Pose2d(0, 0, new Rotation2d(0)));
+    waypoints.add(new Pose2d(1, 5, new Rotation2d(3)));
+    var config = new TrajectoryConfig(8.8, 0.1);
+    final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+    final var endState = trajectory.sample(trajectory.getTotalTimeSeconds());
+
+    final var command = new MecanumControllerCommand(trajectory,
+        this::getRobotPose,
+        m_kinematics,
+        new PIDController(0.6, 0, 0),
+        new PIDController(0.6, 0, 0),
+        m_rotController,
+        8.8,
+        this::setWheelSpeeds,
+        subsystem);
+
+    m_timer.reset();
+    m_timer.start();
+
+    command.initialize();
+    while (!command.isFinished()) {
+      command.execute();
+      m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation();
+    }
+    m_timer.stop();
+    command.end(true);
+
+    assertAll(
+        () -> assertEquals(endState.poseMeters.getTranslation().getX(),
+          getRobotPose().getTranslation().getX(), kxTolerance),
+        () -> assertEquals(endState.poseMeters.getTranslation().getY(),
+          getRobotPose().getTranslation().getY(), kyTolerance),
+        () -> assertEquals(endState.poseMeters.getRotation().getRadians(),
+          getRobotPose().getRotation().getRadians(), kAngularTolerance)
+    );
+  }
+}

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java

@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.ArrayList;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class SwerveControllerCommandTest {
+  private final Timer m_timer = new Timer();
+  private Rotation2d m_angle = new Rotation2d(0);
+
+  private SwerveModuleState[] m_moduleStates = new SwerveModuleState[]{
+    new SwerveModuleState(0, new Rotation2d(0)),
+    new SwerveModuleState(0, new Rotation2d(0)),
+    new SwerveModuleState(0, new Rotation2d(0)),
+    new SwerveModuleState(0, new Rotation2d(0))};
+
+  private final ProfiledPIDController m_rotController = new ProfiledPIDController(1, 0, 0,
+      new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI));
+
+  private static final double kxTolerance = 1 / 12.0;
+  private static final double kyTolerance = 1 / 12.0;
+  private static final double kAngularTolerance = 1 / 12.0;
+
+  private static final double kWheelBase = 0.5;
+  private static final double kTrackWidth = 0.5;
+
+  private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
+      new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+      new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+      new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+      new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+
+  private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics,
+      new Rotation2d(0), new Pose2d(0, 0, new Rotation2d(0)));
+
+  @SuppressWarnings("PMD.ArrayIsStoredDirectly")
+  public void setModuleStates(SwerveModuleState[] moduleStates) {
+    this.m_moduleStates = moduleStates;
+  }
+
+  public Pose2d getRobotPose() {
+    m_odometry.updateWithTime(m_timer.get(), m_angle, m_moduleStates);
+    return m_odometry.getPoseMeters();
+  }
+
+  @Test
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  void testReachesReference() {
+    final var subsystem = new Subsystem() {};
+
+    final var waypoints = new ArrayList<Pose2d>();
+    waypoints.add(new Pose2d(0, 0, new Rotation2d(0)));
+    waypoints.add(new Pose2d(1, 5, new Rotation2d(3)));
+    var config = new TrajectoryConfig(8.8, 0.1);
+    final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+    final var endState = trajectory.sample(trajectory.getTotalTimeSeconds());
+
+    final var command = new SwerveControllerCommand(trajectory,
+        this::getRobotPose,
+        m_kinematics,
+        new PIDController(0.6, 0, 0),
+        new PIDController(0.6, 0, 0),
+        m_rotController,
+        this::setModuleStates,
+        subsystem);
+
+    m_timer.reset();
+    m_timer.start();
+
+    command.initialize();
+    while (!command.isFinished()) {
+      command.execute();
+      m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation();
+    }
+    m_timer.stop();
+    command.end(true);
+
+    assertAll(
+        () -> assertEquals(endState.poseMeters.getTranslation().getX(),
+          getRobotPose().getTranslation().getX(), kxTolerance),
+        () -> assertEquals(endState.poseMeters.getTranslation().getY(),
+          getRobotPose().getTranslation().getY(), kyTolerance),
+        () -> assertEquals(endState.poseMeters.getRotation().getRadians(),
+          getRobotPose().getRotation().getRadians(), kAngularTolerance)
+    );
+  }
+}

wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp

@@ -0,0 +1,116 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc2/Timer.h>
+#include <frc2/command/MecanumControllerCommand.h>
+#include <frc2/command/Subsystem.h>
+
+#include <iostream>
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Translation2d.h>
+#include <frc/kinematics/MecanumDriveKinematics.h>
+#include <frc/kinematics/MecanumDriveOdometry.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+#include <wpi/math>
+
+#include "gtest/gtest.h"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+class MecanumControllerCommandTest : public ::testing::Test {
+  using radians_per_second_squared_t =
+      units::compound_unit<units::radians,
+                           units::inverse<units::squared<units::second>>>;
+
+ protected:
+  frc2::Timer m_timer;
+  frc::Rotation2d m_angle{0_rad};
+
+  units::meters_per_second_t m_frontLeftSpeed = 0.0_mps;
+  units::meters_per_second_t m_rearLeftSpeed = 0.0_mps;
+  units::meters_per_second_t m_frontRightSpeed = 0.0_mps;
+  units::meters_per_second_t m_rearRightSpeed = 0.0_mps;
+
+  frc::ProfiledPIDController<units::radians> m_rotController{
+      1, 0, 0,
+      frc::TrapezoidProfile<units::radians>::Constraints{
+          9_rad_per_s, units::unit_t<radians_per_second_squared_t>(3)}};
+
+  static constexpr units::meter_t kxTolerance{1 / 12.0};
+  static constexpr units::meter_t kyTolerance{1 / 12.0};
+  static constexpr units::radian_t kAngularTolerance{1 / 12.0};
+
+  static constexpr units::meter_t kWheelBase{0.5};
+  static constexpr units::meter_t kTrackWidth{0.5};
+
+  frc::MecanumDriveKinematics m_kinematics{
+      frc::Translation2d{kWheelBase / 2, kTrackWidth / 2},
+      frc::Translation2d{kWheelBase / 2, -kTrackWidth / 2},
+      frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2},
+      frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}};
+
+  frc::MecanumDriveOdometry m_odometry{m_kinematics, 0_rad,
+                                       frc::Pose2d{0_m, 0_m, 0_rad}};
+
+  frc::MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
+    return frc::MecanumDriveWheelSpeeds{m_frontLeftSpeed, m_frontRightSpeed,
+                                        m_rearLeftSpeed, m_rearRightSpeed};
+  }
+
+  frc::Pose2d getRobotPose() {
+    m_odometry.UpdateWithTime(m_timer.Get(), m_angle, getCurrentWheelSpeeds());
+    return m_odometry.GetPose();
+  }
+};
+
+TEST_F(MecanumControllerCommandTest, ReachesReference) {
+  frc2::Subsystem subsystem{};
+
+  auto waypoints =
+      std::vector{frc::Pose2d{0_m, 0_m, 0_rad}, frc::Pose2d{1_m, 5_m, 3_rad}};
+  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      waypoints, {8.8_mps, 0.1_mps_sq});
+
+  auto endState = trajectory.Sample(trajectory.TotalTime());
+
+  auto command = frc2::MecanumControllerCommand(
+      trajectory, [&]() { return getRobotPose(); }, m_kinematics,
+
+      frc2::PIDController(0.6, 0, 0), frc2::PIDController(0.6, 0, 0),
+      m_rotController, units::meters_per_second_t(8.8),
+      [&](units::meters_per_second_t frontLeft,
+          units::meters_per_second_t rearLeft,
+          units::meters_per_second_t frontRight,
+          units::meters_per_second_t rearRight) {
+        m_frontLeftSpeed = frontLeft;
+        m_rearLeftSpeed = rearLeft;
+        m_frontRightSpeed = frontRight;
+        m_rearRightSpeed = rearRight;
+      },
+      {&subsystem});
+
+  m_timer.Reset();
+  m_timer.Start();
+  command.Initialize();
+  while (!command.IsFinished()) {
+    command.Execute();
+    m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation();
+  }
+  m_timer.Stop();
+  command.End(false);
+
+  EXPECT_NEAR_UNITS(endState.pose.Translation().X(),
+                    getRobotPose().Translation().X(), kxTolerance);
+  EXPECT_NEAR_UNITS(endState.pose.Translation().Y(),
+                    getRobotPose().Translation().Y(), kyTolerance);
+  EXPECT_NEAR_UNITS(endState.pose.Rotation().Radians(),
+                    getRobotPose().Rotation().Radians(), kAngularTolerance);
+}

wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp

@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <frc2/Timer.h>
+#include <frc2/command/Subsystem.h>
+#include <frc2/command/SwerveControllerCommand.h>
+
+#include <iostream>
+
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Translation2d.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/kinematics/SwerveDriveOdometry.h>
+#include <frc/kinematics/SwerveModuleState.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+#include <wpi/math>
+
+#include "gtest/gtest.h"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+class SwerveControllerCommandTest : public ::testing::Test {
+  using radians_per_second_squared_t =
+      units::compound_unit<units::radians,
+                           units::inverse<units::squared<units::second>>>;
+
+ protected:
+  frc2::Timer m_timer;
+  frc::Rotation2d m_angle{0_rad};
+
+  std::array<frc::SwerveModuleState, 4> m_moduleStates{
+      frc::SwerveModuleState{}, frc::SwerveModuleState{},
+      frc::SwerveModuleState{}, frc::SwerveModuleState{}};
+
+  frc::ProfiledPIDController<units::radians> m_rotController{
+      1, 0, 0,
+      frc::TrapezoidProfile<units::radians>::Constraints{
+          9_rad_per_s, units::unit_t<radians_per_second_squared_t>(3)}};
+
+  static constexpr units::meter_t kxTolerance{1 / 12.0};
+  static constexpr units::meter_t kyTolerance{1 / 12.0};
+  static constexpr units::radian_t kAngularTolerance{1 / 12.0};
+
+  static constexpr units::meter_t kWheelBase{0.5};
+  static constexpr units::meter_t kTrackWidth{0.5};
+
+  frc::SwerveDriveKinematics<4> m_kinematics{
+      frc::Translation2d{kWheelBase / 2, kTrackWidth / 2},
+      frc::Translation2d{kWheelBase / 2, -kTrackWidth / 2},
+      frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2},
+      frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}};
+
+  frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad,
+                                         frc::Pose2d{0_m, 0_m, 0_rad}};
+
+  std::array<frc::SwerveModuleState, 4> getCurrentWheelSpeeds() {
+    return m_moduleStates;
+  }
+
+  frc::Pose2d getRobotPose() {
+    m_odometry.UpdateWithTime(m_timer.Get(), m_angle, getCurrentWheelSpeeds());
+    return m_odometry.GetPose();
+  }
+};
+
+TEST_F(SwerveControllerCommandTest, ReachesReference) {
+  frc2::Subsystem subsystem{};
+
+  auto waypoints =
+      std::vector{frc::Pose2d{0_m, 0_m, 0_rad}, frc::Pose2d{1_m, 5_m, 3_rad}};
+  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      waypoints, {8.8_mps, 0.1_mps_sq});
+
+  auto endState = trajectory.Sample(trajectory.TotalTime());
+
+  auto command = frc2::SwerveControllerCommand<4>(
+      trajectory, [&]() { return getRobotPose(); }, m_kinematics,
+
+      frc2::PIDController(0.6, 0, 0), frc2::PIDController(0.6, 0, 0),
+      m_rotController,
+      [&](auto moduleStates) { m_moduleStates = moduleStates; }, {&subsystem});
+
+  m_timer.Reset();
+  m_timer.Start();
+  command.Initialize();
+  while (!command.IsFinished()) {
+    command.Execute();
+    m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation();
+  }
+  m_timer.Stop();
+  command.End(false);
+
+  EXPECT_NEAR_UNITS(endState.pose.Translation().X(),
+                    getRobotPose().Translation().X(), kxTolerance);
+  EXPECT_NEAR_UNITS(endState.pose.Translation().Y(),
+                    getRobotPose().Translation().Y(), kyTolerance);
+  EXPECT_NEAR_UNITS(endState.pose.Rotation().Radians(),
+                    getRobotPose().Rotation().Radians(), kAngularTolerance);
+}