From ecb7cfa9ef890b0ff2a1a16d8ca8bf270369d304 Mon Sep 17 00:00:00 2001 From: Jordan McMichael Date: Thu, 19 Oct 2023 20:26:32 -0400 Subject: [PATCH] [wpimath] Add Exponential motion profile (#5720) --- shared/examplecheck.gradle | 4 +- .../ElevatorExponentialProfile/cpp/Robot.cpp | 68 +++ .../include/ExampleSmartMotorController.h | 82 ++++ .../cpp/Robot.cpp | 44 ++ .../cpp/subsystems/Elevator.cpp | 64 +++ .../include/Constants.h | 57 +++ .../include/Robot.h | 27 ++ .../include/subsystems/Elevator.h | 74 +++ .../src/main/cpp/examples/examples.json | 29 ++ .../ExampleSmartMotorController.java | 96 ++++ .../elevatorexponentialprofile/Main.java | 25 + .../elevatorexponentialprofile/Robot.java | 53 +++ .../Constants.java | 39 ++ .../elevatorexponentialsimulation/Main.java | 25 + .../elevatorexponentialsimulation/Robot.java | 62 +++ .../subsystems/Elevator.java | 142 ++++++ .../wpi/first/wpilibj/examples/examples.json | 32 ++ wpimath/algorithms.md | 46 ++ wpimath/algorithms/ExponentialProfileModel.py | 98 ++++ .../math/trajectory/ExponentialProfile.java | 449 ++++++++++++++++++ .../frc/trajectory/ExponentialProfile.h | 194 ++++++++ .../frc/trajectory/ExponentialProfile.inc | 253 ++++++++++ .../trajectory/ExponentialProfileTest.java | 364 ++++++++++++++ .../cpp/trajectory/ExponentialProfileTest.cpp | 338 +++++++++++++ 24 files changed, 2663 insertions(+), 2 deletions(-) create mode 100644 wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/cpp/Robot.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/include/ExampleSmartMotorController.h create mode 100644 wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/cpp/Robot.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/cpp/subsystems/Elevator.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/include/Constants.h create mode 100644 wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/include/Robot.h create mode 100644 wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/include/subsystems/Elevator.h create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/ExampleSmartMotorController.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Main.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Constants.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Main.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Robot.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java create mode 100644 wpimath/algorithms/ExponentialProfileModel.py create mode 100644 wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java create mode 100644 wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h create mode 100644 wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.inc create mode 100644 wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java create mode 100644 wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp diff --git a/shared/examplecheck.gradle b/shared/examplecheck.gradle index 4635d05057..fe43349a45 100644 --- a/shared/examplecheck.gradle +++ b/shared/examplecheck.gradle @@ -67,8 +67,8 @@ def tagList = [ "SmartDashboard", "Shuffleboard", "Sendable", "DataLog", /* --- Controls --- */ - "PID", "State-Space", "Ramsete", "Path Following", "Trajectory", "SysId", - "Simulation", "Trapezoid Profile", "Profiled PID", "Odometry", "LQR", + "Exponential Profile", "PID", "State-Space", "Ramsete", "Path Following", "Trajectory", + "SysId", "Simulation", "Trapezoid Profile", "Profiled PID", "Odometry", "LQR", "Pose Estimator", /* --- Hardware --- */ diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/cpp/Robot.cpp new file mode 100644 index 0000000000..5d65a5aa36 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/cpp/Robot.cpp @@ -0,0 +1,68 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ExampleSmartMotorController.h" + +class Robot : public frc::TimedRobot { + public: + static constexpr units::second_t kDt = 20_ms; + + Robot() { + // Note: These gains are fake, and will have to be tuned for your robot. + m_motor.SetPID(1.3, 0.0, 0.7); + } + + void TeleopPeriodic() override { + if (m_joystick.GetRawButtonPressed(2)) { + m_goal = {5_m, 0_mps}; + } else if (m_joystick.GetRawButtonPressed(3)) { + m_goal = {0_m, 0_mps}; + } + + // Retrieve the profiled setpoint for the next timestep. This setpoint moves + // toward the goal while obeying the constraints. + auto next = m_profile.Calculate(kDt, m_goal, m_setpoint); + + // Send setpoint to offboard controller PID + m_motor.SetSetpoint( + ExampleSmartMotorController::PIDMode::kPosition, + m_setpoint.position.value(), + m_feedforward.Calculate(m_setpoint.velocity, next.velocity, kDt) / + 12_V); + + m_setpoint = next; + } + + private: + frc::Joystick m_joystick{1}; + ExampleSmartMotorController m_motor{1}; + frc::SimpleMotorFeedforward m_feedforward{ + // Note: These gains are fake, and will have to be tuned for your robot. + 1_V, 1_V / 1_mps, 1_V / 1_mps_sq}; + + // Create a motion profile with the given maximum velocity and maximum + // acceleration constraints for the next setpoint. + frc::ExponentialProfile m_profile{ + {10_V, 1_V / 1_mps, 1_V / 1_mps_sq}}; + frc::ExponentialProfile::State m_goal; + frc::ExponentialProfile::State m_setpoint; +}; + +#ifndef RUNNING_FRC_TESTS +int main() { + return frc::StartRobot(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/include/ExampleSmartMotorController.h b/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/include/ExampleSmartMotorController.h new file mode 100644 index 0000000000..5d55839892 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/include/ExampleSmartMotorController.h @@ -0,0 +1,82 @@ +// 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 + +/** + * A simplified stub class that simulates the API of a common "smart" motor + * controller. + * + *

Has no actual functionality. + */ +class ExampleSmartMotorController : public frc::MotorController { + public: + enum PIDMode { kPosition, kVelocity, kMovementWitchcraft }; + + /** + * Creates a new ExampleSmartMotorController. + * + * @param port The port for the controller. + */ + explicit ExampleSmartMotorController(int port) {} + + /** + * Example method for setting the PID gains of the smart controller. + * + * @param kp The proportional gain. + * @param ki The integral gain. + * @param kd The derivative gain. + */ + void SetPID(double kp, double ki, double kd) {} + + /** + * Example method for setting the setpoint of the smart controller in PID + * mode. + * + * @param mode The mode of the PID controller. + * @param setpoint The controller setpoint. + * @param arbFeedforward An arbitrary feedforward output (from -1 to 1). + */ + void SetSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {} + + /** + * Places this motor controller in follower mode. + * + * @param leader The leader to follow. + */ + void Follow(ExampleSmartMotorController leader) {} + + /** + * Returns the encoder distance. + * + * @return The current encoder distance. + */ + double GetEncoderDistance() { return 0; } + + /** + * Returns the encoder rate. + * + * @return The current encoder rate. + */ + double GetEncoderRate() { return 0; } + + /** + * Resets the encoder to zero distance. + */ + void ResetEncoder() {} + + void Set(double speed) override {} + + double Get() const override { return 0; } + + void SetInverted(bool isInverted) override {} + + bool GetInverted() const override { return false; } + + void Disable() override {} + + void StopMotor() override {} +}; diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/cpp/Robot.cpp new file mode 100644 index 0000000000..a6cb55d2d0 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/cpp/Robot.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 "Robot.h" + +#include "Constants.h" + +void Robot::RobotPeriodic() { + // Update the telemetry, including mechanism visualization, regardless of + // mode. + m_elevator.UpdateTelemetry(); +} + +void Robot::SimulationPeriodic() { + // Update the simulation model. + m_elevator.SimulationPeriodic(); +} + +void Robot::TeleopInit() { + // This just makes sure that our simulation code knows that the motor's off. + m_elevator.Reset(); +} + +void Robot::TeleopPeriodic() { + if (m_joystick.GetTrigger()) { + // Here, we set the constant setpoint of 0.75 meters. + m_elevator.ReachGoal(Constants::kSetpoint); + } else { + // Otherwise, we update the setpoint to 0. + m_elevator.ReachGoal(Constants::kLowerSetpoint); + } +} + +void Robot::DisabledInit() { + // This just makes sure that our simulation code knows that the motor's off. + m_elevator.Stop(); +} + +#ifndef RUNNING_FRC_TESTS +int main() { + return frc::StartRobot(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/cpp/subsystems/Elevator.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/cpp/subsystems/Elevator.cpp new file mode 100644 index 0000000000..27832a6df5 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/cpp/subsystems/Elevator.cpp @@ -0,0 +1,64 @@ +// 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 "subsystems/Elevator.h" + +#include +#include +#include + +Elevator::Elevator() { + m_encoder.SetDistancePerPulse(Constants::kArmEncoderDistPerPulse); + + // Put Mechanism 2d to SmartDashboard + // To view the Elevator visualization, select Network Tables -> SmartDashboard + // -> Elevator Sim + frc::SmartDashboard::PutData("Elevator Sim", &m_mech2d); +} + +void Elevator::SimulationPeriodic() { + // In this method, we update our simulation of what our elevator is doing + // First, we set our "inputs" (voltages) + m_elevatorSim.SetInput(frc::Vectord<1>{ + m_motorSim.GetSpeed() * frc::RobotController::GetInputVoltage()}); + + // Next, we update it. The standard loop time is 20ms. + m_elevatorSim.Update(20_ms); + + // Finally, we set our simulated encoder's readings and simulated battery + // voltage + m_encoderSim.SetDistance(m_elevatorSim.GetPosition().value()); + // SimBattery estimates loaded battery voltages + frc::sim::RoboRioSim::SetVInVoltage( + frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()})); +} + +void Elevator::UpdateTelemetry() { + // Update the Elevator length based on the simulated elevator height + m_elevatorMech2d->SetLength(m_encoder.GetDistance()); +} + +void Elevator::ReachGoal(units::meter_t goal) { + frc::ExponentialProfile::State goalState{goal, + 0_mps}; + + auto next = m_profile.Calculate(20_ms, m_setpoint, goalState); + + auto pidOutput = m_controller.Calculate(m_encoder.GetDistance(), + m_setpoint.position / 1_m); + auto feedforwardOutput = + m_feedforward.Calculate(m_setpoint.velocity, next.velocity, 20_ms); + + m_motor.SetVoltage(units::volt_t{pidOutput} + feedforwardOutput); + + m_setpoint = next; +} + +void Elevator::Reset() { + m_setpoint = {m_encoder.GetDistance() * 1_m, 0_mps}; +} + +void Elevator::Stop() { + m_motor.Set(0.0); +} diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/include/Constants.h new file mode 100644 index 0000000000..7c53018fe9 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/include/Constants.h @@ -0,0 +1,57 @@ +// 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 +#include +#include +#include +#include + +/** + * The Constants header provides a convenient place for teams to hold robot-wide + * numerical or bool constants. This should not be used for any other purpose. + * + * It is generally a good idea to place constants into subsystem- or + * command-specific namespaces within this header, which can then be used where + * they are needed. + */ + +namespace Constants { + +static constexpr int kMotorPort = 0; +static constexpr int kEncoderAChannel = 0; +static constexpr int kEncoderBChannel = 1; +static constexpr int kJoystickPort = 0; + +static constexpr double kElevatorKp = 0.75; +static constexpr double kElevatorKi = 0.0; +static constexpr double kElevatorKd = 0.0; + +static constexpr units::volt_t kElevatorMaxV = 10_V; +static constexpr units::volt_t kElevatorkS = 0.0_V; +static constexpr units::volt_t kElevatorkG = 0.62_V; +static constexpr auto kElevatorkV = 3.9_V / 1_mps; +static constexpr auto kElevatorkA = 0.06_V / 1_mps_sq; + +static constexpr double kElevatorGearing = 5.0; +static constexpr units::meter_t kElevatorDrumRadius = 1_in; +static constexpr units::kilogram_t kCarriageMass = 12_lb; + +static constexpr units::meter_t kSetpoint = 42.875_in; +static constexpr units::meter_t kLowerSetpoint = 15_in; +static constexpr units::meter_t kMinElevatorHeight = 0_cm; +static constexpr units::meter_t kMaxElevatorHeight = 50_in; + +// distance per pulse = (distance per revolution) / (pulses per revolution) +// = (Pi * D) / ppr +static constexpr double kArmEncoderDistPerPulse = + 2.0 * std::numbers::pi * kElevatorDrumRadius.value() / 4096.0; + +} // namespace Constants diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/include/Robot.h new file mode 100644 index 0000000000..f10310483d --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/include/Robot.h @@ -0,0 +1,27 @@ +// 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 "subsystems/Elevator.h" + +/** + * This is a sample program to demonstrate the use of elevator simulation. + */ +class Robot : public frc::TimedRobot { + public: + void RobotInit() override {} + void RobotPeriodic() override; + void SimulationPeriodic() override; + void TeleopInit() override; + void TeleopPeriodic() override; + void DisabledInit() override; + + private: + frc::Joystick m_joystick{Constants::kJoystickPort}; + Elevator m_elevator; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/include/subsystems/Elevator.h b/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/include/subsystems/Elevator.h new file mode 100644 index 0000000000..afc5267241 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialSimulation/include/subsystems/Elevator.h @@ -0,0 +1,74 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" + +class Elevator { + public: + Elevator(); + void SimulationPeriodic(); + void UpdateTelemetry(); + void ReachGoal(units::meter_t goal); + void Reset(); + void Stop(); + + private: + // This gearbox represents a gearbox containing 4 Vex 775pro motors. + frc::DCMotor m_elevatorGearbox = frc::DCMotor::NEO(2); + + // Standard classes for controlling our elevator + frc::ExponentialProfile::Constraints + m_constraints{Constants::kElevatorMaxV, Constants::kElevatorkV, + Constants::kElevatorkA}; + frc::ExponentialProfile m_profile{m_constraints}; + frc::ExponentialProfile::State m_setpoint; + + frc::PIDController m_controller{ + Constants::kElevatorKp, Constants::kElevatorKi, Constants::kElevatorKd}; + + frc::ElevatorFeedforward m_feedforward{ + Constants::kElevatorkS, Constants::kElevatorkG, Constants::kElevatorkV, + Constants::kElevatorkA}; + frc::Encoder m_encoder{Constants::kEncoderAChannel, + Constants::kEncoderBChannel}; + frc::PWMSparkMax m_motor{Constants::kMotorPort}; + frc::sim::PWMSim m_motorSim{m_motor}; + + // Simulation classes help us simulate what's going on, including gravity. + frc::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox, + Constants::kElevatorGearing, + Constants::kCarriageMass, + Constants::kElevatorDrumRadius, + Constants::kMinElevatorHeight, + Constants::kMaxElevatorHeight, + true, + 0_m, + {0.005}}; + frc::sim::EncoderSim m_encoderSim{m_encoder}; + + // Create a Mechanism2d display of an elevator + frc::Mechanism2d m_mech2d{10_in / 1_m, 51_in / 1_m}; + frc::MechanismRoot2d* m_elevatorRoot = + m_mech2d.GetRoot("Elevator Root", 5_in / 1_m, 0.5_in / 1_m); + frc::MechanismLigament2d* m_elevatorMech2d = + m_elevatorRoot->Append( + "Elevator", m_elevatorSim.GetPosition().value(), 90_deg); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 88a338159d..ea49b3f4fd 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -205,6 +205,20 @@ "gradlebase": "cpp", "commandversion": 2 }, + { + "name": "Elevator with exponential profiled PID", + "description": "Reach elevator position setpoints with exponential profiles and smart motor controller PID.", + "tags": [ + "Basic Robot", + "Elevator", + "Exponential Profile", + "Smart Motor Controller", + "Joystick" + ], + "foldername": "ElevatorExponentialProfile", + "gradlebase": "cpp", + "commandversion": 2 + }, { "name": "Elevator with trapezoid profiled PID", "description": "Reach elevator position setpoints with trapezoid profiles and smart motor controller PID.", @@ -784,6 +798,21 @@ "gradlebase": "cpp", "commandversion": 2 }, + { + "name": "Elevator Exponential Profile Simulation", + "description": "Simulate an elevator.", + "tags": [ + "Basic Robot", + "Elevator", + "State-Space", + "Simulation", + "Mechanism2d", + "Profiled PID" + ], + "foldername": "ElevatorExponentialSimulation", + "gradlebase": "cpp", + "commandversion": 2 + }, { "name": "DifferentialDrivePoseEstimator", "description": "Combine differential-drive odometry with vision data using DifferentialDrivePoseEstimator.", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/ExampleSmartMotorController.java new file mode 100644 index 0000000000..e252f85003 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/ExampleSmartMotorController.java @@ -0,0 +1,96 @@ +// 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.wpilibj.examples.elevatorexponentialprofile; + +import edu.wpi.first.wpilibj.motorcontrol.MotorController; + +/** + * A simplified stub class that simulates the API of a common "smart" motor controller. + * + *

Has no actual functionality. + */ +public class ExampleSmartMotorController implements MotorController { + public enum PIDMode { + kPosition, + kVelocity, + kMovementWitchcraft + } + + /** + * Creates a new ExampleSmartMotorController. + * + * @param port The port for the controller. + */ + @SuppressWarnings("PMD.UnusedFormalParameter") + public ExampleSmartMotorController(int port) {} + + /** + * Example method for setting the PID gains of the smart controller. + * + * @param kp The proportional gain. + * @param ki The integral gain. + * @param kd The derivative gain. + */ + public void setPID(double kp, double ki, double kd) {} + + /** + * Example method for setting the setpoint of the smart controller in PID mode. + * + * @param mode The mode of the PID controller. + * @param setpoint The controller setpoint. + * @param arbFeedforward An arbitrary feedforward output (from -1 to 1). + */ + public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {} + + /** + * Places this motor controller in follower mode. + * + * @param leader The leader to follow. + */ + public void follow(ExampleSmartMotorController leader) {} + + /** + * Returns the encoder distance. + * + * @return The current encoder distance. + */ + public double getEncoderDistance() { + return 0; + } + + /** + * Returns the encoder rate. + * + * @return The current encoder rate. + */ + public double getEncoderRate() { + return 0; + } + + /** Resets the encoder to zero distance. */ + public void resetEncoder() {} + + @Override + public void set(double speed) {} + + @Override + public double get() { + return 0; + } + + @Override + public void setInverted(boolean isInverted) {} + + @Override + public boolean getInverted() { + return false; + } + + @Override + public void disable() {} + + @Override + public void stopMotor() {} +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Main.java new file mode 100644 index 0000000000..7d89388684 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Main.java @@ -0,0 +1,25 @@ +// 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.wpilibj.examples.elevatorexponentialprofile; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java new file mode 100644 index 0000000000..3aed7b92af --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java @@ -0,0 +1,53 @@ +// 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.wpilibj.examples.elevatorexponentialprofile; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.trajectory.ExponentialProfile; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.TimedRobot; + +public class Robot extends TimedRobot { + private static double kDt = 0.02; + + private final Joystick m_joystick = new Joystick(1); + private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(1); + // Note: These gains are fake, and will have to be tuned for your robot. + private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 1, 1); + + // Create a motion profile with the given maximum voltage and characteristics kV, kA + // These gains should match your feedforward kV, kA for best results. + private final ExponentialProfile m_profile = + new ExponentialProfile(ExponentialProfile.Constraints.fromCharacteristics(10, 1, 1)); + private ExponentialProfile.State m_goal = new ExponentialProfile.State(0, 0); + private ExponentialProfile.State m_setpoint = new ExponentialProfile.State(0, 0); + + @Override + public void robotInit() { + // Note: These gains are fake, and will have to be tuned for your robot. + m_motor.setPID(1.3, 0.0, 0.7); + } + + @Override + public void teleopPeriodic() { + if (m_joystick.getRawButtonPressed(2)) { + m_goal = new ExponentialProfile.State(5, 0); + } else if (m_joystick.getRawButtonPressed(3)) { + m_goal = new ExponentialProfile.State(0, 0); + } + + // Retrieve the profiled setpoint for the next timestep. This setpoint moves + // toward the goal while obeying the constraints. + ExponentialProfile.State next = m_profile.calculate(kDt, m_setpoint, m_goal); + + // Send setpoint to offboard controller PID + m_motor.setSetpoint( + ExampleSmartMotorController.PIDMode.kPosition, + m_setpoint.position, + m_feedforward.calculate(m_setpoint.velocity, next.velocity, 0.02) / 12.0); + + m_setpoint = next; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Constants.java new file mode 100644 index 0000000000..7a6a937fee --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Constants.java @@ -0,0 +1,39 @@ +// 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.wpilibj.examples.elevatorexponentialsimulation; + +import edu.wpi.first.math.util.Units; + +public class Constants { + public static final int kMotorPort = 0; + public static final int kEncoderAChannel = 0; + public static final int kEncoderBChannel = 1; + public static final int kJoystickPort = 0; + + public static final double kElevatorKp = 0.75; + public static final double kElevatorKi = 0; + public static final double kElevatorKd = 0; + + public static final double kElevatorMaxV = 10.0; // volts (V) + public static final double kElevatorkS = 0.0; // volts (V) + public static final double kElevatorkG = 0.62; // volts (V) + public static final double kElevatorkV = 3.9; // volts (V) + public static final double kElevatorkA = 0.06; // volts (V) + + public static final double kElevatorGearing = 5.0; + public static final double kElevatorDrumRadius = Units.inchesToMeters(1.0); + public static final double kCarriageMass = Units.lbsToKilograms(12); // kg + + public static final double kSetpointMeters = Units.inchesToMeters(42.875); + public static final double kLowerkSetpointMeters = Units.inchesToMeters(15); + // Encoder is reset to measure 0 at the bottom, so minimum height is 0. + public static final double kMinElevatorHeightMeters = 0.0; + public static final double kMaxElevatorHeightMeters = Units.inchesToMeters(50); + + // distance per pulse = (distance per revolution) / (pulses per revolution) + // = (Pi * D) / ppr + public static final double kElevatorEncoderDistPerPulse = + 2.0 * Math.PI * kElevatorDrumRadius / 4096; +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Main.java new file mode 100644 index 0000000000..cfd4b9f071 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Main.java @@ -0,0 +1,25 @@ +// 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.wpilibj.examples.elevatorexponentialsimulation; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Robot.java new file mode 100644 index 0000000000..a123414e36 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Robot.java @@ -0,0 +1,62 @@ +// 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.wpilibj.examples.elevatorexponentialsimulation; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.examples.elevatorexponentialsimulation.subsystems.Elevator; + +/** This is a sample program to demonstrate the use of elevator simulation. */ +public class Robot extends TimedRobot { + private final Joystick m_joystick = new Joystick(Constants.kJoystickPort); + private final Elevator m_elevator = new Elevator(); + + public Robot() { + super(0.020); + } + + @Override + public void robotInit() {} + + @Override + public void robotPeriodic() { + // Update the telemetry, including mechanism visualization, regardless of mode. + m_elevator.updateTelemetry(); + } + + @Override + public void simulationPeriodic() { + // Update the simulation model. + m_elevator.simulationPeriodic(); + } + + @Override + public void teleopInit() { + m_elevator.reset(); + } + + @Override + public void teleopPeriodic() { + if (m_joystick.getTrigger()) { + // Here, we set the constant setpoint of 10 meters. + m_elevator.reachGoal(Constants.kSetpointMeters); + } else { + // Otherwise, we update the setpoint to 1 meter. + m_elevator.reachGoal(Constants.kLowerkSetpointMeters); + } + } + + @Override + public void disabledInit() { + // This just makes sure that our simulation code knows that the motor's off. + m_elevator.stop(); + } + + @Override + public void close() { + m_elevator.close(); + super.close(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java new file mode 100644 index 0000000000..6d176e8c35 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java @@ -0,0 +1,142 @@ +// 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.wpilibj.examples.elevatorexponentialsimulation.subsystems; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.trajectory.ExponentialProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.examples.elevatorexponentialsimulation.Constants; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.simulation.EncoderSim; +import edu.wpi.first.wpilibj.simulation.PWMSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class Elevator implements AutoCloseable { + // This gearbox represents a gearbox containing 4 Vex 775pro motors. + private final DCMotor m_elevatorGearbox = DCMotor.getNEO(2); + + private final ExponentialProfile m_profile = + new ExponentialProfile( + ExponentialProfile.Constraints.fromCharacteristics( + Constants.kElevatorMaxV, Constants.kElevatorkV, Constants.kElevatorkA)); + + private ExponentialProfile.State m_setpoint = new ExponentialProfile.State(0, 0); + + // Standard classes for controlling our elevator + private final PIDController m_pidController = + new PIDController(Constants.kElevatorKp, Constants.kElevatorKi, Constants.kElevatorKd); + + ElevatorFeedforward m_feedforward = + new ElevatorFeedforward( + Constants.kElevatorkS, + Constants.kElevatorkG, + Constants.kElevatorkV, + Constants.kElevatorkA); + private final Encoder m_encoder = + new Encoder(Constants.kEncoderAChannel, Constants.kEncoderBChannel); + private final PWMSparkMax m_motor = new PWMSparkMax(Constants.kMotorPort); + + // Simulation classes help us simulate what's going on, including gravity. + private final ElevatorSim m_elevatorSim = + new ElevatorSim( + m_elevatorGearbox, + Constants.kElevatorGearing, + Constants.kCarriageMass, + Constants.kElevatorDrumRadius, + Constants.kMinElevatorHeightMeters, + Constants.kMaxElevatorHeightMeters, + true, + 0, + VecBuilder.fill(0.005)); + private final EncoderSim m_encoderSim = new EncoderSim(m_encoder); + private final PWMSim m_motorSim = new PWMSim(m_motor); + + // Create a Mechanism2d visualization of the elevator + private final Mechanism2d m_mech2d = + new Mechanism2d(Units.inchesToMeters(10), Units.inchesToMeters(51)); + private final MechanismRoot2d m_mech2dRoot = + m_mech2d.getRoot("Elevator Root", Units.inchesToMeters(5), Units.inchesToMeters(0.5)); + private final MechanismLigament2d m_elevatorMech2d = + m_mech2dRoot.append( + new MechanismLigament2d("Elevator", m_elevatorSim.getPositionMeters(), 90)); + + /** Subsystem constructor. */ + public Elevator() { + m_encoder.setDistancePerPulse(Constants.kElevatorEncoderDistPerPulse); + + // Publish Mechanism2d to SmartDashboard + // To view the Elevator visualization, select Network Tables -> SmartDashboard -> Elevator Sim + SmartDashboard.putData("Elevator Sim", m_mech2d); + } + + /** Advance the simulation. */ + public void simulationPeriodic() { + // In this method, we update our simulation of what our elevator is doing + // First, we set our "inputs" (voltages) + m_elevatorSim.setInput(m_motorSim.getSpeed() * RobotController.getBatteryVoltage()); + + // Next, we update it. The standard loop time is 20ms. + m_elevatorSim.update(0.020); + + // Finally, we set our simulated encoder's readings and simulated battery voltage + m_encoderSim.setDistance(m_elevatorSim.getPositionMeters()); + // SimBattery estimates loaded battery voltages + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps())); + } + + /** + * Run control loop to reach and maintain goal. + * + * @param goal the position to maintain + */ + public void reachGoal(double goal) { + var goalState = new ExponentialProfile.State(goal, 0); + + var next = m_profile.calculate(0.020, m_setpoint, goalState); + + // With the setpoint value we run PID control like normal + double pidOutput = m_pidController.calculate(m_encoder.getDistance(), m_setpoint.position); + double feedforwardOutput = m_feedforward.calculate(m_setpoint.velocity, next.velocity, 0.020); + + m_motor.setVoltage(pidOutput + feedforwardOutput); + + m_setpoint = next; + } + + /** Stop the control loop and motor output. */ + public void stop() { + m_motor.set(0.0); + } + + /** Reset Exponential profile to begin from current position on enable. */ + public void reset() { + m_setpoint = new ExponentialProfile.State(m_encoder.getDistance(), 0); + } + + /** Update telemetry, including the mechanism visualization. */ + public void updateTelemetry() { + // Update elevator visualization with position + m_elevatorMech2d.setLength(m_encoder.getDistance()); + } + + @Override + public void close() { + m_encoder.close(); + m_motor.close(); + m_mech2d.close(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index 99e20270f3..2b6e5f90a5 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -173,6 +173,21 @@ "mainclass": "Main", "commandversion": 2 }, + { + "name": "Elevator with exponential profile", + "description": "Reach elevator position setpoints with exponential profiles and smart motor controller PID.", + "tags": [ + "Basic Robot", + "Elevator", + "Exponential Profile", + "Smart Motor Controller", + "Joystick" + ], + "foldername": "elevatorexponentialprofile", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, { "name": "Elevator with profiled PID controller", "description": "Reach elevator position setpoints with an encoder and profiled PID control.", @@ -802,6 +817,23 @@ "mainclass": "Main", "commandversion": 2 }, + { + "name": "Elevator Exponential Profile Simulation", + "description": "Simulate an elevator.", + "tags": [ + "Basic Robot", + "Elevator", + "State-Space", + "Simulation", + "Mechanism2d", + "PID", + "Exponential Profile" + ], + "foldername": "elevatorexponentialsimulation", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, { "name": "ArmSimulation", "description": "Simulate a single-jointed arm.", diff --git a/wpimath/algorithms.md b/wpimath/algorithms.md index 3b27418e1c..fc2904c965 100644 --- a/wpimath/algorithms.md +++ b/wpimath/algorithms.md @@ -457,3 +457,49 @@ rĖ‚ = ð‘Ģ⃗ / ||ð‘Ģ⃗|| 𝑟⃗ = Îļ * rĖ‚ 𝑟⃗ = 2 * atan2(||ð‘Ģ⃗||, s) / ||ð‘Ģ⃗|| * ð‘Ģ⃗ ``` + +## Closed form solution for an Exponential Motion Profile + +### [Derivation of continuous-time model](wpimath/algorithms/docs/ExponentialProfileModel.py) + + +### Heuristic for input direction in Exponential Profile + +Demonstration: https://www.desmos.com/calculator/3jamollwrk + +The fastest path possible for an exponential profile (and the placement of the inflection point) depend on boundary conditions. + +Specifically, the placement (xf, vf) relative to the possible trajectories that cross through (x0, v0) decides this. There are two possible trajectories to take from the initial state. In the desmos demo these are colored Green and Purple, which arise from applying +input and -input from the initial state respectively. Red and Yellow trajectories arise from applying -input and +input respectively from terminal conditions. + +In order to reach the terminal state from the initial state by following Green in the +v direction, the second step is following Red in the -v direction. +Likewise, Purple must be followed in the -v direction, and then Yellow must be followed in the +v direction. + +The specific conditions surrounding this decision are fourfold: +- A: v0 >= 0 +- B: vf >= 0 +- C: vf >= x1_ps(vf, U) +- D: vf >= x1_ps(vf, -U) + +Where x1_ps(v, U) follows the Green line, and x1_ps(v, -U) follows the Purple line. + +This creates a decision table: +| v0>=0 | vf>=0 | vf>=x1_ps(vf,U) | vf>=x1_ps(vf,-U) | Output Sign | +|-------|-------|-----------------|------------------|------------:| +| False | False | False | False | -1 | +| False | False | False | True | 1 | +| False | False | True | False | 1 | +| False | False | True | True | 1 | +| False | True | False | False | -1 | +| False | True | False | True | -1 | +| False | True | True | False | 1 | +| False | True | True | True | 1 | +| True | False | False | False | -1 | +| True | False | False | True | 1 | +| True | False | True | False | -1 | +| True | False | True | True | 1 | +| True | True | False | False | -1 | +| True | True | False | True | -1 | +| True | True | True | False | -1 | +| True | True | True | True | 1 | + +Which is equivalent to `-1 if (A & ~D) | (B & ~C) | (~C & ~D) else 1`. diff --git a/wpimath/algorithms/ExponentialProfileModel.py b/wpimath/algorithms/ExponentialProfileModel.py new file mode 100644 index 0000000000..cc8fbc06cc --- /dev/null +++ b/wpimath/algorithms/ExponentialProfileModel.py @@ -0,0 +1,98 @@ +from sympy import * +from sympy.logic.boolalg import * + +init_printing() + +U, A, B, t, x0, xf, v0, vf, c1, c2, v, V, kV, kA = symbols( + "U, A, B t, x0, xf, v0, vf, C1, C2, v, V, kV, kA" +) + +x = symbols("x", cls=Function) + +# Exponential profiles are derived from a differential equation: xĖˆ - A * xĖ‡ = B * U +diffeq = Eq(x(t).diff(t, t) - A * x(t).diff(t), B * U) + +x = dsolve(diffeq).rhs +dx = x.diff(t) + +x = x.subs( + [ + (c1, solve(Eq(x.subs(t, 0), x0), c1)[0]), + (c2, solve(Eq(dx.subs(t, 0), v0), c2)[0]), + ] +) + +print(f"General Solution: {x}") + +# We need two specific solutions to this equation for an Exponential Profile: +# One that passes through (x0, v0) and has input U +# Another that passes through (xf, vf) and has input -U + +# x1 is for the accelerate step +x1 = x.subs({x0: x0, v0: v0, U: U}) + +dx1 = x1.diff(t) +t1_eqn = solve(Eq(dx1, v), t)[0] +# x1 in phase space (input v, output x) +x1_ps = x1.subs(t, t1_eqn) + + +# x2 is for the decelerate step +x2 = x.subs({x0: xf, v0: vf, U: -U}) + +dx2 = x2.diff(t) +t2_eqn = solve(Eq(dx2, v), t)[0] +# x2 in phase space (input v, output x) +x2_ps = x2.subs(t, t2_eqn) + +# The point at which we switch from input U to -U is the inflection point. +# In phase space, this is a point (x, v) where x1(v) = x2(v) +# For now, we will just solve for +U and assume inflection velocity is positive. +# The other possible solutions are -v_soln, and the solutions to v_equality.subs(U, -U) +equality = simplify(Eq(x1_ps, x2_ps).expand()).expand() +equality = Eq(equality.lhs - x0 + v0 / A - v / A, equality.rhs - x0 + v0 / A - v / A) +equality = Eq( + equality.lhs + - B * U * log(A * v / (A * vf - B * U) - B * U / (A * vf - B * U)) / A**2, + equality.rhs + - B * U * log(A * v / (A * vf - B * U) - B * U / (A * vf - B * U)) / A**2, +) +equality = Eq(equality.lhs / (-B * U / A / A), equality.rhs / (-B * U / A / A)) +equality = Eq(exp(equality.lhs.simplify()), exp(equality.rhs.simplify())) +equality = Eq( + equality.lhs * (A * v0 + B * U) * (A * vf - B * U), + equality.rhs * (A * v0 + B * U) * (A * vf - B * U), +) +equality = Eq(-equality.lhs.expand() + equality.rhs, 0) + +# This is a quadratic equation of the form ax^2 + c = 0 +v_equality = equality + +# solve, take positive result +v_soln = solve(v_equality, v)[0] + +# With this information, we can calculate the inflection point (x, v) +# and calculate the times that x1 and x2 reach the inflection point +inflection_x = x1_ps.subs(v, v_soln) +inflection_t1 = t1_eqn.subs(v, v_soln) +inflection_t2 = t2_eqn.subs(v, v_soln) + +# inflection_t2 < 0 because in order for the profile to get to +# the inflection point from the terminal state, it must go back in time. +totalTime = inflection_t1 - inflection_t2 + +print(f"x1: {expand(simplify(x1))}") +print(f"x2: {expand(simplify(x2))}") +print(f"dx1: {expand(simplify(dx1))}") +print(f"dx2: {expand(simplify(dx2))}") +print(f"t1: {expand(simplify(t1_eqn))}") +print(f"t2: {expand(simplify(t2_eqn))}") +print(f"x1 phase space: {expand(simplify(x1.subs(t, t1_eqn)))}") +print(f"x2 phase space: {expand(simplify(x2.subs(t, t2_eqn)))}") +print(f"vi equality: {v_equality}") + + +a, b, c, d = symbols("a, b, c, d") + +expression = SOPform([a, b, c, d], minterms=[0, 4, 5, 8, 10, 12, 13, 14]) +print(f"Truth Table Expression: {expression}") diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java new file mode 100644 index 0000000000..1580e853c7 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java @@ -0,0 +1,449 @@ +// 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.trajectory; + +import java.util.Objects; + +/** + * A exponential curve-shaped velocity profile. + * + *

While this class can be used for a profiled movement from start to finish, the intended usage + * is to filter a reference's dynamics based on state-space model dynamics. To compute the reference + * obeying this constraint, do the following. + * + *

Initialization: + * + * 


+ * ExponentialProfile.Constraints constraints =
+ *   ExponentialProfile.Constraints.fromCharacteristics(kMaxV, kV, kA);
+ * ExponentialProfile.State previousProfiledReference =
+ *   new ExponentialProfile.State(initialReference, 0.0);
+ * ExponentialProfile profile = new ExponentialProfile(constraints);
+ *
+ * + *

Run on update: + * + * 


+ * previousProfiledReference =
+ * profile.calculate(timeSincePreviousUpdate, previousProfiledReference, unprofiledReference);
+ *
+ * + *

where `unprofiledReference` is free to change between calls. Note that when the unprofiled + * reference is within the constraints, `calculate()` returns the unprofiled reference unchanged. + * + *

Otherwise, a timer can be started to provide monotonic values for `calculate()` and to + * determine when the profile has completed via `isFinished()`. + */ +public class ExponentialProfile { + private final Constraints m_constraints; + + public static class ProfileTiming { + public final double inflectionTime; + public final double totalTime; + + protected ProfileTiming(double inflectionTime, double totalTime) { + this.inflectionTime = inflectionTime; + this.totalTime = totalTime; + } + + /** + * Decides if the profile is finished by time t. + * + * @param t The time since the beginning of the profile. + * @return if the profile is finished at time t. + */ + public boolean isFinished(double t) { + return t > inflectionTime; + } + } + + public static class Constraints { + public final double maxInput; + + public final double A; + public final double B; + + /** + * Construct constraints for an ExponentialProfile. + * + * @param maxInput maximum unsigned input voltage + * @param A The State-Space 1x1 system matrix. + * @param B The State-Space 1x1 input matrix. + */ + private Constraints(double maxInput, double A, double B) { + this.maxInput = maxInput; + this.A = A; + this.B = B; + } + + /** + * Computes the max achievable velocity for an Exponential Profile. + * + * @return The seady-state velocity achieved by this profile. + */ + public double maxVelocity() { + return -maxInput * B / A; + } + + /** + * Construct constraints for an ExponentialProfile from characteristics. + * + * @param maxInput maximum unsigned input voltage + * @param kV The velocity gain. + * @param kA The acceleration gain. + * @return The Constraints object. + */ + public static Constraints fromCharacteristics(double maxInput, double kV, double kA) { + return new Constraints(maxInput, -kV / kA, 1.0 / kA); + } + + /** + * Construct constraints for an ExponentialProfile from State-Space parameters. + * + * @param maxInput maximum unsigned input voltage + * @param A The State-Space 1x1 system matrix. + * @param B The State-Space 1x1 input matrix. + * @return The Constraints object. + */ + public static Constraints fromStateSpace(double maxInput, double A, double B) { + return new Constraints(maxInput, A, B); + } + } + + public static class State { + public final double position; + + public final double velocity; + + /** + * Construct a state within an exponential profile. + * + * @param position The position at this state. + * @param velocity The velocity at this state. + */ + public State(double position, double velocity) { + this.position = position; + this.velocity = velocity; + } + + @Override + public boolean equals(Object other) { + if (other instanceof State) { + State rhs = (State) other; + return this.position == rhs.position && this.velocity == rhs.velocity; + } else { + return false; + } + } + + @Override + public int hashCode() { + return Objects.hash(position, velocity); + } + } + + /** + * Construct an ExponentialProfile. + * + * @param constraints The constraints on the profile, like maximum input. + */ + public ExponentialProfile(Constraints constraints) { + m_constraints = constraints; + } + + /** + * Calculate the correct position and velocity for the profile at a time t where the current state + * is at time t = 0. + * + * @param t The time since the beginning of the profile. + * @param current The current state. + * @param goal The desired state when the profile is complete. + * @return The position and velocity of the profile at time t. + */ + public State calculate(double t, State current, State goal) { + var direction = shouldFlipInput(current, goal) ? -1 : 1; + var u = direction * m_constraints.maxInput; + + var inflectionPoint = calculateInflectionPoint(current, goal, u); + var timing = calculateProfileTiming(current, inflectionPoint, goal, u); + + if (t < 0) { + return current; + } else if (t < timing.inflectionTime) { + return new State( + computeDistanceFromTime(t, u, current), computeVelocityFromTime(t, u, current)); + } else if (t < timing.totalTime) { + return new State( + computeDistanceFromTime(t - timing.totalTime, -u, goal), + computeVelocityFromTime(t - timing.totalTime, -u, goal)); + } else { + return goal; + } + } + + /** + * Calculate the point after which the fastest way to reach the goal state is to apply input in + * the opposite direction. + * + * @param current The current state. + * @param goal The desired state when the profile is complete. + * @return The position and velocity of the profile at the inflection point. + */ + public State calculateInflectionPoint(State current, State goal) { + var direction = shouldFlipInput(current, goal) ? -1 : 1; + var u = direction * m_constraints.maxInput; + + return calculateInflectionPoint(current, goal, u); + } + + /** + * Calculate the point after which the fastest way to reach the goal state is to apply input in + * the opposite direction. + * + * @param current The current state. + * @param goal The desired state when the profile is complete. + * @param input The signed input applied to this profile from the current state. + * @return The position and velocity of the profile at the inflection point. + */ + private State calculateInflectionPoint(State current, State goal, double input) { + var u = input; + + if (current.equals(goal)) { + return current; + } + + var inflectionVelocity = solveForInflectionVelocity(u, current, goal); + var inflectionPosition = computeDistanceFromVelocity(inflectionVelocity, -u, goal); + + return new State(inflectionPosition, inflectionVelocity); + } + + /** + * Calculate the time it will take for this profile to reach the goal state. + * + * @param current The current state. + * @param goal The desired state when the profile is complete. + * @return The total duration of this profile. + */ + public double timeLeftUntil(State current, State goal) { + var timing = calculateProfileTiming(current, goal); + + return timing.totalTime; + } + + /** + * Calculate the time it will take for this profile to reach the inflection point, and the time it + * will take for this profile to reach the goal state. + * + * @param current The current state. + * @param goal The desired state when the profile is complete. + * @return The timing information for this profile. + */ + public ProfileTiming calculateProfileTiming(State current, State goal) { + var direction = shouldFlipInput(current, goal) ? -1 : 1; + var u = direction * m_constraints.maxInput; + + var inflectionPoint = calculateInflectionPoint(current, goal, u); + return calculateProfileTiming(current, inflectionPoint, goal, u); + } + + /** + * Calculate the time it will take for this profile to reach the inflection point, and the time it + * will take for this profile to reach the goal state. + * + * @param current The current state. + * @param inflectionPoint The inflection point of this profile. + * @param goal The desired state when the profile is complete. + * @param input The signed input applied to this profile from the current state. + * @return The timing information for this profile. + */ + private ProfileTiming calculateProfileTiming( + State current, State inflectionPoint, State goal, double input) { + var u = input; + + double inflectionT_forward; + + // We need to handle 5 cases here: + // + // - Approaching -maxVelocity from below + // - Approaching -maxVelocity from above + // - Approaching maxVelocity from below + // - Approaching maxVelocity from above + // - At +-maxVelocity + // + // For cases 1 and 3, we want to subtract epsilon from the inflection point velocity. + // For cases 2 and 4, we want to add epsilon to the inflection point velocity. + // For case 5, we have reached inflection point velocity. + double epsilon = 1e-9; + if (Math.abs(Math.signum(input) * m_constraints.maxVelocity() - inflectionPoint.velocity) + < epsilon) { + double solvableV = inflectionPoint.velocity; + double t_to_solvable_v; + double x_at_solvable_v; + if (Math.abs(current.velocity - inflectionPoint.velocity) < epsilon) { + t_to_solvable_v = 0; + x_at_solvable_v = current.position; + } else { + if (Math.abs(current.velocity) > m_constraints.maxVelocity()) { + solvableV += Math.signum(u) * epsilon; + } else { + solvableV -= Math.signum(u) * epsilon; + } + + t_to_solvable_v = computeTimeFromVelocity(solvableV, u, current.velocity); + x_at_solvable_v = computeDistanceFromVelocity(solvableV, u, current); + } + + inflectionT_forward = + t_to_solvable_v + + Math.signum(input) + * (inflectionPoint.position - x_at_solvable_v) + / m_constraints.maxVelocity(); + } else { + inflectionT_forward = computeTimeFromVelocity(inflectionPoint.velocity, u, current.velocity); + } + + var inflectionT_backward = computeTimeFromVelocity(inflectionPoint.velocity, -u, goal.velocity); + + return new ProfileTiming(inflectionT_forward, inflectionT_forward - inflectionT_backward); + } + + /** + * Calculate the position reached after t seconds when applying an input from the initial state. + * + * @param t The time since the initial state. + * @param input The signed input applied to this profile from the initial state. + * @param initial The initial state. + * @return The distance travelled by this profile. + */ + private double computeDistanceFromTime(double t, double input, State initial) { + var A = m_constraints.A; + var B = m_constraints.B; + var u = input; + + return initial.position + + (-B * u * t + (initial.velocity + B * u / A) * (Math.exp(A * t) - 1)) / A; + } + + /** + * Calculate the velocity reached after t seconds when applying an input from the initial state. + * + * @param t The time since the initial state. + * @param input The signed input applied to this profile from the initial state. + * @param initial The initial state. + * @return The distance travelled by this profile. + */ + private double computeVelocityFromTime(double t, double input, State initial) { + var A = m_constraints.A; + var B = m_constraints.B; + var u = input; + + return (initial.velocity + B * u / A) * Math.exp(A * t) - B * u / A; + } + + /** + * Calculate the time required to reach a specified velocity given the initial velocity. + * + * @param velocity The goal velocity. + * @param input The signed input applied to this profile from the initial state. + * @param initial The initial velocity. + * @return The time required to reach the goal velocity. + */ + private double computeTimeFromVelocity(double velocity, double input, double initial) { + var A = m_constraints.A; + var B = m_constraints.B; + var u = input; + + return Math.log((A * velocity + B * u) / (A * initial + B * u)) / A; + } + + /** + * Calculate the distance reached at the same time as the given velocity when applying the given + * input from the initial state. + * + * @param velocity The velocity reached by this profile + * @param input The signed input applied to this profile from the initial state. + * @param initial The initial state. + * @return The distance reached when the given velocity is reached. + */ + private double computeDistanceFromVelocity(double velocity, double input, State initial) { + var A = m_constraints.A; + var B = m_constraints.B; + var u = input; + + return initial.position + + (velocity - initial.velocity) / A + - B * u / (A * A) * Math.log((A * velocity + B * u) / (A * initial.velocity + B * u)); + } + + /** + * Calculate the velocity at which input should be reversed in order to reach the goal state from + * the current state. + * + * @param input The signed input applied to this profile from the current state. + * @param current The current state. + * @param goal The goal state. + * @return The inflection velocity. + */ + private double solveForInflectionVelocity(double input, State current, State goal) { + var A = m_constraints.A; + var B = m_constraints.B; + var u = input; + + var U_dir = Math.signum(u); + + var position_delta = goal.position - current.position; + var velocity_delta = goal.velocity - current.velocity; + + var scalar = (A * current.velocity + B * u) * (A * goal.velocity - B * u); + var power = -A / B / u * (A * position_delta - velocity_delta); + + var a = -A * A; + var c = (B * B) * (u * u) + scalar * Math.exp(power); + + if (-1e-9 < c && c < 0) { + // Numerical stability issue - the heuristic gets it right but c is around -1e-13 + return 0; + } + + return U_dir * Math.sqrt(-c / a); + } + + /** + * Returns true if the profile should be inverted. + * + *

The profile is inverted if we should first apply negative input in order to reach the goal + * state. + * + * @param current The initial state (usually the current state). + * @param goal The desired state when the profile is complete. + */ + @SuppressWarnings("UnnecessaryParentheses") + private boolean shouldFlipInput(State current, State goal) { + var u = m_constraints.maxInput; + + var xf = goal.position; + var v0 = current.velocity; + var vf = goal.velocity; + + var x_forward = computeDistanceFromVelocity(vf, u, current); + var x_reverse = computeDistanceFromVelocity(vf, -u, current); + + if (v0 >= m_constraints.maxVelocity()) { + return xf < x_reverse; + } + + if (v0 <= -m_constraints.maxVelocity()) { + return xf < x_forward; + } + + var a = v0 >= 0; + var b = vf >= 0; + var c = xf >= x_forward; + var d = xf >= x_reverse; + + return (a && !d) || (b && !c) || (!c && !d); + } +} diff --git a/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h b/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h new file mode 100644 index 0000000000..f45987b7d0 --- /dev/null +++ b/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.h @@ -0,0 +1,194 @@ +// 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 "units/time.h" +#include "wpimath/MathShared.h" + +namespace frc { + +/** + * A Exponential-shaped velocity profile. + * + * While this class can be used for a profiled movement from start to finish, + * the intended usage is to filter a reference's dynamics based on + * ExponentialProfile velocity constraints. To compute the reference obeying + * this constraint, do the following. + * + * Initialization: + * @code{.cpp} + * ExponentialProfile::Constraints constraints{kMaxV, kV, kA}; + * State previousProfiledReference = {initialReference, 0_mps}; + * @endcode + * + * Run on update: + * @code{.cpp} + * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate, + * previousProfiledReference, unprofiledReference); + * @endcode + * + * where `unprofiledReference` is free to change between calls. Note that when + * the unprofiled reference is within the constraints, `Calculate()` returns the + * unprofiled reference unchanged. + * + * Otherwise, a timer can be started to provide monotonic values for + * `Calculate()` and to determine when the profile has completed via + * `IsFinished()`. + */ +template +class ExponentialProfile { + public: + using Distance_t = units::unit_t; + using Velocity = + units::compound_unit>; + using Velocity_t = units::unit_t; + using Acceleration = + units::compound_unit>; + using Input_t = units::unit_t; + using A_t = units::unit_t>; + using B_t = + units::unit_t>>; + using KV = units::compound_unit>; + using kV_t = units::unit_t; + using KA = units::compound_unit>; + using kA_t = units::unit_t; + + class Constraints { + public: + Constraints(Input_t maxInput, A_t A, B_t B) + : maxInput{maxInput}, A{A}, B{B} {} + Constraints(Input_t maxInput, kV_t kV, kA_t kA) + : maxInput{maxInput}, A{-kV / kA}, B{1 / kA} {} + Velocity_t MaxVelocity() const { return -maxInput * B / A; } + + Input_t maxInput{0}; + A_t A{0}; + B_t B{0}; + }; + + class State { + public: + Distance_t position{0}; + Velocity_t velocity{0}; + bool operator==(const State&) const = default; + }; + + class ProfileTiming { + public: + units::second_t inflectionTime; + units::second_t totalTime; + + bool IsFinished(const units::second_t& time) const { + return time > totalTime; + } + }; + + /** + * Construct a ExponentialProfile. + * + * @param constraints The constraints on the profile, like maximum input. + */ + explicit ExponentialProfile(Constraints constraints); + + ExponentialProfile(const ExponentialProfile&) = default; + ExponentialProfile& operator=(const ExponentialProfile&) = default; + ExponentialProfile(ExponentialProfile&&) = default; + ExponentialProfile& operator=(ExponentialProfile&&) = default; + + /** + * Calculate the correct position and velocity for the profile at a time t + * where the current state is at time t = 0. + */ + State Calculate(const units::second_t& t, const State& current, + const State& goal) const; + + /** + * Calculate the point after which the fastest way to reach the goal state is + * to apply input in the opposite direction. + */ + State CalculateInflectionPoint(const State& current, const State& goal) const; + + /** + * Calculate the time it will take for this profile to reach the goal state. + */ + units::second_t TimeLeftUntil(const State& current, const State& goal) const; + + /** + * Calculate the time it will take for this profile to reach the inflection + * point, and the time it will take for this profile to reach the goal state. + */ + ProfileTiming CalculateProfileTiming(const State& current, + const State& goal) const; + + private: + /** + * Calculate the point after which the fastest way to reach the goal state is + * to apply input in the opposite direction. + */ + State CalculateInflectionPoint(const State& current, const State& goal, + const Input_t& input) const; + + /** + * Calculate the time it will take for this profile to reach the inflection + * point, and the time it will take for this profile to reach the goal state. + */ + ProfileTiming CalculateProfileTiming(const State& current, + const State& inflectionPoint, + const State& goal, + const Input_t& input) const; + + /** + * Calculate the velocity reached after t seconds when applying an input from + * the initial state. + */ + Velocity_t ComputeVelocityFromTime(const units::second_t& time, + const Input_t& input, + const State& initial) const; + + /** + * Calculate the position reached after t seconds when applying an input from + * the initial state. + */ + Distance_t ComputeDistanceFromTime(const units::second_t& time, + const Input_t& input, + const State& initial) const; + + /** + * Calculate the distance reached at the same time as the given velocity when + * applying the given input from the initial state. + */ + Distance_t ComputeDistanceFromVelocity(const Velocity_t& velocity, + const Input_t& input, + const State& initial) const; + + /** + * Calculate the time required to reach a specified velocity given the initial + * velocity. + */ + units::second_t ComputeTimeFromVelocity(const Velocity_t& velocity, + const Input_t& input, + const Velocity_t& initial) const; + + /** + * Calculate the velocity at which input should be reversed in order to reach + * the goal state from the current state. + */ + Velocity_t SolveForInflectionVelocity(const Input_t& input, + const State& current, + const State& goal) const; + + /** + * Returns true if the profile should be inverted. + * + *

The profile is inverted if we should first apply negative input in order + * to reach the goal state. + */ + bool ShouldFlipInput(const State& current, const State& goal) const; + + Constraints m_constraints; +}; +} // namespace frc + +#include "ExponentialProfile.inc" diff --git a/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.inc b/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.inc new file mode 100644 index 0000000000..ded52452e5 --- /dev/null +++ b/wpimath/src/main/native/include/frc/trajectory/ExponentialProfile.inc @@ -0,0 +1,253 @@ +// 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/trajectory/ExponentialProfile.h" +#include "units/math.h" + +namespace frc { +template +ExponentialProfile::ExponentialProfile(Constraints constraints) + : m_constraints(constraints) {} + +template +typename ExponentialProfile::State +ExponentialProfile::Calculate(const units::second_t& t, + const State& current, + const State& goal) const { + auto direction = ShouldFlipInput(current, goal) ? -1 : 1; + auto u = direction * m_constraints.maxInput; + + auto inflectionPoint = CalculateInflectionPoint(current, goal, u); + auto timing = CalculateProfileTiming(current, inflectionPoint, goal, u); + + if (t < 0_s) { + return current; + } else if (t < timing.inflectionTime) { + return {ComputeDistanceFromTime(t, u, current), + ComputeVelocityFromTime(t, u, current)}; + } else if (t < timing.totalTime) { + return {ComputeDistanceFromTime(t - timing.totalTime, -u, goal), + ComputeVelocityFromTime(t - timing.totalTime, -u, goal)}; + } else { + return goal; + } +} + +template +typename ExponentialProfile::State +ExponentialProfile::CalculateInflectionPoint( + const State& current, const State& goal) const { + auto direction = ShouldFlipInput(current, goal) ? -1 : 1; + auto u = direction * m_constraints.maxInput; + + return CalculateInflectionPoint(current, goal, u); +} + +template +typename ExponentialProfile::State +ExponentialProfile::CalculateInflectionPoint( + const State& current, const State& goal, const Input_t& input) const { + auto u = input; + + if (current == goal) { + return current; + } + + auto inflectionVelocity = SolveForInflectionVelocity(u, current, goal); + auto inflectionPosition = + ComputeDistanceFromVelocity(inflectionVelocity, -u, goal); + + return {inflectionPosition, inflectionVelocity}; +} + +template +units::second_t ExponentialProfile::TimeLeftUntil( + const State& current, const State& goal) const { + auto timing = CalculateProfileTiming(current, goal); + + return timing.totalTime; +} + +template +typename ExponentialProfile::ProfileTiming +ExponentialProfile::CalculateProfileTiming( + const State& current, const State& goal) const { + auto direction = ShouldFlipInput(current, goal) ? -1 : 1; + auto u = direction * m_constraints.maxInput; + + auto inflectionPoint = CalculateInflectionPoint(current, goal, u); + return CalculateProfileTiming(current, inflectionPoint, goal, u); +} + +template +typename ExponentialProfile::ProfileTiming +ExponentialProfile::CalculateProfileTiming( + const State& current, const State& inflectionPoint, const State& goal, + const Input_t& input) const { + auto u = input; + auto u_dir = units::math::abs(u) / u; + + units::second_t inflectionT_forward; + + // We need to handle 5 cases here: + // + // - Approaching -maxVelocity from below + // - Approaching -maxVelocity from above + // - Approaching maxVelocity from below + // - Approaching maxVelocity from above + // - At +-maxVelocity + // + // For cases 1 and 3, we want to subtract epsilon from the inflection point + // velocity For cases 2 and 4, we want to add epsilon to the inflection point + // velocity. For case 5, we have reached inflection point velocity. + auto epsilon = Velocity_t(1e-9); + if (units::math::abs(u_dir * m_constraints.MaxVelocity() - + inflectionPoint.velocity) < epsilon) { + auto solvableV = inflectionPoint.velocity; + units::second_t t_to_solvable_v; + Distance_t x_at_solvable_v; + if (units::math::abs(current.velocity - inflectionPoint.velocity) < + epsilon) { + t_to_solvable_v = 0_s; + x_at_solvable_v = current.position; + } else { + if (units::math::abs(current.velocity) > m_constraints.MaxVelocity()) { + solvableV += u_dir * epsilon; + } else { + solvableV -= u_dir * epsilon; + } + + t_to_solvable_v = ComputeTimeFromVelocity(solvableV, u, current.velocity); + x_at_solvable_v = ComputeDistanceFromVelocity(solvableV, u, current); + } + + inflectionT_forward = + t_to_solvable_v + u_dir * (inflectionPoint.position - x_at_solvable_v) / + m_constraints.MaxVelocity(); + } else { + inflectionT_forward = + ComputeTimeFromVelocity(inflectionPoint.velocity, u, current.velocity); + } + + auto inflectionT_backward = + ComputeTimeFromVelocity(inflectionPoint.velocity, -u, goal.velocity); + + return {inflectionT_forward, inflectionT_forward - inflectionT_backward}; +} + +template +typename ExponentialProfile::Distance_t +ExponentialProfile::ComputeDistanceFromTime( + const units::second_t& time, const Input_t& input, + const State& initial) const { + auto A = m_constraints.A; + auto B = m_constraints.B; + auto u = input; + + return initial.position + + (-B * u * time + + (initial.velocity + B * u / A) * (units::math::exp(A * time) - 1)) / + A; +} + +template +typename ExponentialProfile::Velocity_t +ExponentialProfile::ComputeVelocityFromTime( + const units::second_t& time, const Input_t& input, + const State& initial) const { + auto A = m_constraints.A; + auto B = m_constraints.B; + auto u = input; + + return (initial.velocity + B * u / A) * units::math::exp(A * time) - + B * u / A; +} + +template +units::second_t ExponentialProfile::ComputeTimeFromVelocity( + const Velocity_t& velocity, const Input_t& input, + const Velocity_t& initial) const { + auto A = m_constraints.A; + auto B = m_constraints.B; + auto u = input; + + return units::math::log((A * velocity + B * u) / (A * initial + B * u)) / A; +} + +template +typename ExponentialProfile::Distance_t +ExponentialProfile::ComputeDistanceFromVelocity( + const Velocity_t& velocity, const Input_t& input, + const State& initial) const { + auto A = m_constraints.A; + auto B = m_constraints.B; + auto u = input; + + return initial.position + (velocity - initial.velocity) / A - + B * u / (A * A) * + units::math::log((A * velocity + B * u) / + (A * initial.velocity + B * u)); +} + +template +typename ExponentialProfile::Velocity_t +ExponentialProfile::SolveForInflectionVelocity( + const Input_t& input, const State& current, const State& goal) const { + auto A = m_constraints.A; + auto B = m_constraints.B; + auto u = input; + + auto u_dir = u / units::math::abs(u); + + auto position_delta = goal.position - current.position; + auto velocity_delta = goal.velocity - current.velocity; + + auto scalar = (A * current.velocity + B * u) * (A * goal.velocity - B * u); + auto power = -A / B / u * (A * position_delta - velocity_delta); + + auto a = -A * A; + auto c = B * B * u * u + scalar * units::math::exp(power); + + if (-1e-9 < c.value() && c.value() < 0) { + // numeric instability - the heuristic gets it right but c is around -1e-13 + return Velocity_t(0); + } + + return u_dir * units::math::sqrt(-c / a); +} + +template +bool ExponentialProfile::ShouldFlipInput( + const State& current, const State& goal) const { + auto u = m_constraints.maxInput; + + auto v0 = current.velocity; + auto xf = goal.position; + auto vf = goal.velocity; + + auto x_forward = ComputeDistanceFromVelocity(vf, u, current); + auto x_reverse = ComputeDistanceFromVelocity(vf, -u, current); + + if (v0 >= m_constraints.MaxVelocity()) { + return xf < x_reverse; + } + + if (v0 <= -m_constraints.MaxVelocity()) { + return xf < x_forward; + } + + auto a = v0 >= Velocity_t(0); + auto b = vf >= Velocity_t(0); + auto c = xf >= x_forward; + auto d = xf >= x_reverse; + + return (a && !d) || (b && !c) || (!c && !d); +} +} // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java new file mode 100644 index 0000000000..e9c535e1fb --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java @@ -0,0 +1,364 @@ +// 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.trajectory; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import java.util.List; +import org.junit.jupiter.api.Test; + +class ExponentialProfileTest { + private static final double kDt = 0.01; + private static final SimpleMotorFeedforward feedforward = + new SimpleMotorFeedforward(0, 2.5629, 0.43277); + private static final ExponentialProfile.Constraints constraints = + ExponentialProfile.Constraints.fromCharacteristics(12, 2.5629, 0.43277); + + /** + * Asserts "val1" is within "eps" of "val2". + * + * @param val1 First operand in comparison. + * @param val2 Second operand in comparison. + * @param eps Tolerance for whether values are near to each other. + */ + private static void assertNear(double val1, double val2, double eps) { + assertTrue( + Math.abs(val1 - val2) <= eps, + "Difference between " + val1 + " and " + val2 + " is greater than " + eps); + } + + private static void assertNear( + ExponentialProfile.State val1, ExponentialProfile.State val2, double eps) { + assertAll( + () -> assertNear(val1.position, val2.position, eps), + () -> assertNear(val1.position, val2.position, eps)); + } + + private static ExponentialProfile.State checkDynamics( + ExponentialProfile profile, ExponentialProfile.State current, ExponentialProfile.State goal) { + var next = profile.calculate(kDt, current, goal); + + var signal = feedforward.calculate(current.velocity, next.velocity, kDt); + + assertTrue(Math.abs(signal) < constraints.maxInput + 1e-9); + + return next; + } + + @Test + void reachesGoal() { + ExponentialProfile profile = new ExponentialProfile(constraints); + + ExponentialProfile.State goal = new ExponentialProfile.State(10, 0); + ExponentialProfile.State state = new ExponentialProfile.State(0, 0); + + for (int i = 0; i < 450; ++i) { + state = checkDynamics(profile, state, goal); + } + assertEquals(state, goal); + } + + // Tests that decreasing the maximum velocity in the middle when it is already + // moving faster than the new max is handled correctly + @Test + void posContinuousUnderVelChange() { + ExponentialProfile profile = new ExponentialProfile(constraints); + + ExponentialProfile.State goal = new ExponentialProfile.State(10, 0); + ExponentialProfile.State state = new ExponentialProfile.State(0, 0); + + for (int i = 0; i < 300; ++i) { + if (i == 150) { + profile = + new ExponentialProfile( + ExponentialProfile.Constraints.fromStateSpace(9, constraints.A, constraints.B)); + } + + state = checkDynamics(profile, state, goal); + } + assertEquals(state, goal); + } + + // Tests that decreasing the maximum velocity in the middle when it is already + // moving faster than the new max is handled correctly + @Test + void posContinuousUnderVelChangeBackward() { + ExponentialProfile profile = new ExponentialProfile(constraints); + + ExponentialProfile.State goal = new ExponentialProfile.State(-10, 0); + ExponentialProfile.State state = new ExponentialProfile.State(0, 0); + + for (int i = 0; i < 300; ++i) { + if (i == 150) { + profile = + new ExponentialProfile( + ExponentialProfile.Constraints.fromStateSpace(9, constraints.A, constraints.B)); + } + + state = checkDynamics(profile, state, goal); + } + assertEquals(state, goal); + } + + // There is some somewhat tricky code for dealing with going backwards + @Test + void backwards() { + ExponentialProfile.State goal = new ExponentialProfile.State(-10, 0); + ExponentialProfile.State state = new ExponentialProfile.State(0, 0); + + ExponentialProfile profile = new ExponentialProfile(constraints); + + for (int i = 0; i < 400; ++i) { + state = checkDynamics(profile, state, goal); + } + assertEquals(state, goal); + } + + @Test + void switchGoalInMiddle() { + ExponentialProfile.State goal = new ExponentialProfile.State(-10, 0); + ExponentialProfile.State state = new ExponentialProfile.State(0, 0); + + ExponentialProfile profile = new ExponentialProfile(constraints); + for (int i = 0; i < 50; ++i) { + state = checkDynamics(profile, state, goal); + } + assertNotEquals(state, goal); + + goal = new ExponentialProfile.State(0.0, 0.0); + for (int i = 0; i < 100; ++i) { + state = checkDynamics(profile, state, goal); + } + assertEquals(state, goal); + } + + // Checks to make sure that it hits top speed + @Test + void topSpeed() { + ExponentialProfile.State goal = new ExponentialProfile.State(40, 0); + ExponentialProfile.State state = new ExponentialProfile.State(0, 0); + + ExponentialProfile profile = new ExponentialProfile(constraints); + double maxSpeed = 0; + for (int i = 0; i < 900; ++i) { + state = checkDynamics(profile, state, goal); + maxSpeed = Math.max(maxSpeed, state.velocity); + } + + assertNear(constraints.maxVelocity(), maxSpeed, 10e-5); + assertEquals(state, goal); + } + + @Test + void topSpeedBackward() { + ExponentialProfile.State goal = new ExponentialProfile.State(-40, 0); + ExponentialProfile.State state = new ExponentialProfile.State(0, 0); + + ExponentialProfile profile = new ExponentialProfile(constraints); + double maxSpeed = 0; + for (int i = 0; i < 900; ++i) { + state = checkDynamics(profile, state, goal); + maxSpeed = Math.min(maxSpeed, state.velocity); + } + + assertNear(-constraints.maxVelocity(), maxSpeed, 10e-5); + assertEquals(state, goal); + } + + @Test + void largeInitialVelocity() { + ExponentialProfile.State goal = new ExponentialProfile.State(40, 0); + ExponentialProfile.State state = new ExponentialProfile.State(0, 8); + + ExponentialProfile profile = new ExponentialProfile(constraints); + for (int i = 0; i < 900; ++i) { + state = checkDynamics(profile, state, goal); + } + + assertEquals(state, goal); + } + + @Test + void largeNegativeInitialVelocity() { + ExponentialProfile.State goal = new ExponentialProfile.State(-40, 0); + ExponentialProfile.State state = new ExponentialProfile.State(0, -8); + + ExponentialProfile profile = new ExponentialProfile(constraints); + for (int i = 0; i < 900; ++i) { + state = checkDynamics(profile, state, goal); + } + + assertEquals(state, goal); + } + + @SuppressWarnings("PMD.TestClassWithoutTestCases") + static class TestCase { + public final ExponentialProfile.State initial; + public final ExponentialProfile.State goal; + public final ExponentialProfile.State inflectionPoint; + + TestCase( + ExponentialProfile.State initial, + ExponentialProfile.State goal, + ExponentialProfile.State inflectionPoint) { + this.initial = initial; + this.goal = goal; + this.inflectionPoint = inflectionPoint; + } + } + + @Test + void testHeuristic() { + List testCases = + List.of( + new TestCase( + new ExponentialProfile.State(0.0, -4), + new ExponentialProfile.State(0.75, -4), + new ExponentialProfile.State(1.3758, 4.4304)), + new TestCase( + new ExponentialProfile.State(0.0, -4), + new ExponentialProfile.State(1.4103, 4), + new ExponentialProfile.State(1.3758, 4.4304)), + new TestCase( + new ExponentialProfile.State(0.6603, 4), + new ExponentialProfile.State(0.75, -4), + new ExponentialProfile.State(1.3758, 4.4304)), + new TestCase( + new ExponentialProfile.State(0.6603, 4), + new ExponentialProfile.State(1.4103, 4), + new ExponentialProfile.State(1.3758, 4.4304)), + new TestCase( + new ExponentialProfile.State(0.0, -4), + new ExponentialProfile.State(0.5, -2), + new ExponentialProfile.State(0.4367, 3.7217)), + new TestCase( + new ExponentialProfile.State(0.0, -4), + new ExponentialProfile.State(0.546, 2), + new ExponentialProfile.State(0.4367, 3.7217)), + new TestCase( + new ExponentialProfile.State(0.6603, 4), + new ExponentialProfile.State(0.5, -2), + new ExponentialProfile.State(0.5560, -2.9616)), + new TestCase( + new ExponentialProfile.State(0.6603, 4), + new ExponentialProfile.State(0.546, 2), + new ExponentialProfile.State(0.5560, -2.9616)), + new TestCase( + new ExponentialProfile.State(0.0, -4), + new ExponentialProfile.State(-0.75, -4), + new ExponentialProfile.State(-0.7156, -4.4304)), + new TestCase( + new ExponentialProfile.State(0.0, -4), + new ExponentialProfile.State(-0.0897, 4), + new ExponentialProfile.State(-0.7156, -4.4304)), + new TestCase( + new ExponentialProfile.State(0.6603, 4), + new ExponentialProfile.State(-0.75, -4), + new ExponentialProfile.State(-0.7156, -4.4304)), + new TestCase( + new ExponentialProfile.State(0.6603, 4), + new ExponentialProfile.State(-0.0897, 4), + new ExponentialProfile.State(-0.7156, -4.4304)), + new TestCase( + new ExponentialProfile.State(0.0, -4), + new ExponentialProfile.State(-0.5, -4.5), + new ExponentialProfile.State(1.095, 4.314)), + new TestCase( + new ExponentialProfile.State(0.0, -4), + new ExponentialProfile.State(1.0795, 4.5), + new ExponentialProfile.State(-0.5122, -4.351)), + new TestCase( + new ExponentialProfile.State(0.6603, 4), + new ExponentialProfile.State(-0.5, -4.5), + new ExponentialProfile.State(1.095, 4.314)), + new TestCase( + new ExponentialProfile.State(0.6603, 4), + new ExponentialProfile.State(1.0795, 4.5), + new ExponentialProfile.State(-0.5122, -4.351)), + new TestCase( + new ExponentialProfile.State(0.0, -8), + new ExponentialProfile.State(0, 0), + new ExponentialProfile.State(-0.1384, 3.342)), + new TestCase( + new ExponentialProfile.State(0.0, -8), + new ExponentialProfile.State(-1, 0), + new ExponentialProfile.State(-0.562, -6.792)), + new TestCase( + new ExponentialProfile.State(0.0, 8), + new ExponentialProfile.State(1, 0), + new ExponentialProfile.State(0.562, 6.792)), + new TestCase( + new ExponentialProfile.State(0.0, 8), + new ExponentialProfile.State(-1, 0), + new ExponentialProfile.State(-0.785, -4.346))); + + var profile = new ExponentialProfile(constraints); + + for (var testCase : testCases) { + var state = profile.calculateInflectionPoint(testCase.initial, testCase.goal); + assertNear(testCase.inflectionPoint, state, 1e-3); + } + } + + @Test + void timingToCurrent() { + ExponentialProfile.State goal = new ExponentialProfile.State(2, 0); + ExponentialProfile.State state = new ExponentialProfile.State(0, 0); + + ExponentialProfile profile = new ExponentialProfile(constraints); + for (int i = 0; i < 400; i++) { + state = checkDynamics(profile, state, goal); + assertNear(profile.timeLeftUntil(state, state), 0, 2e-2); + } + } + + @Test + void timingToGoal() { + ExponentialProfile profile = new ExponentialProfile(constraints); + + ExponentialProfile.State goal = new ExponentialProfile.State(2, 0); + ExponentialProfile.State state = new ExponentialProfile.State(0, 0); + + double predictedTimeLeft = profile.timeLeftUntil(state, goal); + boolean reachedGoal = false; + for (int i = 0; i < 400; i++) { + state = checkDynamics(profile, state, goal); + + if (!reachedGoal && state.equals(goal)) { + // Expected value using for loop index is just an approximation since + // the time left in the profile doesn't increase linearly at the + // endpoints + assertNear(predictedTimeLeft, i / 100.0, 0.25); + reachedGoal = true; + } + } + } + + @Test + void timingToNegativeGoal() { + ExponentialProfile profile = new ExponentialProfile(constraints); + + ExponentialProfile.State goal = new ExponentialProfile.State(-2, 0); + ExponentialProfile.State state = new ExponentialProfile.State(0, 0); + + double predictedTimeLeft = profile.timeLeftUntil(state, goal); + boolean reachedGoal = false; + for (int i = 0; i < 400; i++) { + state = checkDynamics(profile, state, goal); + + if (!reachedGoal && state.equals(goal)) { + // Expected value using for loop index is just an approximation since + // the time left in the profile doesn't increase linearly at the + // endpoints + assertNear(predictedTimeLeft, i / 100.0, 0.25); + reachedGoal = true; + } + } + } +} diff --git a/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp b/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp new file mode 100644 index 0000000000..8df3aa1e4b --- /dev/null +++ b/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp @@ -0,0 +1,338 @@ +// 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/trajectory/ExponentialProfile.h" // NOLINT(build/include_order) + +#include +#include +#include +#include + +#include + +#include "frc/controller/SimpleMotorFeedforward.h" +#include "units/acceleration.h" +#include "units/frequency.h" +#include "units/length.h" +#include "units/math.h" +#include "units/velocity.h" +#include "units/voltage.h" + +static constexpr auto kDt = 10_ms; +static constexpr auto kV = 2.5629_V / 1_mps; +static constexpr auto kA = 0.43277_V / 1_mps_sq; + +#define EXPECT_NEAR_UNITS(val1, val2, eps) \ + EXPECT_LE(units::math::abs(val1 - val2), eps) + +#define EXPECT_LT_OR_NEAR_UNITS(val1, val2, eps) \ + if (val1 <= val2) { \ + EXPECT_LE(val1, val2); \ + } else { \ + EXPECT_NEAR_UNITS(val1, val2, eps); \ + } + +frc::ExponentialProfile::State CheckDynamics( + frc::ExponentialProfile profile, + frc::ExponentialProfile::Constraints + constraints, + frc::SimpleMotorFeedforward feedforward, + frc::ExponentialProfile::State current, + frc::ExponentialProfile::State goal) { + auto next = profile.Calculate(kDt, current, goal); + auto signal = feedforward.Calculate(current.velocity, next.velocity, kDt); + + EXPECT_LE(units::math::abs(signal), constraints.maxInput + 1e-9_V); + + return next; +} + +TEST(ExponentialProfileTest, ReachesGoal) { + frc::ExponentialProfile::Constraints constraints{ + 12_V, -kV / kA, 1 / kA}; + frc::ExponentialProfile profile{constraints}; + frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, + 0.43277_V / 1_mps_sq}; + frc::ExponentialProfile::State goal{10_m, 0_mps}; + frc::ExponentialProfile::State state{0_m, 0_mps}; + + for (int i = 0; i < 450; ++i) { + state = CheckDynamics(profile, constraints, feedforward, state, goal); + } + EXPECT_EQ(state, goal); +} + +// Tests that decreasing the maximum velocity in the middle when it is already +// moving faster than the new max is handled correctly +TEST(ExponentialProfileTest, PosContinousUnderVelChange) { + frc::ExponentialProfile::Constraints constraints{ + 12_V, -kV / kA, 1 / kA}; + frc::ExponentialProfile profile{constraints}; + frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, + 0.43277_V / 1_mps_sq}; + frc::ExponentialProfile::State goal{10_m, 0_mps}; + frc::ExponentialProfile::State state{0_m, 0_mps}; + + for (int i = 0; i < 300; ++i) { + if (i == 150) { + constraints.maxInput = 9_V; + profile = + frc::ExponentialProfile{constraints}; + } + + state = CheckDynamics(profile, constraints, feedforward, state, goal); + } + EXPECT_EQ(state, goal); +} + +// Tests that decreasing the maximum velocity in the middle when it is already +// moving faster than the new max is handled correctly +TEST(ExponentialProfileTest, PosContinousUnderVelChangeBackward) { + frc::ExponentialProfile::Constraints constraints{ + 12_V, -kV / kA, 1 / kA}; + frc::ExponentialProfile profile{constraints}; + frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, + 0.43277_V / 1_mps_sq}; + frc::ExponentialProfile::State goal{-10_m, 0_mps}; + frc::ExponentialProfile::State state{0_m, 0_mps}; + + for (int i = 0; i < 300; ++i) { + if (i == 150) { + constraints.maxInput = 9_V; + profile = + frc::ExponentialProfile{constraints}; + } + + state = CheckDynamics(profile, constraints, feedforward, state, goal); + } + EXPECT_EQ(state, goal); +} + +// There is some somewhat tricky code for dealing with going backwards +TEST(ExponentialProfileTest, Backwards) { + frc::ExponentialProfile::Constraints constraints{ + 12_V, -kV / kA, 1 / kA}; + frc::ExponentialProfile profile{constraints}; + frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, + 0.43277_V / 1_mps_sq}; + frc::ExponentialProfile::State goal{-10_m, 0_mps}; + frc::ExponentialProfile::State state; + + for (int i = 0; i < 400; ++i) { + state = CheckDynamics(profile, constraints, feedforward, state, goal); + } + EXPECT_EQ(state, goal); +} + +TEST(ExponentialProfileTest, SwitchGoalInMiddle) { + frc::ExponentialProfile::Constraints constraints{ + 12_V, -kV / kA, 1 / kA}; + frc::ExponentialProfile profile{constraints}; + frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, + 0.43277_V / 1_mps_sq}; + frc::ExponentialProfile::State goal{-10_m, 0_mps}; + frc::ExponentialProfile::State state{0_m, 0_mps}; + + for (int i = 0; i < 50; ++i) { + state = CheckDynamics(profile, constraints, feedforward, state, goal); + } + EXPECT_NE(state, goal); + + goal = {0.0_m, 0.0_mps}; + for (int i = 0; i < 100; ++i) { + state = CheckDynamics(profile, constraints, feedforward, state, goal); + } + EXPECT_EQ(state, goal); +} + +// Checks to make sure that it hits top speed on long trajectories +TEST(ExponentialProfileTest, TopSpeed) { + frc::ExponentialProfile::Constraints constraints{ + 12_V, -kV / kA, 1 / kA}; + frc::ExponentialProfile profile{constraints}; + frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, + 0.43277_V / 1_mps_sq}; + frc::ExponentialProfile::State goal{40_m, 0_mps}; + frc::ExponentialProfile::State state; + + units::meters_per_second_t maxSpeed = 0_mps; + + for (int i = 0; i < 900; ++i) { + state = CheckDynamics(profile, constraints, feedforward, state, goal); + maxSpeed = units::math::max(state.velocity, maxSpeed); + } + + EXPECT_NEAR_UNITS(constraints.MaxVelocity(), maxSpeed, 1e-5_mps); + EXPECT_EQ(state, goal); +} + +// Checks to make sure that it hits top speed on long trajectories +TEST(ExponentialProfileTest, TopSpeedBackward) { + frc::ExponentialProfile::Constraints constraints{ + 12_V, -kV / kA, 1 / kA}; + frc::ExponentialProfile profile{constraints}; + frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, + 0.43277_V / 1_mps_sq}; + frc::ExponentialProfile::State goal{-40_m, 0_mps}; + frc::ExponentialProfile::State state; + + units::meters_per_second_t maxSpeed = 0_mps; + + for (int i = 0; i < 900; ++i) { + state = CheckDynamics(profile, constraints, feedforward, state, goal); + maxSpeed = units::math::min(state.velocity, maxSpeed); + } + + EXPECT_NEAR_UNITS(-constraints.MaxVelocity(), maxSpeed, 1e-5_mps); + EXPECT_EQ(state, goal); +} + +// Checks to make sure that it hits top speed on long trajectories +TEST(ExponentialProfileTest, HighInitialSpeed) { + frc::ExponentialProfile::Constraints constraints{ + 12_V, -kV / kA, 1 / kA}; + frc::ExponentialProfile profile{constraints}; + frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, + 0.43277_V / 1_mps_sq}; + frc::ExponentialProfile::State goal{40_m, 0_mps}; + frc::ExponentialProfile::State state{0_m, 8_mps}; + + for (int i = 0; i < 900; ++i) { + state = CheckDynamics(profile, constraints, feedforward, state, goal); + } + + EXPECT_EQ(state, goal); +} + +// Checks to make sure that it hits top speed on long trajectories +TEST(ExponentialProfileTest, HighInitialSpeedBackward) { + frc::ExponentialProfile::Constraints constraints{ + 12_V, -kV / kA, 1 / kA}; + frc::ExponentialProfile profile{constraints}; + frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, + 0.43277_V / 1_mps_sq}; + frc::ExponentialProfile::State goal{-40_m, 0_mps}; + frc::ExponentialProfile::State state{0_m, -8_mps}; + + for (int i = 0; i < 900; ++i) { + state = CheckDynamics(profile, constraints, feedforward, state, goal); + } + + EXPECT_EQ(state, goal); +} + +TEST(ExponentialProfileTest, TestHeuristic) { + frc::ExponentialProfile::Constraints constraints{ + 12_V, -kV / kA, 1 / kA}; + frc::ExponentialProfile profile{constraints}; + std::vector::State, // initial + frc::ExponentialProfile::State, // goal + frc::ExponentialProfile::State> // inflection + // point + > + testCases{ + // red > green and purple => always positive => false + {{0_m, -4_mps}, {0.75_m, -4_mps}, {1.3758_m, 4.4304_mps}}, + {{0_m, -4_mps}, {1.4103_m, 4_mps}, {1.3758_m, 4.4304_mps}}, + {{0.6603_m, 4_mps}, {0.75_m, -4_mps}, {1.3758_m, 4.4304_mps}}, + {{0.6603_m, 4_mps}, {1.4103_m, 4_mps}, {1.3758_m, 4.4304_mps}}, + + // purple > red > green => positive if v0 < 0 => c && !d && a + {{0_m, -4_mps}, {0.5_m, -2_mps}, {0.4367_m, 3.7217_mps}}, + {{0_m, -4_mps}, {0.546_m, 2_mps}, {0.4367_m, 3.7217_mps}}, + {{0.6603_m, 4_mps}, {0.5_m, -2_mps}, {0.5560_m, -2.9616_mps}}, + {{0.6603_m, 4_mps}, {0.546_m, 2_mps}, {0.5560_m, -2.9616_mps}}, + + // red < green and purple => always negative => true => !c && !d + {{0_m, -4_mps}, {-0.75_m, -4_mps}, {-0.7156_m, -4.4304_mps}}, + {{0_m, -4_mps}, {-0.0897_m, 4_mps}, {-0.7156_m, -4.4304_mps}}, + {{0.6603_m, 4_mps}, {-0.75_m, -4_mps}, {-0.7156_m, -4.4304_mps}}, + {{0.6603_m, 4_mps}, {-0.0897_m, 4_mps}, {-0.7156_m, -4.4304_mps}}, + + // green > red > purple => positive if vf < 0 => !c && d && b + {{0_m, -4_mps}, {-0.5_m, -4.5_mps}, {1.095_m, 4.314_mps}}, + {{0_m, -4_mps}, {1.0795_m, 4.5_mps}, {-0.5122_m, -4.351_mps}}, + {{0.6603_m, 4_mps}, {-0.5_m, -4.5_mps}, {1.095_m, 4.314_mps}}, + {{0.6603_m, 4_mps}, {1.0795_m, 4.5_mps}, {-0.5122_m, -4.351_mps}}, + + // tests for initial velocity > V/kV + {{0_m, -8_mps}, {0_m, 0_mps}, {-0.1384_m, 3.342_mps}}, + {{0_m, -8_mps}, {-1_m, 0_mps}, {-0.562_m, -6.792_mps}}, + {{0_m, 8_mps}, {1_m, 0_mps}, {0.562_m, 6.792_mps}}, + {{0_m, 8_mps}, {-1_m, 0_mps}, {-0.785_m, -4.346_mps}}, + }; + + for (auto& testCase : testCases) { + auto state = profile.CalculateInflectionPoint(std::get<0>(testCase), + std::get<1>(testCase)); + EXPECT_NEAR_UNITS(std::get<2>(testCase).position / 1_m, + state.position / 1_m, 1e-3); + EXPECT_NEAR_UNITS(std::get<2>(testCase).velocity / 1_mps, + state.velocity / 1_mps, 1e-3); + } +} + +TEST(ExponentialProfileTest, TimingToCurrent) { + frc::ExponentialProfile::Constraints constraints{ + 12_V, -kV / kA, 1 / kA}; + frc::ExponentialProfile profile{constraints}; + frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, + 0.43277_V / 1_mps_sq}; + frc::ExponentialProfile::State goal{2_m, 0_mps}; + frc::ExponentialProfile::State state{0_m, 0_mps}; + + for (int i = 0; i < 900; ++i) { + state = CheckDynamics(profile, constraints, feedforward, state, goal); + EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state, state), 0_s, 2e-2_s); + } + + EXPECT_EQ(state, goal); +} + +TEST(ExponentialProfileTest, TimingToGoal) { + frc::ExponentialProfile::Constraints constraints{ + 12_V, -kV / kA, 1 / kA}; + frc::ExponentialProfile profile{constraints}; + frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, + 0.43277_V / 1_mps_sq}; + frc::ExponentialProfile::State goal{2_m, 0_mps}; + frc::ExponentialProfile::State state{0_m, 0_mps}; + + auto prediction = profile.TimeLeftUntil(state, goal); + auto reachedGoal = false; + + for (int i = 0; i < 900; ++i) { + state = CheckDynamics(profile, constraints, feedforward, state, goal); + if (!reachedGoal && state == goal) { + EXPECT_NEAR_UNITS(prediction, i * 0.01_s, 0.25_s); + reachedGoal = true; + } + } + + EXPECT_EQ(state, goal); +} + +TEST(ExponentialProfileTest, TimingToNegativeGoal) { + frc::ExponentialProfile::Constraints constraints{ + 12_V, -kV / kA, 1 / kA}; + frc::ExponentialProfile profile{constraints}; + frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, + 0.43277_V / 1_mps_sq}; + frc::ExponentialProfile::State goal{-2_m, 0_mps}; + frc::ExponentialProfile::State state{0_m, 0_mps}; + + auto prediction = profile.TimeLeftUntil(state, goal); + auto reachedGoal = false; + + for (int i = 0; i < 900; ++i) { + state = CheckDynamics(profile, constraints, feedforward, state, goal); + if (!reachedGoal && state == goal) { + EXPECT_NEAR_UNITS(prediction, i * 0.01_s, 0.25_s); + reachedGoal = true; + } + } + + EXPECT_EQ(state, goal); +}