Add kinematics suite (#1787)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>

wpilibc/src/main/native/cpp/kinematics/DifferentialDriveKinematics.cpp

@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "frc/kinematics/DifferentialDriveKinematics.h"
+
+using namespace frc;
+
+DifferentialDriveKinematics::DifferentialDriveKinematics(
+    units::meter_t trackWidth)
+    : m_trackWidth(trackWidth) {}
+
+ChassisSpeeds DifferentialDriveKinematics::ToChassisSpeeds(
+    const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
+  return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
+          (wheelSpeeds.right - wheelSpeeds.left) / m_trackWidth * 1_rad};
+}
+
+DifferentialDriveWheelSpeeds DifferentialDriveKinematics::ToWheelSpeeds(
+    const ChassisSpeeds& chassisSpeeds) const {
+  return {chassisSpeeds.vx - m_trackWidth / 2 * chassisSpeeds.omega / 1_rad,
+          chassisSpeeds.vx + m_trackWidth / 2 * chassisSpeeds.omega / 1_rad};
+}

wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp

@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "frc/kinematics/DifferentialDriveOdometry.h"
+
+using namespace frc;
+
+DifferentialDriveOdometry::DifferentialDriveOdometry(
+    DifferentialDriveKinematics kinematics, const Pose2d& initialPose)
+    : m_kinematics(kinematics), m_pose(initialPose) {
+  m_previousAngle = m_pose.Rotation();
+}
+
+const Pose2d& DifferentialDriveOdometry::UpdateWithTime(
+    units::second_t currentTime, const Rotation2d& angle,
+    const DifferentialDriveWheelSpeeds& wheelSpeeds) {
+  units::second_t deltaTime =
+      (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
+  m_previousTime = currentTime;
+
+  auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+  static_cast<void>(dtheta);
+
+  auto newPose = m_pose.Exp(
+      {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
+
+  m_previousAngle = angle;
+  m_pose = {newPose.Translation(), angle};
+
+  return m_pose;
+}

wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp

@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+
+using namespace frc;
+
+void DifferentialDriveWheelSpeeds::Normalize(
+    units::meters_per_second_t attainableMaxSpeed) {
+  auto realMaxSpeed =
+      units::math::max(units::math::abs(left), units::math::abs(right));
+
+  if (realMaxSpeed > attainableMaxSpeed) {
+    left = left / realMaxSpeed * attainableMaxSpeed;
+    right = right / realMaxSpeed * attainableMaxSpeed;
+  }
+}

wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp

@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "frc/kinematics/MecanumDriveKinematics.h"
+
+using namespace frc;
+
+MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
+    const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) {
+  // We have a new center of rotation. We need to compute the matrix again.
+  if (centerOfRotation.X() != m_previousCoR.X() ||
+      centerOfRotation.Y() != m_previousCoR.Y()) {
+    auto fl = m_frontLeftWheel - centerOfRotation;
+    auto fr = m_frontRightWheel - centerOfRotation;
+    auto rl = m_rearLeftWheel - centerOfRotation;
+    auto rr = m_rearRightWheel - centerOfRotation;
+
+    SetInverseKinematics(fl, fr, rl, rr);
+
+    m_previousCoR = centerOfRotation;
+  }
+
+  Eigen::Vector3d chassisSpeedsVector;
+  chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
+      chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
+
+  Eigen::Matrix<double, 4, 1> wheelsMatrix =
+      m_inverseKinematics * chassisSpeedsVector;
+
+  MecanumDriveWheelSpeeds wheelSpeeds;
+  wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsMatrix(0, 0)};
+  wheelSpeeds.frontRight = units::meters_per_second_t{wheelsMatrix(1, 0)};
+  wheelSpeeds.rearLeft = units::meters_per_second_t{wheelsMatrix(2, 0)};
+  wheelSpeeds.rearRight = units::meters_per_second_t{wheelsMatrix(3, 0)};
+  return wheelSpeeds;
+}
+
+ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
+    const MecanumDriveWheelSpeeds& wheelSpeeds) {
+  Eigen::Matrix<double, 4, 1> wheelSpeedsMatrix;
+  // clang-format off
+  wheelSpeedsMatrix << wheelSpeeds.frontLeft.to<double>(), wheelSpeeds.frontRight.to<double>(),
+                       wheelSpeeds.rearLeft.to<double>(), wheelSpeeds.rearRight.to<double>();
+  // clang-format on
+
+  Eigen::Vector3d chassisSpeedsVector =
+      m_forwardKinematics.solve(wheelSpeedsMatrix);
+
+  return {units::meters_per_second_t{chassisSpeedsVector(0)},
+          units::meters_per_second_t{chassisSpeedsVector(1)},
+          units::radians_per_second_t{chassisSpeedsVector(2)}};
+}
+
+void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
+                                                  Translation2d fr,
+                                                  Translation2d rl,
+                                                  Translation2d rr) {
+  // clang-format off
+  m_inverseKinematics << 1, -1, (-(fl.X() + fl.Y())).template to<double>(),
+                         1,  1, (fr.X() - fr.Y()).template to<double>(),
+                         1,  1, (rl.X() - rl.Y()).template to<double>(),
+                         1, -1, (-(rr.X() + rr.Y())).template to<double>();
+  // clang-format on
+  m_inverseKinematics /= std::sqrt(2);
+}

wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp

