diff --git a/wpimath/src/main/java/edu/wpi/first/math/InterpolatingMatrixTreeMap.java b/wpimath/src/main/java/edu/wpi/first/math/InterpolatingMatrixTreeMap.java new file mode 100644 index 0000000000..a7b922c969 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/InterpolatingMatrixTreeMap.java @@ -0,0 +1,91 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math; + +import java.util.TreeMap; + +/** + * Interpolating Tree Maps are used to get values at points that are not defined by making a guess + * from points that are defined. This uses linear interpolation. + */ +public class InterpolatingMatrixTreeMap { + private final TreeMap> m_map = new TreeMap<>(); + + /** + * Inserts a key-value pair. + * + * @param key The key. + * @param value The value. + */ + public void put(K key, Matrix value) { + m_map.put(key, value); + } + + /** + * Returns the value associated with a given key. + * + *

If there's no matching key, the value returned will be a linear interpolation between the + * keys before and after the provided one. + * + * @param key The key. + * @return The value associated with the given key. + */ + public Matrix get(K key) { + Matrix val = m_map.get(key); + if (val == null) { + K ceilingKey = m_map.ceilingKey(key); + K floorKey = m_map.floorKey(key); + + if (ceilingKey == null && floorKey == null) { + return null; + } + if (ceilingKey == null) { + return m_map.get(floorKey); + } + if (floorKey == null) { + return m_map.get(ceilingKey); + } + Matrix floor = m_map.get(floorKey); + Matrix ceiling = m_map.get(ceilingKey); + + return interpolate(floor, ceiling, inverseInterpolate(ceilingKey, key, floorKey)); + } else { + return val; + } + } + + /** + * Return the value interpolated between val1 and val2 by the interpolant d. + * + * @param val1 The lower part of the interpolation range. + * @param val2 The upper part of the interpolation range. + * @param d The interpolant in the range [0, 1]. + * @return The interpolated value. + */ + public Matrix interpolate(Matrix val1, Matrix val2, double d) { + var dydx = val2.minus(val1); + return dydx.times(d).plus(val1); + } + + /** + * Return where within interpolation range [0, 1] q is between down and up. + * + * @param up Upper part of interpolation range. + * @param q Query. + * @param down Lower part of interpolation range. + * @return Interpolant in range [0, 1]. + */ + public double inverseInterpolate(K up, K q, K down) { + double upperToLower = up.doubleValue() - down.doubleValue(); + if (upperToLower <= 0) { + return 0.0; + } + double queryToLower = q.doubleValue() - down.doubleValue(); + if (queryToLower <= 0) { + return 0.0; + } + return queryToLower / upperToLower; + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java new file mode 100644 index 0000000000..5b87aa6cd1 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java @@ -0,0 +1,289 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.controller; + +import edu.wpi.first.math.InterpolatingMatrixTreeMap; +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.StateSpaceUtil; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.numbers.N5; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.trajectory.Trajectory; + +/** + * The linear time-varying differential drive controller has a similar form to the LQR, but the + * model used to compute the controller gain is the nonlinear model linearized around the + * drivetrain's current state. We precomputed gains for important places in our state-space, then + * interpolated between them with a LUT to save computational resources. + * + *

See section 8.7 in Controls Engineering in FRC for a derivation of the control law we used + * shown in theorem 8.7.4. + */ +public class LTVDifferentialDriveController { + private final double m_trackwidth; + + // LUT from drivetrain linear velocity to LQR gain + private final InterpolatingMatrixTreeMap m_table = + new InterpolatingMatrixTreeMap<>(); + + private Matrix m_error = new Matrix<>(Nat.N5(), Nat.N1()); + private Matrix m_tolerance = new Matrix<>(Nat.N5(), Nat.N1()); + + /** Motor voltages for a differential drive. */ + @SuppressWarnings("MemberName") + public static class WheelVoltages { + public double left; + public double right; + + public WheelVoltages() {} + + public WheelVoltages(double left, double right) { + this.left = left; + this.right = right; + } + } + + /** States of the drivetrain system. */ + enum State { + kX(0), + kY(1), + kHeading(2), + kLeftVelocity(3), + kRightVelocity(4); + + @SuppressWarnings("MemberName") + public final int value; + + @SuppressWarnings("ParameterName") + State(int i) { + this.value = i; + } + } + + /** + * Constructs a linear time-varying differential drive controller. + * + * @param plant The drivetrain velocity plant. + * @param trackwidth The drivetrain's trackwidth in meters. + * @param qelems The maximum desired error tolerance for each state. + * @param relems The maximum desired control effort for each input. + * @param dt Discretization timestep in seconds. + */ + @SuppressWarnings("LocalVariableName") + public LTVDifferentialDriveController( + LinearSystem plant, + double trackwidth, + Vector qelems, + Vector relems, + double dt) { + m_trackwidth = trackwidth; + + var A = + new MatBuilder<>(Nat.N5(), Nat.N5()) + .fill( + 0.0, + 0.0, + 0.0, + 0.5, + 0.5, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + -1.0 / m_trackwidth, + 1.0 / m_trackwidth, + 0.0, + 0.0, + 0.0, + plant.getA(0, 0), + plant.getA(0, 1), + 0.0, + 0.0, + 0.0, + plant.getA(1, 0), + plant.getA(1, 1)); + var B = + new MatBuilder<>(Nat.N5(), Nat.N2()) + .fill( + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + plant.getB(0, 0), + plant.getB(0, 1), + plant.getB(1, 0), + plant.getB(1, 1)); + var Q = StateSpaceUtil.makeCostMatrix(qelems); + var R = StateSpaceUtil.makeCostMatrix(relems); + + // dx/dt = Ax + Bu + // 0 = Ax + Bu + // Ax = -Bu + // x = -A⁻¹Bu + double maxV = + plant + .getA() + .solve(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0))) + .times(-1.0) + .get(0, 0); + + var x = new Matrix<>(Nat.N5(), Nat.N1()); + for (double velocity = -maxV; velocity < maxV; velocity += 0.01) { + x.set(State.kLeftVelocity.value, 0, velocity); + x.set(State.kRightVelocity.value, 0, velocity); + + // The DARE is ill-conditioned if the velocity is close to zero, so don't + // let the system stop. + if (Math.abs(velocity) < 1e-4) { + m_table.put(velocity, new Matrix<>(Nat.N2(), Nat.N5())); + } else { + A.set(State.kY.value, State.kHeading.value, velocity); + m_table.put(velocity, new LinearQuadraticRegulator(A, B, Q, R, dt).getK()); + } + } + } + + /** + * Returns true if the pose error is within tolerance of the reference. + * + * @return True if the pose error is within tolerance of the reference. + */ + public boolean atReference() { + return Math.abs(m_error.get(0, 0)) < m_tolerance.get(0, 0) + && Math.abs(m_error.get(1, 0)) < m_tolerance.get(1, 0) + && Math.abs(m_error.get(2, 0)) < m_tolerance.get(2, 0) + && Math.abs(m_error.get(3, 0)) < m_tolerance.get(3, 0) + && Math.abs(m_error.get(4, 0)) < m_tolerance.get(4, 0); + } + + /** + * Sets the pose error which is considered tolerable for use with atReference(). + * + * @param poseTolerance Pose error which is tolerable. + * @param leftVelocityTolerance Left velocity error which is tolerable in meters per second. + * @param rightVelocityTolerance Right velocity error which is tolerable in meters per second. + */ + public void setTolerance( + Pose2d poseTolerance, double leftVelocityTolerance, double rightVelocityTolerance) { + m_tolerance = + new MatBuilder<>(Nat.N5(), Nat.N1()) + .fill( + poseTolerance.getX(), + poseTolerance.getY(), + poseTolerance.getRotation().getRadians(), + leftVelocityTolerance, + rightVelocityTolerance); + } + + /** + * Returns the left and right output voltages of the LTV controller. + * + *

The reference pose, linear velocity, and angular velocity should come from a drivetrain + * trajectory. + * + * @param currentPose The current pose. + * @param leftVelocity The current left velocity in meters per second. + * @param rightVelocity The current right velocity in meters per second. + * @param poseRef The desired pose. + * @param leftVelocityRef The desired left velocity in meters per second. + * @param rightVelocityRef The desired right velocity in meters per second. + * @return Left and right output voltages of the LTV controller. + */ + @SuppressWarnings("LocalVariableName") + public WheelVoltages calculate( + Pose2d currentPose, + double leftVelocity, + double rightVelocity, + Pose2d poseRef, + double leftVelocityRef, + double rightVelocityRef) { + // This implements the linear time-varying differential drive controller in + // theorem 9.6.3 of https://tavsys.net/controls-in-frc. + var x = + new MatBuilder<>(Nat.N5(), Nat.N1()) + .fill( + currentPose.getX(), + currentPose.getY(), + currentPose.getRotation().getRadians(), + leftVelocity, + rightVelocity); + + var inRobotFrame = Matrix.eye(Nat.N5()); + inRobotFrame.set(0, 0, Math.cos(x.get(State.kHeading.value, 0))); + inRobotFrame.set(0, 1, Math.sin(x.get(State.kHeading.value, 0))); + inRobotFrame.set(1, 0, -Math.sin(x.get(State.kHeading.value, 0))); + inRobotFrame.set(1, 1, Math.cos(x.get(State.kHeading.value, 0))); + + var r = + new MatBuilder<>(Nat.N5(), Nat.N1()) + .fill( + poseRef.getX(), + poseRef.getY(), + poseRef.getRotation().getRadians(), + leftVelocityRef, + rightVelocityRef); + m_error = r.minus(x); + m_error.set( + State.kHeading.value, 0, MathUtil.angleModulus(m_error.get(State.kHeading.value, 0))); + + double velocity = (leftVelocity + rightVelocity) / 2.0; + var K = m_table.get(velocity); + + var u = K.times(inRobotFrame).times(m_error); + + return new WheelVoltages(u.get(0, 0), u.get(1, 0)); + } + + /** + * Returns the left and right output voltages of the LTV controller. + * + *

The reference pose, linear velocity, and angular velocity should come from a drivetrain + * trajectory. + * + * @param currentPose The current pose. + * @param leftVelocity The left velocity in meters per second. + * @param rightVelocity The right velocity in meters per second. + * @param desiredState The desired pose, linear velocity, and angular velocity from a trajectory. + * @return The left and right output voltages of the LTV controller. + */ + public WheelVoltages calculate( + Pose2d currentPose, + double leftVelocity, + double rightVelocity, + Trajectory.State desiredState) { + // v = (v_r + v_l) / 2 (1) + // w = (v_r - v_l) / (2r) (2) + // k = w / v (3) + // + // v_l = v - wr + // v_l = v - (vk)r + // v_l = v(1 - kr) + // + // v_r = v + wr + // v_r = v + (vk)r + // v_r = v(1 + kr) + return calculate( + currentPose, + leftVelocity, + rightVelocity, + desiredState.poseMeters, + desiredState.velocityMetersPerSecond + * (1 - (desiredState.curvatureRadPerMeter * m_trackwidth / 2.0)), + desiredState.velocityMetersPerSecond + * (1 + (desiredState.curvatureRadPerMeter * m_trackwidth / 2.0))); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java new file mode 100644 index 0000000000..dfd8efd92e --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java @@ -0,0 +1,165 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.controller; + +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.StateSpaceUtil; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.trajectory.Trajectory; + +/** + * The linear time-varying unicycle controller has a similar form to the LQR, but the model used to + * compute the controller gain is the nonlinear model linearized around the drivetrain's current + * state. + * + *

See section 8.9 in Controls Engineering in FRC for a derivation of the control law we used + * shown in theorem 8.9.1. + */ +@SuppressWarnings("MemberName") +public class LTVUnicycleController { + private final Matrix m_A = new Matrix<>(Nat.N3(), Nat.N3()); + private final Matrix m_B = + new MatBuilder<>(Nat.N3(), Nat.N2()).fill(1.0, 0.0, 0.0, 0.0, 0.0, 1.0); + private final Matrix m_Q; + private final Matrix m_R; + private final double m_dt; + + private Pose2d m_poseError; + private Pose2d m_poseTolerance; + private boolean m_enabled = true; + + /** States of the drivetrain system. */ + enum State { + kX(0), + kY(1), + kHeading(2); + + @SuppressWarnings("MemberName") + public final int value; + + @SuppressWarnings("ParameterName") + State(int i) { + this.value = i; + } + } + + /** Inputs of the drivetrain system. */ + enum Input { + kLeftVelocity(3), + kRightVelocity(4); + + @SuppressWarnings("MemberName") + public final int value; + + @SuppressWarnings("ParameterName") + Input(int i) { + this.value = i; + } + } + + /** + * Constructs a linear time-varying unicycle controller. + * + * @param qelems The maximum desired error tolerance for each state. + * @param relems The maximum desired control effort for each input. + * @param dt Discretization timestep in seconds. + */ + public LTVUnicycleController(Vector qelems, Vector relems, double dt) { + m_dt = dt; + m_Q = StateSpaceUtil.makeCostMatrix(qelems); + m_R = StateSpaceUtil.makeCostMatrix(relems); + } + + /** + * Returns true if the pose error is within tolerance of the reference. + * + * @return True if the pose error is within tolerance of the reference. + */ + public boolean atReference() { + final var eTranslate = m_poseError.getTranslation(); + final var eRotate = m_poseError.getRotation(); + final var tolTranslate = m_poseTolerance.getTranslation(); + final var tolRotate = m_poseTolerance.getRotation(); + return Math.abs(eTranslate.getX()) < tolTranslate.getX() + && Math.abs(eTranslate.getY()) < tolTranslate.getY() + && Math.abs(eRotate.getRadians()) < tolRotate.getRadians(); + } + + /** + * Sets the pose error which is considered tolerable for use with atReference(). + * + * @param poseTolerance Pose error which is tolerable. + */ + public void setTolerance(Pose2d poseTolerance) { + m_poseTolerance = poseTolerance; + } + + /** + * Returns the linear and angular velocity outputs of the LTV controller. + * + *

The reference pose, linear velocity, and angular velocity should come from a drivetrain + * trajectory. + * + * @param currentPose The current pose. + * @param poseRef The desired pose. + * @param linearVelocityRef The desired linear velocity in meters per second. + * @param angularVelocityRef The desired angular velocity in radians per second. + * @return The linear and angular velocity outputs of the LTV controller. + */ + public ChassisSpeeds calculate( + Pose2d currentPose, Pose2d poseRef, double linearVelocityRef, double angularVelocityRef) { + if (!m_enabled) { + return new ChassisSpeeds(linearVelocityRef, 0.0, angularVelocityRef); + } + + m_poseError = poseRef.relativeTo(currentPose); + + if (Math.abs(linearVelocityRef) < 1e-4) { + m_A.set(State.kY.value, State.kHeading.value, 1e-4); + } else { + m_A.set(State.kY.value, State.kHeading.value, linearVelocityRef); + } + var e = + new MatBuilder<>(Nat.N3(), Nat.N1()) + .fill(m_poseError.getX(), m_poseError.getY(), m_poseError.getRotation().getRadians()); + var u = new LinearQuadraticRegulator(m_A, m_B, m_Q, m_R, m_dt).getK().times(e); + + return new ChassisSpeeds( + linearVelocityRef + u.get(0, 0), 0.0, angularVelocityRef + u.get(1, 0)); + } + + /** + * Returns the next output of the LTV controller. + * + *

The reference pose, linear velocity, and angular velocity should come from a drivetrain + * trajectory. + * + * @param currentPose The current pose. + * @param desiredState The desired pose, linear velocity, and angular velocity from a trajectory. + * @return The linear and angular velocity outputs of the LTV controller. + */ + public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) { + return calculate( + currentPose, + desiredState.poseMeters, + desiredState.velocityMetersPerSecond, + desiredState.velocityMetersPerSecond * desiredState.curvatureRadPerMeter); + } + + /** + * Enables and disables the controller for troubleshooting purposes. + * + * @param enabled If the controller is enabled or not. + */ + public void setEnabled(boolean enabled) { + m_enabled = enabled; + } +} diff --git a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp new file mode 100644 index 0000000000..b596523bcf --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp @@ -0,0 +1,152 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/controller/LTVDifferentialDriveController.h" + +#include + +#include "frc/MathUtil.h" +#include "frc/StateSpaceUtil.h" +#include "frc/controller/LinearQuadraticRegulator.h" + +using namespace frc; + +/** + * States of the drivetrain system. + */ +class State { + public: + /// X position in global coordinate frame. + static constexpr int kX = 0; + + /// Y position in global coordinate frame. + static constexpr int kY = 1; + + /// Heading in global coordinate frame. + static constexpr int kHeading = 2; + + /// Left encoder velocity. + static constexpr int kLeftVelocity = 3; + + /// Right encoder velocity. + static constexpr int kRightVelocity = 4; +}; + +LTVDifferentialDriveController::LTVDifferentialDriveController( + const frc::LinearSystem<2, 2, 2>& plant, units::meter_t trackwidth, + const wpi::array& Qelems, const wpi::array& Relems, + units::second_t dt) + : m_trackwidth{trackwidth} { + Matrixd<5, 5> A{ + {0.0, 0.0, 0.0, 0.5, 0.5}, + {0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, -1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()}, + {0.0, 0.0, 0.0, plant.A(0, 0), plant.A(0, 1)}, + {0.0, 0.0, 0.0, plant.A(1, 0), plant.A(1, 1)}}; + Matrixd<5, 2> B{{0.0, 0.0}, + {0.0, 0.0}, + {0.0, 0.0}, + {plant.B(0, 0), plant.B(0, 1)}, + {plant.B(1, 0), plant.B(1, 1)}}; + Matrixd<5, 5> Q = frc::MakeCostMatrix(Qelems); + Matrixd<2, 2> R = frc::MakeCostMatrix(Relems); + + // dx/dt = Ax + Bu + // 0 = Ax + Bu + // Ax = -Bu + // x = -A⁻¹Bu + units::meters_per_second_t maxV{ + -plant.A().householderQr().solve(plant.B() * Vectord<2>{12.0, 12.0})(0)}; + + Vectord<5> x = Vectord<5>::Zero(); + for (auto velocity = -maxV; velocity < maxV; velocity += 0.01_mps) { + x(State::kLeftVelocity) = velocity.value(); + x(State::kRightVelocity) = velocity.value(); + + // The DARE is ill-conditioned if the velocity is close to zero, so don't + // let the system stop. + if (units::math::abs(velocity) < 1e-4_mps) { + m_table.insert(velocity, Matrixd<2, 5>::Zero()); + } else { + A(State::kY, State::kHeading) = velocity.value(); + m_table.insert(velocity, + frc::LinearQuadraticRegulator<5, 2>{A, B, Q, R, dt}.K()); + } + } +} + +bool LTVDifferentialDriveController::AtReference() const { + return std::abs(m_error(0)) < m_tolerance(0) && + std::abs(m_error(1)) < m_tolerance(1) && + std::abs(m_error(2)) < m_tolerance(2) && + std::abs(m_error(3)) < m_tolerance(3) && + std::abs(m_error(4)) < m_tolerance(4); +} + +void LTVDifferentialDriveController::SetTolerance( + const Pose2d& poseTolerance, + units::meters_per_second_t leftVelocityTolerance, + units::meters_per_second_t rightVelocityTolerance) { + m_tolerance = + Vectord<5>{poseTolerance.X().value(), poseTolerance.Y().value(), + poseTolerance.Rotation().Radians().value(), + leftVelocityTolerance.value(), rightVelocityTolerance.value()}; +} + +LTVDifferentialDriveController::WheelVoltages +LTVDifferentialDriveController::Calculate( + const Pose2d& currentPose, units::meters_per_second_t leftVelocity, + units::meters_per_second_t rightVelocity, const Pose2d& poseRef, + units::meters_per_second_t leftVelocityRef, + units::meters_per_second_t rightVelocityRef) { + // This implements the linear time-varying differential drive controller in + // theorem 9.6.3 of https://tavsys.net/controls-in-frc. + Vectord<5> x{currentPose.X().value(), currentPose.Y().value(), + currentPose.Rotation().Radians().value(), leftVelocity.value(), + rightVelocity.value()}; + + Matrixd<5, 5> inRobotFrame = Matrixd<5, 5>::Identity(); + inRobotFrame(0, 0) = std::cos(x(State::kHeading)); + inRobotFrame(0, 1) = std::sin(x(State::kHeading)); + inRobotFrame(1, 0) = -std::sin(x(State::kHeading)); + inRobotFrame(1, 1) = std::cos(x(State::kHeading)); + + Vectord<5> r{poseRef.X().value(), poseRef.Y().value(), + poseRef.Rotation().Radians().value(), leftVelocityRef.value(), + rightVelocityRef.value()}; + m_error = r - x; + m_error(State::kHeading) = + frc::AngleModulus(units::radian_t{m_error(State::kHeading)}).value(); + + units::meters_per_second_t velocity{(leftVelocity + rightVelocity) / 2.0}; + const auto& K = m_table[velocity]; + + Vectord<2> u = K * inRobotFrame * m_error; + + return WheelVoltages{units::volt_t{u(0)}, units::volt_t{u(1)}}; +} + +LTVDifferentialDriveController::WheelVoltages +LTVDifferentialDriveController::Calculate( + const Pose2d& currentPose, units::meters_per_second_t leftVelocity, + units::meters_per_second_t rightVelocity, + const Trajectory::State& desiredState) { + // v = (v_r + v_l) / 2 (1) + // w = (v_r - v_l) / (2r) (2) + // k = w / v (3) + // + // v_l = v - wr + // v_l = v - (vk)r + // v_l = v(1 - kr) + // + // v_r = v + wr + // v_r = v + (vk)r + // v_r = v(1 + kr) + return Calculate( + currentPose, leftVelocity, rightVelocity, desiredState.pose, + desiredState.velocity * + (1 - (desiredState.curvature / 1_rad * m_trackwidth / 2.0)), + desiredState.velocity * + (1 + (desiredState.curvature / 1_rad * m_trackwidth / 2.0))); +} diff --git a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp new file mode 100644 index 0000000000..59a11fa32a --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp @@ -0,0 +1,95 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/controller/LTVUnicycleController.h" + +#include "frc/StateSpaceUtil.h" +#include "frc/controller/LinearQuadraticRegulator.h" +#include "units/math.h" + +using namespace frc; + +/** + * States of the drivetrain system. + */ +class State { + public: + /// X position in global coordinate frame. + static constexpr int kX = 0; + + /// Y position in global coordinate frame. + static constexpr int kY = 1; + + /// Heading in global coordinate frame. + static constexpr int kHeading = 2; +}; + +/** + * Inputs of the drivetrain system. + */ +class Input { + public: + /// Linear velocity. + static constexpr int kLinearVelocity = 3; + + /// Angular velocity. + static constexpr int kAngularVelocity = 4; +}; + +LTVUnicycleController::LTVUnicycleController( + const wpi::array& Qelems, const wpi::array& Relems, + units::second_t dt) + : m_dt{dt} { + m_Q = frc::MakeCostMatrix(Qelems); + m_R = frc::MakeCostMatrix(Relems); +} + +bool LTVUnicycleController::AtReference() const { + const auto& eTranslate = m_poseError.Translation(); + const auto& eRotate = m_poseError.Rotation(); + const auto& tolTranslate = m_poseTolerance.Translation(); + const auto& tolRotate = m_poseTolerance.Rotation(); + return units::math::abs(eTranslate.X()) < tolTranslate.X() && + units::math::abs(eTranslate.Y()) < tolTranslate.Y() && + units::math::abs(eRotate.Radians()) < tolRotate.Radians(); +} + +void LTVUnicycleController::SetTolerance(const Pose2d& poseTolerance) { + m_poseTolerance = poseTolerance; +} + +ChassisSpeeds LTVUnicycleController::Calculate( + const Pose2d& currentPose, const Pose2d& poseRef, + units::meters_per_second_t linearVelocityRef, + units::radians_per_second_t angularVelocityRef) { + if (!m_enabled) { + return ChassisSpeeds{linearVelocityRef, 0_mps, angularVelocityRef}; + } + + m_poseError = poseRef.RelativeTo(currentPose); + + if (units::math::abs(linearVelocityRef) < 1e-4_mps) { + m_A(State::kY, State::kHeading) = 1e-4; + } else { + m_A(State::kY, State::kHeading) = linearVelocityRef.value(); + } + Vectord<3> e{m_poseError.X().value(), m_poseError.Y().value(), + m_poseError.Rotation().Radians().value()}; + Vectord<2> u = + frc::LinearQuadraticRegulator<3, 2>{m_A, m_B, m_Q, m_R, m_dt}.K() * e; + + return ChassisSpeeds{linearVelocityRef + units::meters_per_second_t{u(0)}, + 0_mps, + angularVelocityRef + units::radians_per_second_t{u(1)}}; +} + +ChassisSpeeds LTVUnicycleController::Calculate( + const Pose2d& currentPose, const Trajectory::State& desiredState) { + return Calculate(currentPose, desiredState.pose, desiredState.velocity, + desiredState.velocity * desiredState.curvature); +} + +void LTVUnicycleController::SetEnabled(bool enabled) { + m_enabled = enabled; +} diff --git a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h new file mode 100644 index 0000000000..c97c66ae8d --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h @@ -0,0 +1,132 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include +#include + +#include "frc/EigenCore.h" +#include "frc/geometry/Pose2d.h" +#include "frc/system/LinearSystem.h" +#include "frc/trajectory/Trajectory.h" +#include "units/length.h" +#include "units/time.h" +#include "units/velocity.h" +#include "units/voltage.h" + +namespace frc { + +/** + * The linear time-varying differential drive controller has a similar form to + * the LQR, but the model used to compute the controller gain is the nonlinear + * model linearized around the drivetrain's current state. We precomputed gains + * for important places in our state-space, then interpolated between them with + * a LUT to save computational resources. + * + * See section 8.7 in Controls Engineering in FRC for a derivation of the + * control law we used shown in theorem 8.7.4. + */ +class WPILIB_DLLEXPORT LTVDifferentialDriveController { + public: + /** + * Motor voltages for a differential drive. + */ + struct WheelVoltages { + units::volt_t left = 0_V; + units::volt_t right = 0_V; + }; + + /** + * Constructs a linear time-varying differential drive controller. + * + * @param plant The drivetrain velocity plant. + * @param trackwidth The drivetrain's trackwidth. + * @param Qelems The maximum desired error tolerance for each state. + * @param Relems The maximum desired control effort for each input. + * @param dt Discretization timestep. + */ + LTVDifferentialDriveController(const frc::LinearSystem<2, 2, 2>& plant, + units::meter_t trackwidth, + const wpi::array& Qelems, + const wpi::array& Relems, + units::second_t dt); + + /** + * Move constructor. + */ + LTVDifferentialDriveController(LTVDifferentialDriveController&&) = default; + + /** + * Move assignment operator. + */ + LTVDifferentialDriveController& operator=(LTVDifferentialDriveController&&) = + default; + + /** + * Returns true if the pose error is within tolerance of the reference. + */ + bool AtReference() const; + + /** + * Sets the pose error which is considered tolerable for use with + * AtReference(). + * + * @param poseTolerance Pose error which is tolerable. + * @param leftVelocityTolerance Left velocity error which is tolerable. + * @param rightVelocityTolerance Right velocity error which is tolerable. + */ + void SetTolerance(const Pose2d& poseTolerance, + units::meters_per_second_t leftVelocityTolerance, + units::meters_per_second_t rightVelocityTolerance); + + /** + * Returns the left and right output voltages of the LTV controller. + * + * The reference pose, linear velocity, and angular velocity should come from + * a drivetrain trajectory. + * + * @param currentPose The current pose. + * @param leftVelocity The current left velocity. + * @param rightVelocity The current right velocity. + * @param poseRef The desired pose. + * @param leftVelocityRef The desired left velocity. + * @param rightVelocityRef The desired right velocity. + */ + WheelVoltages Calculate(const Pose2d& currentPose, + units::meters_per_second_t leftVelocity, + units::meters_per_second_t rightVelocity, + const Pose2d& poseRef, + units::meters_per_second_t leftVelocityRef, + units::meters_per_second_t rightVelocityRef); + + /** + * Returns the left and right output voltages of the LTV controller. + * + * The reference pose, linear velocity, and angular velocity should come from + * a drivetrain trajectory. + * + * @param currentPose The current pose. + * @param leftVelocity The left velocity. + * @param rightVelocity The right velocity. + * @param desiredState The desired pose, linear velocity, and angular velocity + * from a trajectory. + */ + WheelVoltages Calculate(const Pose2d& currentPose, + units::meters_per_second_t leftVelocity, + units::meters_per_second_t rightVelocity, + const Trajectory::State& desiredState); + + private: + units::meter_t m_trackwidth; + + // LUT from drivetrain linear velocity to LQR gain + wpi::interpolating_map> m_table; + + Vectord<5> m_error; + Vectord<5> m_tolerance; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h new file mode 100644 index 0000000000..80b8d05574 --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h @@ -0,0 +1,111 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "frc/EigenCore.h" +#include "frc/geometry/Pose2d.h" +#include "frc/kinematics/DifferentialDriveKinematics.h" +#include "frc/trajectory/Trajectory.h" +#include "units/angular_velocity.h" +#include "units/time.h" +#include "units/velocity.h" + +namespace frc { + +/** + * The linear time-varying unicycle controller has a similar form to the LQR, + * but the model used to compute the controller gain is the nonlinear model + * linearized around the drivetrain's current state. + * + * See section 8.9 in Controls Engineering in FRC for a derivation of the + * control law we used shown in theorem 8.9.1. + */ +class WPILIB_DLLEXPORT LTVUnicycleController { + public: + /** + * Constructs a linear time-varying unicycle controller. + * + * @param Qelems The maximum desired error tolerance for each state. + * @param Relems The maximum desired control effort for each input. + * @param dt Discretization timestep. + */ + LTVUnicycleController(const wpi::array& Qelems, + const wpi::array& Relems, + units::second_t dt); + + /** + * Move constructor. + */ + LTVUnicycleController(LTVUnicycleController&&) = default; + + /** + * Move assignment operator. + */ + LTVUnicycleController& operator=(LTVUnicycleController&&) = default; + + /** + * Returns true if the pose error is within tolerance of the reference. + */ + bool AtReference() const; + + /** + * Sets the pose error which is considered tolerable for use with + * AtReference(). + * + * @param poseTolerance Pose error which is tolerable. + */ + void SetTolerance(const Pose2d& poseTolerance); + + /** + * Returns the linear and angular velocity outputs of the LTV controller. + * + * The reference pose, linear velocity, and angular velocity should come from + * a drivetrain trajectory. + * + * @param currentPose The current pose. + * @param poseRef The desired pose. + * @param linearVelocityRef The desired linear velocity. + * @param angularVelocityRef The desired angular velocity. + */ + ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef, + units::meters_per_second_t linearVelocityRef, + units::radians_per_second_t angularVelocityRef); + + /** + * Returns the linear and angular velocity outputs of the LTV controller. + * + * The reference pose, linear velocity, and angular velocity should come from + * a drivetrain trajectory. + * + * @param currentPose The current pose. + * @param desiredState The desired pose, linear velocity, and angular velocity + * from a trajectory. + */ + ChassisSpeeds Calculate(const Pose2d& currentPose, + const Trajectory::State& desiredState); + + /** + * Enables and disables the controller for troubleshooting purposes. + * + * @param enabled If the controller is enabled or not. + */ + void SetEnabled(bool enabled); + + private: + Matrixd<3, 3> m_A = Matrixd<3, 3>::Zero(); + Matrixd<3, 2> m_B{{1.0, 0.0}, {0.0, 0.0}, {0.0, 1.0}}; + Matrixd<3, 3> m_Q; + Matrixd<2, 2> m_R; + units::second_t m_dt; + + Pose2d m_poseError; + Pose2d m_poseTolerance; + bool m_enabled = true; +}; + +} // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java new file mode 100644 index 0000000000..efb9c5ead8 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java @@ -0,0 +1,128 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.controller; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.numbers.N5; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.NumericalIntegration; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import java.util.ArrayList; +import org.junit.jupiter.api.Test; + +class LTVDifferentialDriveControllerTest { + private static final double kTolerance = 1 / 12.0; + private static final double kAngularTolerance = Math.toRadians(2); + + /** States of the drivetrain system. */ + static class State { + /// X position in global coordinate frame. + public static final int kX = 0; + + /// Y position in global coordinate frame. + public static final int kY = 1; + + /// Heading in global coordinate frame. + public static final int kHeading = 2; + + /// Left encoder velocity. + public static final int kLeftVelocity = 3; + + /// Right encoder velocity. + public static final int kRightVelocity = 4; + } + + private static final double kLinearV = 3.02; // V/(m/s) + private static final double kLinearA = 0.642; // V/(m/s²) + private static final double kAngularV = 1.382; // V/(m/s) + private static final double kAngularA = 0.08495; // V/(m/s²) + private static final LinearSystem plant = + LinearSystemId.identifyDrivetrainSystem(kLinearV, kLinearA, kAngularV, kAngularA); + private static final double kTrackwidth = 0.9; + + private static Matrix dynamics(Matrix x, Matrix u) { + double v = (x.get(State.kLeftVelocity, 0) + x.get(State.kRightVelocity, 0)) / 2.0; + + var xdot = new Matrix<>(Nat.N5(), Nat.N1()); + xdot.set(0, 0, v * Math.cos(x.get(State.kHeading, 0))); + xdot.set(1, 0, v * Math.sin(x.get(State.kHeading, 0))); + xdot.set(2, 0, (x.get(State.kRightVelocity, 0) - x.get(State.kLeftVelocity, 0)) / kTrackwidth); + xdot.assignBlock( + 3, 0, plant.getA().times(x.block(Nat.N2(), Nat.N1(), 3, 0)).plus(plant.getB().times(u))); + return xdot; + } + + @Test + void testReachesReference() { + final double kDt = 0.02; + + final var controller = + new LTVDifferentialDriveController( + plant, + kTrackwidth, + VecBuilder.fill(0.0625, 0.125, 2.5, 0.95, 0.95), + VecBuilder.fill(12.0, 12.0), + kDt); + var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0)); + + final var waypoints = new ArrayList(); + waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0))); + waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846))); + var config = new TrajectoryConfig(8.8, 0.1); + final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config); + + var x = + new MatBuilder<>(Nat.N5(), Nat.N1()) + .fill( + robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), 0.0, 0.0); + + final var totalTime = trajectory.getTotalTimeSeconds(); + for (int i = 0; i < (totalTime / kDt); ++i) { + var state = trajectory.sample(kDt * i); + robotPose = + new Pose2d( + x.get(State.kX, 0), x.get(State.kY, 0), new Rotation2d(x.get(State.kHeading, 0))); + final var output = + controller.calculate( + robotPose, x.get(State.kLeftVelocity, 0), x.get(State.kRightVelocity, 0), state); + + x = + NumericalIntegration.rkdp( + LTVDifferentialDriveControllerTest::dynamics, + x, + new MatBuilder<>(Nat.N2(), Nat.N1()).fill(output.left, output.right), + kDt); + } + + final var states = trajectory.getStates(); + final var endPose = states.get(states.size() - 1).poseMeters; + + // Java lambdas require local variables referenced from a lambda expression + // must be final or effectively final. + final var finalRobotPose = robotPose; + assertAll( + () -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance), + () -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance), + () -> + assertEquals( + 0.0, + MathUtil.angleModulus( + endPose.getRotation().getRadians() - finalRobotPose.getRotation().getRadians()), + kAngularTolerance)); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVUnicycleControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVUnicycleControllerTest.java new file mode 100644 index 0000000000..463e1ce754 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVUnicycleControllerTest.java @@ -0,0 +1,65 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.controller; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import java.util.ArrayList; +import org.junit.jupiter.api.Test; + +class LTVUnicycleControllerTest { + private static final double kTolerance = 1 / 12.0; + private static final double kAngularTolerance = Math.toRadians(2); + + @Test + void testReachesReference() { + final double kDt = 0.02; + + final var controller = + new LTVUnicycleController( + VecBuilder.fill(0.0625, 0.125, 2.5), VecBuilder.fill(4.0, 4.0), kDt); + var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0)); + + final var waypoints = new ArrayList(); + waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0))); + waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846))); + var config = new TrajectoryConfig(8.8, 0.1); + final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config); + + final var totalTime = trajectory.getTotalTimeSeconds(); + for (int i = 0; i < (totalTime / kDt); ++i) { + var state = trajectory.sample(kDt * i); + + var output = controller.calculate(robotPose, state); + robotPose = + robotPose.exp( + new Twist2d(output.vxMetersPerSecond * kDt, 0, output.omegaRadiansPerSecond * kDt)); + } + + final var states = trajectory.getStates(); + final var endPose = states.get(states.size() - 1).poseMeters; + + // Java lambdas require local variables referenced from a lambda expression + // must be final or effectively final. + final var finalRobotPose = robotPose; + assertAll( + () -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance), + () -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance), + () -> + assertEquals( + 0.0, + MathUtil.angleModulus( + endPose.getRotation().getRadians() - finalRobotPose.getRotation().getRadians()), + kAngularTolerance)); + } +} diff --git a/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp new file mode 100644 index 0000000000..d82e256fd8 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp @@ -0,0 +1,101 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include "frc/MathUtil.h" +#include "frc/controller/LTVDifferentialDriveController.h" +#include "frc/system/NumericalIntegration.h" +#include "frc/system/plant/LinearSystemId.h" +#include "frc/trajectory/TrajectoryGenerator.h" +#include "gtest/gtest.h" +#include "units/math.h" + +#define EXPECT_NEAR_UNITS(val1, val2, eps) \ + EXPECT_LE(units::math::abs(val1 - val2), eps) + +static constexpr units::meter_t kTolerance{1 / 12.0}; +static constexpr units::radian_t kAngularTolerance{2.0 * wpi::numbers::pi / + 180.0}; + +/** + * States of the drivetrain system. + */ +class State { + public: + /// X position in global coordinate frame. + static constexpr int kX = 0; + + /// Y position in global coordinate frame. + static constexpr int kY = 1; + + /// Heading in global coordinate frame. + static constexpr int kHeading = 2; + + /// Left encoder velocity. + static constexpr int kLeftVelocity = 3; + + /// Right encoder velocity. + static constexpr int kRightVelocity = 4; +}; + +static constexpr auto kLinearV = 3.02_V / 1_mps; +static constexpr auto kLinearA = 0.642_V / 1_mps_sq; +static constexpr auto kAngularV = 1.382_V / 1_mps; +static constexpr auto kAngularA = 0.08495_V / 1_mps_sq; +static auto plant = frc::LinearSystemId::IdentifyDrivetrainSystem( + kLinearV, kLinearA, kAngularV, kAngularA); +static constexpr auto kTrackwidth = 0.9_m; + +frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) { + double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0; + + frc::Vectord<5> xdot; + xdot(0) = v * std::cos(x(State::kHeading)); + xdot(1) = v * std::sin(x(State::kHeading)); + xdot(2) = ((x(State::kRightVelocity) - x(State::kLeftVelocity)) / kTrackwidth) + .value(); + xdot.block<2, 1>(3, 0) = plant.A() * x.block<2, 1>(3, 0) + plant.B() * u; + return xdot; +} + +TEST(LTVDifferentialDriveControllerTest, ReachesReference) { + constexpr auto kDt = 0.02_s; + + frc::LTVDifferentialDriveController controller{ + plant, kTrackwidth, {0.0625, 0.125, 2.5, 0.95, 0.95}, {12.0, 12.0}, kDt}; + frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}}; + + auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad}, + frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}}; + auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory( + waypoints, {8.8_mps, 0.1_mps_sq}); + + frc::Vectord<5> x = frc::Vectord<5>::Zero(); + x(State::kX) = robotPose.X().value(); + x(State::kY) = robotPose.Y().value(); + x(State::kHeading) = robotPose.Rotation().Radians().value(); + + auto totalTime = trajectory.TotalTime(); + for (size_t i = 0; i < (totalTime / kDt).value(); ++i) { + auto state = trajectory.Sample(kDt * i); + robotPose = + frc::Pose2d{units::meter_t{x(State::kX)}, units::meter_t{x(State::kY)}, + units::radian_t{x(State::kHeading)}}; + auto [leftVoltage, rightVoltage] = controller.Calculate( + robotPose, units::meters_per_second_t{x(State::kLeftVelocity)}, + units::meters_per_second_t{x(State::kRightVelocity)}, state); + + x = frc::RKDP(&Dynamics, x, + frc::Vectord<2>{leftVoltage.value(), rightVoltage.value()}, + kDt); + } + + auto& endPose = trajectory.States().back().pose; + EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance); + EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance); + EXPECT_NEAR_UNITS(frc::AngleModulus(endPose.Rotation().Radians() - + robotPose.Rotation().Radians()), + 0_rad, kAngularTolerance); +} diff --git a/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp b/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp new file mode 100644 index 0000000000..2b5e6145a6 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp @@ -0,0 +1,44 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/MathUtil.h" +#include "frc/controller/LTVUnicycleController.h" +#include "frc/trajectory/TrajectoryGenerator.h" +#include "gtest/gtest.h" +#include "units/math.h" + +#define EXPECT_NEAR_UNITS(val1, val2, eps) \ + EXPECT_LE(units::math::abs(val1 - val2), eps) + +static constexpr units::meter_t kTolerance{1 / 12.0}; +static constexpr units::radian_t kAngularTolerance{2.0 * wpi::numbers::pi / + 180.0}; + +TEST(LTVUnicycleControllerTest, ReachesReference) { + constexpr auto kDt = 0.02_s; + + frc::LTVUnicycleController controller{{0.0625, 0.125, 2.5}, {4.0, 4.0}, kDt}; + frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}}; + + auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad}, + frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}}; + auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory( + waypoints, {8.8_mps, 0.1_mps_sq}); + + auto totalTime = trajectory.TotalTime(); + for (size_t i = 0; i < (totalTime / kDt).value(); ++i) { + auto state = trajectory.Sample(kDt * i); + auto [vx, vy, omega] = controller.Calculate(robotPose, state); + static_cast(vy); + + robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, 0_m, omega * kDt}); + } + + auto& endPose = trajectory.States().back().pose; + EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance); + EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance); + EXPECT_NEAR_UNITS(frc::AngleModulus(endPose.Rotation().Radians() - + robotPose.Rotation().Radians()), + 0_rad, kAngularTolerance); +}