@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "frc/kinematics/MecanumDriveOdometry.h"
+
+using namespace frc;
+
+MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
+                                           const Pose2d& initialPose)
+    : m_kinematics(kinematics), m_pose(initialPose) {
+  m_previousAngle = m_pose.Rotation();
+}
+
+const Pose2d& MecanumDriveOdometry::UpdateWithTime(
+    units::second_t currentTime, const Rotation2d& angle,
+    MecanumDriveWheelSpeeds wheelSpeeds) {
+  units::second_t deltaTime =
+      (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
+  m_previousTime = currentTime;
+
+  auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+  static_cast<void>(dtheta);
+
+  auto newPose = m_pose.Exp(
+      {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
+
+  m_previousAngle = angle;
+  m_pose = {newPose.Translation(), angle};
+
+  return m_pose;
+}

wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp

@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "frc/kinematics/MecanumDriveWheelSpeeds.h"
+
+#include <algorithm>
+#include <array>
+#include <cmath>
+
+using namespace frc;
+
+void MecanumDriveWheelSpeeds::Normalize(
+    units::meters_per_second_t attainableMaxSpeed) {
+  std::array<units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
+                                                        rearLeft, rearRight};
+  units::meters_per_second_t realMaxSpeed = *std::max_element(
+      wheelSpeeds.begin(), wheelSpeeds.end(), [](const auto& a, const auto& b) {
+        return units::math::abs(a) < units::math::abs(b);
+      });
+
+  if (realMaxSpeed > attainableMaxSpeed) {
+    for (int i = 0; i < 4; ++i) {
+      wheelSpeeds[i] = wheelSpeeds[i] / realMaxSpeed * attainableMaxSpeed;
+    }
+    frontLeft = wheelSpeeds[0];
+    frontRight = wheelSpeeds[1];
+    rearLeft = wheelSpeeds[2];
+    rearRight = wheelSpeeds[3];
+  }
+}

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

@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <units/units.h>
+
+#include "frc/geometry/Rotation2d.h"
+
+namespace frc {
+/**
+ * Represents the speed of a robot chassis. Although this struct contains
+ * similar members compared to a Twist2d, they do NOT represent the same thing.
+ * Whereas a Twist2d represents a change in pose w.r.t to the robot frame of
+ * reference, this ChassisSpeeds struct represents a velocity w.r.t to the robot
+ * frame of reference.
+ *
+ * A strictly non-holonomic drivetrain, such as a differential drive, should
+ * never have a dy component because it can never move sideways. Holonomic
+ * drivetrains such as swerve and mecanum will often have all three components.
+ */
+struct ChassisSpeeds {
+  /**
+   * Represents forward velocity w.r.t the robot frame of reference. (Fwd is +)
+   */
+  units::meters_per_second_t vx = 0_mps;
+
+  /**
+   * Represents strafe velocity w.r.t the robot frame of reference. (Left is +)
+   */
+  units::meters_per_second_t vy = 0_mps;
+
+  /**
+   * Represents the angular velocity of the robot frame. (CCW is +)
+   */
+  units::radians_per_second_t omega = 0_rad_per_s;
+
+  /**
+   * Converts a user provided field-relative set of speeds into a robot-relative
+   * ChassisSpeeds object.
+   *
+   * @param vx The component of speed in the x direction relative to the field.
+   * Positive x is away from your alliance wall.
+   * @param vy The component of speed in the y direction relative to the field.
+   * Positive y is to your left when standing behind the alliance wall.
+   * @param omega The angular rate of the robot.
+   * @param robotAngle The angle of the robot as measured by a gyroscope. The
+   * robot's angle is considered to be zero when it is facing directly away from
+   * your alliance station wall. Remember that this should be CCW positive.
+   *
+   * @return ChassisSpeeds object representing the speeds in the robot's frame
+   * of reference.
+   */
+  static ChassisSpeeds FromFieldRelativeSpeeds(
+      units::meters_per_second_t vx, units::meters_per_second_t vy,
+      units::radians_per_second_t omega, const Rotation2d& robotAngle) {
+    return {vx * robotAngle.Cos() + vy * robotAngle.Sin(),
+            -vx * robotAngle.Sin() + vy * robotAngle.Cos(), omega};
+  }
+};
+}  // namespace frc

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

@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <units/units.h>
+
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+
+namespace frc {
+/**
+ * Helper class that converts a chassis velocity (dx and dtheta components) to
+ * left and right wheel velocities for a differential drive.
+ *
+ * Inverse kinematics converts a desired chassis speed into left and right
+ * velocity components whereas forward kinematics converts left and right
+ * component velocities into a linear and angular chassis speed.
+ */
+class DifferentialDriveKinematics {
+ public:
+  /**
+   * Constructs a differential drive kinematics object.
+   *
+   * @param trackWidth The track width of the drivetrain. Theoretically, this is
+   * the distance between the left wheels and right wheels. However, the
+   * empirical value may be larger than the physical measured value due to
+   * scrubbing effects.
+   */
+  explicit DifferentialDriveKinematics(units::meter_t trackWidth);
+
+  /**
+   * Returns a chassis speed from left and right component velocities using
+   * forward kinematics.
+   *
+   * @param wheelSpeeds The left and right velocities.
+   * @return The chassis speed.
+   */
+  ChassisSpeeds ToChassisSpeeds(
+      const DifferentialDriveWheelSpeeds& wheelSpeeds) const;
+
+  /**
+   * Returns left and right component velocities from a chassis speed using
+   * inverse kinematics.
+   *
+   * @param chassisSpeeds The linear and angular (dx and dtheta) components that
+   * represent the chassis' speed.
+   * @return The left and right velocities.
+   */
+  DifferentialDriveWheelSpeeds ToWheelSpeeds(
+      const ChassisSpeeds& chassisSpeeds) const;
+
+ private:
+  units::meter_t m_trackWidth;
+};
+}  // namespace frc