Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-05 03:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Refactor TrapezoidProfile API (#5457)

Browse Source
This commit is contained in:
Gold856
2023-07-19 20:25:10 -04:00
committed by GitHub
parent 72a4543493
commit 86e91e6724
24 changed files with 492 additions and 184 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
View File

@@ -10,6 +10,7 @@ import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Timer;
import java.util.function.Consumer;
import java.util.function.Supplier;
/**
* A command that runs a {@link TrapezoidProfile}. Useful for smoothly controlling mechanism motion.
@@ -19,7 +20,9 @@ import java.util.function.Consumer;
public class TrapezoidProfileCommand extends Command {
private final TrapezoidProfile m_profile;
private final Consumer<State> m_output;
private final Supplier<State> m_goal;
private final Supplier<State> m_currentState;
private final boolean m_newAPI; // TODO: Remove
private final Timer m_timer = new Timer();
/**
@@ -28,12 +31,42 @@ public class TrapezoidProfileCommand extends Command {
*
* @param profile The motion profile to execute.
* @param output The consumer for the profile output.
* @param goal The supplier for the desired state
* @param currentState The current state
* @param requirements The subsystems required by this command.
*/
public TrapezoidProfileCommand(
TrapezoidProfile profile,
Consumer<State> output,
Supplier<State> goal,
Supplier<State> currentState,
Subsystem... requirements) {
m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand");
m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand");
m_goal = goal;
m_currentState = currentState;
m_newAPI = true;
addRequirements(requirements);
}
/**
* Creates a new TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}.
* Output will be piped to the provided consumer function.
*
* @param profile The motion profile to execute.
* @param output The consumer for the profile output.
* @param requirements The subsystems required by this command.
* @deprecated The new constructor allows you to pass in a supplier for desired and current state.
* This allows you to change goals at runtime.
*/
@Deprecated(since = "2024", forRemoval = true)
public TrapezoidProfileCommand(
TrapezoidProfile profile, Consumer<State> output, Subsystem... requirements) {
m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand");
m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand");
m_newAPI = false;
m_goal = null;
m_currentState = null;
addRequirements(requirements);
}
@@ -43,8 +76,13 @@ public class TrapezoidProfileCommand extends Command {
}
@Override
@SuppressWarnings("removal")
public void execute() {
m_output.accept(m_profile.calculate(m_timer.get()));
if (m_newAPI) {
m_output.accept(m_profile.calculate(m_timer.get(), m_goal.get(), m_currentState.get()));
} else {
m_output.accept(m_profile.calculate(m_timer.get()));
}
}
@Override

8
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
View File

@@ -16,7 +16,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile;
*/
public abstract class TrapezoidProfileSubsystem extends Subsystem {
private final double m_period;
private final TrapezoidProfile.Constraints m_constraints;
private final TrapezoidProfile m_profile;
private TrapezoidProfile.State m_state;
private TrapezoidProfile.State m_goal;
@@ -33,7 +33,8 @@ public abstract class TrapezoidProfileSubsystem extends Subsystem {
*/
public TrapezoidProfileSubsystem(
TrapezoidProfile.Constraints constraints, double initialPosition, double period) {
m_constraints = requireNonNullParam(constraints, "constraints", "TrapezoidProfileSubsystem");
requireNonNullParam(constraints, "constraints", "TrapezoidProfileSubsystem");
m_profile = new TrapezoidProfile(constraints);
m_state = new TrapezoidProfile.State(initialPosition, 0);
setGoal(initialPosition);
m_period = period;
@@ -62,8 +63,7 @@ public abstract class TrapezoidProfileSubsystem extends Subsystem {
@Override
public void periodic() {
var profile = new TrapezoidProfile(m_constraints, m_goal, m_state);
m_state = profile.calculate(m_period);
m_state = m_profile.calculate(m_period, m_goal, m_state);
if (m_enabled) {
useState(m_state);
}

68
wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
View File

@@ -39,13 +39,44 @@ class TrapezoidProfileCommand
*
* @param profile The motion profile to execute.
* @param output The consumer for the profile output.
* @param goal The supplier for the desired state
* @param currentState The current state
* @param requirements The list of requirements.
*/
TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
std::function<void(State)> output,
std::function<State()> goal,
std::function<State()> currentState,
std::initializer_list<Subsystem*> requirements)
: m_profile(profile), m_output(output) {
: m_profile(profile),
m_output(output),
m_goal(goal),
m_currentState(currentState) {
this->AddRequirements(requirements);
m_newAPI = true;
}
/**
* Creates a new TrapezoidProfileCommand that will execute the given
* TrapezoidalProfile. Output will be piped to the provided consumer function.
*
* @param profile The motion profile to execute.
* @param output The consumer for the profile output.
* @param goal The supplier for the desired state
* @param currentState The current state
* @param requirements The list of requirements.
*/
TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
std::function<void(State)> output,
std::function<State()> goal,
std::function<State()> currentState,
std::span<Subsystem* const> requirements = {})
: m_profile(profile),
m_output(output),
m_goal(goal),
m_currentState(currentState) {
this->AddRequirements(requirements);
m_newAPI = true;
}
/**
@@ -55,17 +86,46 @@ class TrapezoidProfileCommand
* @param profile The motion profile to execute.
* @param output The consumer for the profile output.
* @param requirements The list of requirements.
* @deprecated The new constructor allows you to pass in a supplier for
* desired and current state. This allows you to change goals at runtime.
*/
WPI_DEPRECATED(
"The new constructor allows you to pass in a supplier for desired and "
"current state. This allows you to change goals at runtime.")
TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
std::function<void(State)> output,
std::initializer_list<Subsystem*> requirements)
: m_profile(profile), m_output(output) {
this->AddRequirements(requirements);
m_newAPI = false;
}
/**
* Creates a new TrapezoidProfileCommand that will execute the given
* TrapezoidalProfile. Output will be piped to the provided consumer function.
*
* @param profile The motion profile to execute.
* @param output The consumer for the profile output.
* @param requirements The list of requirements.
* @deprecated The new constructor allows you to pass in a supplier for
* desired and current state. This allows you to change goals at runtime.
*/
WPI_DEPRECATED(
"The new constructor allows you to pass in a supplier for desired and "
"current state. This allows you to change goals at runtime.")
TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
std::function<void(State)> output,
std::span<Subsystem* const> requirements = {})
: m_profile(profile), m_output(output) {
this->AddRequirements(requirements);
m_newAPI = false;
}
void Initialize() override { m_timer.Restart(); }
void Execute() override { m_output(m_profile.Calculate(m_timer.Get())); }
void Execute() override {
m_output(m_profile.Calculate(m_timer.Get(), m_goal(), m_currentState()));
}
void End(bool interrupted) override { m_timer.Stop(); }
@@ -76,7 +136,9 @@ class TrapezoidProfileCommand
private:
frc::TrapezoidProfile<Distance> m_profile;
std::function<void(State)> m_output;
std::function<State()> m_goal;
std::function<State()> m_currentState;
bool m_newAPI; // TODO: Remove
frc::Timer m_timer;
};

8
wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
View File

@@ -39,15 +39,13 @@ class TrapezoidProfileSubsystem : public Subsystem {
explicit TrapezoidProfileSubsystem(Constraints constraints,
Distance_t initialPosition = Distance_t{0},
units::second_t period = 20_ms)
: m_constraints(constraints),
: m_profile(constraints),
m_state{initialPosition, Velocity_t(0)},
m_goal{initialPosition, Velocity_t{0}},
m_period(period) {}
void Periodic() override {
auto profile =
frc::TrapezoidProfile<Distance>(m_constraints, m_goal, m_state);
m_state = profile.Calculate(m_period);
m_state = m_profile.Calculate(m_period, m_goal, m_state);
if (m_enabled) {
UseState(m_state);
}
@@ -87,7 +85,7 @@ class TrapezoidProfileSubsystem : public Subsystem {
void Disable() { m_enabled = false; }
private:
Constraints m_constraints;
frc::TrapezoidProfile<Distance> m_profile;
State m_state;
State m_goal;
units::second_t m_period;

25
wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp
View File

@@ -13,13 +13,18 @@
ReplaceMeTrapezoidProfileCommand::ReplaceMeTrapezoidProfileCommand()
: CommandHelper
// The profile to execute
(frc::TrapezoidProfile<units::meters>(
// The maximum velocity and acceleration of the profile
{5_mps, 5_mps_sq},
// The goal state of the profile
{10_m, 0_mps},
// The initial state of the profile
{0_m, 0_mps}),
[](frc::TrapezoidProfile<units::meters>::State state) {
// Use the computed intermediate trajectory state here
}) {}
(
frc::TrapezoidProfile<units::meters>(
// The maximum velocity and acceleration of the profile
{5_mps, 5_mps_sq}),
[](frc::TrapezoidProfile<units::meters>::State state) {
// Use the computed intermediate trajectory state here
},
// The goal state of the profile
[] {
return frc::TrapezoidProfile<units::meters>::State{10_m, 0_mps};
},
// The initial state of the profile
[] {
return frc::TrapezoidProfile<units::meters>::State{0_m, 0_mps};
}) {}

10
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp
View File

@@ -40,13 +40,17 @@ void RobotContainer::ConfigureButtonBindings() {
frc2::TrapezoidProfileCommand<units::meters>(
frc::TrapezoidProfile<units::meters>(
// Limit the max acceleration and velocity
{DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration},
// End at desired position in meters; implicitly starts at 0
{3_m, 0_mps}),
{DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration}),
// Pipe the profile state to the drive
[this](auto setpointState) {
m_drive.SetDriveStates(setpointState, setpointState);
},
// End at desired position in meters; implicitly starts at 0
[] {
return frc::TrapezoidProfile<units::meters>::State{3_m, 0_mps};
},
// Current position
[] { return frc::TrapezoidProfile<units::meters>::State{}; },
// Require the drive
{&m_drive})
// Convert to CommandPtr

9
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp
View File

@@ -13,13 +13,16 @@ DriveDistanceProfiled::DriveDistanceProfiled(units::meter_t distance,
: CommandHelper{
frc::TrapezoidProfile<units::meters>{
// Limit the max acceleration and velocity
{kMaxSpeed, kMaxAcceleration},
// End at desired position in meters; implicitly starts at 0
{distance, 0_mps}},
{kMaxSpeed, kMaxAcceleration}},
// Pipe the profile state to the drive
[drive](auto setpointState) {
drive->SetDriveStates(setpointState, setpointState);
},
// End at desired position in meters; implicitly starts at 0
[distance] {
return frc::TrapezoidProfile<units::meters>::State{distance, 0_mps};
},
[] { return frc::TrapezoidProfile<units::meters>::State{}; },
// Require the drive
{drive}} {
// Reset drive encoders since we're starting at 0

8
wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
View File

@@ -33,14 +33,12 @@ class Robot : public frc::TimedRobot {
}
// Create a motion profile with the given maximum velocity and maximum
// acceleration constraints for the next setpoint, the desired goal, and the
// current setpoint.
frc::TrapezoidProfile<units::meters> profile{m_constraints, m_goal,
m_setpoint};
// acceleration constraints for the next setpoint.
frc::TrapezoidProfile<units::meters> profile{m_constraints};
// Retrieve the profiled setpoint for the next timestep. This setpoint moves
// toward the goal while obeying the constraints.
m_setpoint = profile.Calculate(kDt);
m_setpoint = profile.Calculate(kDt, m_goal, m_setpoint);
// Send setpoint to offboard controller PID
m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,

8
wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp
View File

@@ -86,8 +86,8 @@ class Robot : public frc::TimedRobot {
frc::PWMSparkMax m_motor{kMotorPort};
frc::XboxController m_joystick{kJoystickPort};
frc::TrapezoidProfile<units::radians>::Constraints m_constraints{
45_deg_per_s, 90_deg_per_s / 1_s};
frc::TrapezoidProfile<units::radians> m_profile{
{45_deg_per_s, 90_deg_per_s / 1_s}};
frc::TrapezoidProfile<units::radians>::State m_lastProfiledReference;
@@ -117,9 +117,7 @@ class Robot : public frc::TimedRobot {
goal = {kLoweredPosition, 0_rad_per_s};
}
m_lastProfiledReference =
(frc::TrapezoidProfile<units::radians>(m_constraints, goal,
m_lastProfiledReference))
.Calculate(20_ms);
m_profile.Calculate(20_ms, goal, m_lastProfiledReference);
m_loop.SetNextR(frc::Vectord<2>{m_lastProfiledReference.position.value(),
m_lastProfiledReference.velocity.value()});

7
wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp
View File

@@ -86,8 +86,7 @@ class Robot : public frc::TimedRobot {
frc::PWMSparkMax m_motor{kMotorPort};
frc::XboxController m_joystick{kJoystickPort};
frc::TrapezoidProfile<units::meters>::Constraints m_constraints{3_fps,
6_fps_sq};
frc::TrapezoidProfile<units::meters> m_profile{{3_fps, 6_fps_sq}};
frc::TrapezoidProfile<units::meters>::State m_lastProfiledReference;
@@ -118,9 +117,7 @@ class Robot : public frc::TimedRobot {
goal = {kLoweredPosition, 0_fps};
}
m_lastProfiledReference =
(frc::TrapezoidProfile<units::meters>(m_constraints, goal,
m_lastProfiledReference))
.Calculate(20_ms);
m_profile.Calculate(20_ms, goal, m_lastProfiledReference);
m_loop.SetNextR(frc::Vectord<2>{m_lastProfiledReference.position.value(),
m_lastProfiledReference.velocity.value()});

12
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java
View File

@@ -17,13 +17,13 @@ public class ReplaceMeTrapezoidProfileCommand extends TrapezoidProfileCommand {
// The motion profile to be executed
new TrapezoidProfile(
// The motion profile constraints
new TrapezoidProfile.Constraints(0, 0),
// Goal state
new TrapezoidProfile.State(),
// Initial state
new TrapezoidProfile.State()),
new TrapezoidProfile.Constraints(0, 0)),
state -> {
// Use current trajectory state here
});
},
// Goal state
() -> new TrapezoidProfile.State(),
// Current state
() -> new TrapezoidProfile.State());
}
}

8
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
View File

@@ -72,11 +72,13 @@ public class RobotContainer {
// Limit the max acceleration and velocity
new TrapezoidProfile.Constraints(
DriveConstants.kMaxSpeedMetersPerSecond,
DriveConstants.kMaxAccelerationMetersPerSecondSquared),
// End at desired position in meters; implicitly starts at 0
new TrapezoidProfile.State(3, 0)),
DriveConstants.kMaxAccelerationMetersPerSecondSquared)),
// Pipe the profile state to the drive
setpointState -> m_robotDrive.setDriveStates(setpointState, setpointState),
// End at desired position in meters; implicitly starts at 0
() -> new TrapezoidProfile.State(3, 0),
// Current position
() -> new TrapezoidProfile.State(),
// Require the drive
m_robotDrive)
.beforeStarting(m_robotDrive::resetEncoders)

8
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java
View File

@@ -23,11 +23,13 @@ public class DriveDistanceProfiled extends TrapezoidProfileCommand {
// Limit the max acceleration and velocity
new TrapezoidProfile.Constraints(
DriveConstants.kMaxSpeedMetersPerSecond,
DriveConstants.kMaxAccelerationMetersPerSecondSquared),
// End at desired position in meters; implicitly starts at 0
new TrapezoidProfile.State(meters, 0)),
DriveConstants.kMaxAccelerationMetersPerSecondSquared)),
// Pipe the profile state to the drive
setpointState -> drive.setDriveStates(setpointState, setpointState),
// End at desired position in meters; implicitly starts at 0
() -> new TrapezoidProfile.State(meters, 0),
// Current position
() -> new TrapezoidProfile.State(),
// Require the drive
drive);
// Reset drive encoders since we're starting at 0

9
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
View File

@@ -33,17 +33,16 @@ public class Robot extends TimedRobot {
if (m_joystick.getRawButtonPressed(2)) {
m_goal = new TrapezoidProfile.State(5, 0);
} else if (m_joystick.getRawButtonPressed(3)) {
m_goal = new TrapezoidProfile.State(0, 0);
m_goal = new TrapezoidProfile.State();
}
// Create a motion profile with the given maximum velocity and maximum
// acceleration constraints for the next setpoint, the desired goal, and the
// current setpoint.
var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);
// acceleration constraints for the next setpoint.
var profile = new TrapezoidProfile(m_constraints);
// Retrieve the profiled setpoint for the next timestep. This setpoint moves
// toward the goal while obeying the constraints.
m_setpoint = profile.calculate(kDt);
m_setpoint = profile.calculate(kDt, m_goal, m_setpoint);
// Send setpoint to offboard controller PID
m_motor.setSetpoint(

13
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java
View File

@@ -41,10 +41,11 @@ public class Robot extends TimedRobot {
// the motors, this number should be greater than one.
private static final double kArmGearing = 10.0;
private final TrapezoidProfile.Constraints m_constraints =
new TrapezoidProfile.Constraints(
Units.degreesToRadians(45),
Units.degreesToRadians(90)); // Max arm speed and acceleration.
private final TrapezoidProfile m_profile =
new TrapezoidProfile(
new TrapezoidProfile.Constraints(
Units.degreesToRadians(45),
Units.degreesToRadians(90))); // Max arm speed and acceleration.
private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State();
// The plant holds a state-space model of our arm. This system has the following properties:
@@ -125,10 +126,8 @@ public class Robot extends TimedRobot {
goal = new TrapezoidProfile.State(kLoweredPosition, 0.0);
}
// Step our TrapezoidalProfile forward 20ms and set it as our next reference
m_lastProfiledReference =
(new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference)).calculate(0.020);
m_lastProfiledReference = m_profile.calculate(0.020, goal, m_lastProfiledReference);
m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity);
// Correct our Kalman filter's state vector estimate with encoder data.
m_loop.correct(VecBuilder.fill(m_encoder.getDistance()));

11
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java
View File

@@ -43,9 +43,11 @@ public class Robot extends TimedRobot {
// the motors, this number should be greater than one.
private static final double kElevatorGearing = 6.0;
private final TrapezoidProfile.Constraints m_constraints =
new TrapezoidProfile.Constraints(
Units.feetToMeters(3.0), Units.feetToMeters(6.0)); // Max elevator speed and acceleration.
private final TrapezoidProfile m_profile =
new TrapezoidProfile(
new TrapezoidProfile.Constraints(
Units.feetToMeters(3.0),
Units.feetToMeters(6.0))); // Max elevator speed and acceleration.
private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State();
/* The plant holds a state-space model of our elevator. This system has the following properties:
@@ -128,8 +130,7 @@ public class Robot extends TimedRobot {
goal = new TrapezoidProfile.State(kLowGoalPosition, 0.0);
}
// Step our TrapezoidalProfile forward 20ms and set it as our next reference
m_lastProfiledReference =
(new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference)).calculate(0.020);
m_lastProfiledReference = m_profile.calculate(0.020, goal, m_lastProfiledReference);
m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity);
// Correct our Kalman filter's state vector estimate with encoder data.

4
wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
View File

@@ -343,8 +343,8 @@ public class ProfiledPIDController implements Sendable {
m_setpoint.position = setpointMinDistance + measurement;
}
var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);
m_setpoint = profile.calculate(getPeriod());
var profile = new TrapezoidProfile(m_constraints);
m_setpoint = profile.calculate(getPeriod(), m_goal, m_setpoint);
return m_controller.calculate(measurement, m_setpoint.position);
}

126
wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
View File

@@ -22,14 +22,14 @@ import java.util.Objects;
* new TrapezoidProfile.Constraints(kMaxV, kMaxA);
* TrapezoidProfile.State previousProfiledReference =
* new TrapezoidProfile.State(initialReference, 0.0);
* TrapezoidProfile profile = new TrapezoidProfile(constraints);
* </code></pre>
*
* <p>Run on update:
*
* <pre><code>
* TrapezoidProfile profile =
* new TrapezoidProfile(constraints, unprofiledReference, previousProfiledReference);
* previousProfiledReference = profile.calculate(timeSincePreviousUpdate);
* previousProfiledReference =
* profile.calculate(timeSincePreviousUpdate, unprofiledReference, previousProfiledReference);
* </code></pre>
*
* <p>where `unprofiledReference` is free to change between calls. Note that when the unprofiled
@@ -42,9 +42,10 @@ public class TrapezoidProfile {
// The direction of the profile, either 1 for forwards or -1 for inverted
private int m_direction;
private Constraints m_constraints;
private State m_initial;
private State m_goal;
private final Constraints m_constraints;
private State m_current;
private State m_goal; // TODO: Remove
private final boolean m_newAPI; // TODO: Remove
private double m_endAccel;
private double m_endFullSpeed;
@@ -96,27 +97,40 @@ public class TrapezoidProfile {
}
}
/**
* Construct a TrapezoidProfile.
*
* @param constraints The constraints on the profile, like maximum velocity.
*/
public TrapezoidProfile(Constraints constraints) {
m_constraints = constraints;
m_newAPI = true;
}
/**
* Construct a TrapezoidProfile.
*
* @param constraints The constraints on the profile, like maximum velocity.
* @param goal The desired state when the profile is complete.
* @param initial The initial state (usually the current state).
* @deprecated Pass the desired and current state into calculate instead of constructing a new
* TrapezoidProfile with the desired and current state
*/
@Deprecated(since = "2024", forRemoval = true)
public TrapezoidProfile(Constraints constraints, State goal, State initial) {
m_direction = shouldFlipAcceleration(initial, goal) ? -1 : 1;
m_constraints = constraints;
m_initial = direct(initial);
m_current = direct(initial);
m_goal = direct(goal);
if (m_initial.velocity > m_constraints.maxVelocity) {
m_initial.velocity = m_constraints.maxVelocity;
m_newAPI = false;
if (m_current.velocity > m_constraints.maxVelocity) {
m_current.velocity = m_constraints.maxVelocity;
}
// Deal with a possibly truncated motion profile (with nonzero initial or
// final velocity) by calculating the parameters as if the profile began and
// ended at zero velocity
double cutoffBegin = m_initial.velocity / m_constraints.maxAcceleration;
double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration;
double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
double cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
@@ -126,7 +140,7 @@ public class TrapezoidProfile {
// of a truncated one
double fullTrapezoidDist =
cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
cutoffDistBegin + (m_goal.position - m_current.position) + cutoffDistEnd;
double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
double fullSpeedDist =
@@ -148,7 +162,10 @@ public class TrapezoidProfile {
*
* @param constraints The constraints on the profile, like maximum velocity.
* @param goal The desired state when the profile is complete.
* @deprecated Pass the desired and current state into calculate instead of constructing a new
* TrapezoidProfile with the desired and current state
*/
@Deprecated(since = "2024", forRemoval = true)
public TrapezoidProfile(Constraints constraints, State goal) {
this(constraints, goal, new State(0, 0));
}
@@ -159,17 +176,23 @@ public class TrapezoidProfile {
*
* @param t The time since the beginning of the profile.
* @return The position and velocity of the profile at time t.
* @deprecated Pass the desired and current state into calculate instead of constructing a new
* TrapezoidProfile with the desired and current state
*/
@Deprecated(since = "2024", forRemoval = true)
public State calculate(double t) {
State result = new State(m_initial.position, m_initial.velocity);
if (m_newAPI) {
throw new RuntimeException("Cannot use new constructor with deprecated calculate()");
}
State result = new State(m_current.position, m_current.velocity);
if (t < m_endAccel) {
result.velocity += t * m_constraints.maxAcceleration;
result.position += (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
} else if (t < m_endFullSpeed) {
result.velocity = m_constraints.maxVelocity;
result.position +=
(m_initial.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
(m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
+ m_constraints.maxVelocity * (t - m_endAccel);
} else if (t <= m_endDeccel) {
result.velocity = m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
@@ -184,6 +207,75 @@ public class TrapezoidProfile {
return direct(result);
}
/**
* Calculate the correct position and velocity for the profile at a time t where the beginning of
* the profile was at time t = 0.
*
* @param t The time since the beginning of the profile.
* @param goal The desired state when the profile is complete.
* @param current The current state.
* @return The position and velocity of the profile at time t.
*/
public State calculate(double t, State goal, State current) {
m_direction = shouldFlipAcceleration(current, goal) ? -1 : 1;
m_current = direct(current);
goal = direct(goal);
if (m_current.velocity > m_constraints.maxVelocity) {
m_current.velocity = m_constraints.maxVelocity;
}
// Deal with a possibly truncated motion profile (with nonzero initial or
// final velocity) by calculating the parameters as if the profile began and
// ended at zero velocity
double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration;
double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
double cutoffEnd = goal.velocity / m_constraints.maxAcceleration;
double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
// Now we can calculate the parameters as if it was a full trapezoid instead
// of a truncated one
double fullTrapezoidDist =
cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd;
double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
double fullSpeedDist =
fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration;
// Handle the case where the profile never reaches full speed
if (fullSpeedDist < 0) {
accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
fullSpeedDist = 0;
}
m_endAccel = accelerationTime - cutoffBegin;
m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
State result = new State(m_current.position, m_current.velocity);
if (t < m_endAccel) {
result.velocity += t * m_constraints.maxAcceleration;
result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
} else if (t < m_endFullSpeed) {
result.velocity = m_constraints.maxVelocity;
result.position +=
(m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
+ m_constraints.maxVelocity * (t - m_endAccel);
} else if (t <= m_endDeccel) {
result.velocity = goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
double timeLeft = m_endDeccel - t;
result.position =
goal.position
- (goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * timeLeft;
} else {
result = goal;
}
return direct(result);
}
/**
* Returns the time left until a target distance in the profile is reached.
*
@@ -191,8 +283,8 @@ public class TrapezoidProfile {
* @return The time left until a target distance in the profile is reached.
*/
public double timeLeftUntil(double target) {
double position = m_initial.position * m_direction;
double velocity = m_initial.velocity * m_direction;
double position = m_current.position * m_direction;
double velocity = m_current.velocity * m_direction;
double endAccel = m_endAccel * m_direction;
double endFullSpeed = m_endFullSpeed * m_direction - endAccel;

4
wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
View File

@@ -317,8 +317,8 @@ class ProfiledPIDController
m_setpoint.position = setpointMinDistance + measurement;
}
frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
m_setpoint = profile.Calculate(GetPeriod());
frc::TrapezoidProfile<Distance> profile{m_constraints};
m_setpoint = profile.Calculate(GetPeriod(), m_goal, m_setpoint);
return m_controller.Calculate(measurement.value(),
m_setpoint.position.value());
}

43
wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
View File

@@ -4,6 +4,8 @@
#pragma once
#include <wpi/deprecated.h>
#include "units/time.h"
#include "wpimath/MathShared.h"
@@ -21,13 +23,14 @@ namespace frc {
* @code{.cpp}
* TrapezoidProfile::Constraints constraints{kMaxV, kMaxA};
* double previousProfiledReference = initialReference;
* TrapezoidProfile profile{constraints};
* @endcode
*
* Run on update:
* @code{.cpp}
* TrapezoidProfile profile{constraints, unprofiledReference,
* previousProfiledReference};
* previousProfiledReference = profile.Calculate(timeSincePreviousUpdate);
* previousProfiledReference = profile.Calculate(timeSincePreviousUpdate,
* unprofiledReference,
* previousProfiledReference);
* @endcode
*
* where `unprofiledReference` is free to change between calls. Note that when
@@ -71,13 +74,26 @@ class TrapezoidProfile {
bool operator==(const State&) const = default;
};
/**
* Construct a TrapezoidProfile.
*
* @param constraints The constraints on the profile, like maximum velocity.
*/
TrapezoidProfile(Constraints constraints); // NOLINT
/**
* Construct a TrapezoidProfile.
*
* @param constraints The constraints on the profile, like maximum velocity.
* @param goal The desired state when the profile is complete.
* @param initial The initial state (usually the current state).
* @deprecated Pass the desired and current state into calculate instead of
* constructing a new TrapezoidProfile with the desired and current state
*/
WPI_DEPRECATED(
"Pass the desired and current state into calculate instead of "
"constructing a new TrapezoidProfile with the desired and current "
"state")
TrapezoidProfile(Constraints constraints, State goal,
State initial = State{Distance_t{0}, Velocity_t{0}});
@@ -91,9 +107,25 @@ class TrapezoidProfile {
* where the beginning of the profile was at time t = 0.
*
* @param t The time since the beginning of the profile.
* @deprecated Pass the desired and current state into calculate instead of
* constructing a new TrapezoidProfile with the desired and current state
*/
[[deprecated(
"Pass the desired and current state into calculate instead of "
"constructing a new TrapezoidProfile with the desired and current "
"state")]]
State Calculate(units::second_t t) const;
/**
* Calculate the correct position and velocity for the profile at a time t
* where the beginning of the profile was at time t = 0.
*
* @param t The time since the beginning of the profile.
* @param goal The desired state when the profile is complete.
* @param current The initial state (usually the current state).
*/
State Calculate(units::second_t t, State goal, State current);
/**
* Returns the time left until a target distance in the profile is reached.
*
@@ -141,8 +173,9 @@ class TrapezoidProfile {
int m_direction;
Constraints m_constraints;
State m_initial;
State m_goal;
State m_current;
State m_goal; // TODO: remove
bool m_newAPI; // TODO: remove
units::second_t m_endAccel;
units::second_t m_endFullSpeed;

102
wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
View File

@@ -10,22 +10,27 @@
#include "units/math.h"
namespace frc {
template <class Distance>
TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints)
: m_constraints(constraints), m_newAPI(true) {}
template <class Distance>
TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints,
State goal, State initial)
: m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1},
m_constraints(constraints),
m_initial(Direct(initial)),
m_goal(Direct(goal)) {
if (m_initial.velocity > m_constraints.maxVelocity) {
m_initial.velocity = m_constraints.maxVelocity;
m_current(Direct(initial)),
m_goal(Direct(goal)),
m_newAPI(false) {
if (m_current.velocity > m_constraints.maxVelocity) {
m_current.velocity = m_constraints.maxVelocity;
}
// Deal with a possibly truncated motion profile (with nonzero initial or
// final velocity) by calculating the parameters as if the profile began and
// ended at zero velocity
units::second_t cutoffBegin =
m_initial.velocity / m_constraints.maxAcceleration;
m_current.velocity / m_constraints.maxAcceleration;
Distance_t cutoffDistBegin =
cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
@@ -37,7 +42,7 @@ TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints,
// of a truncated one
Distance_t fullTrapezoidDist =
cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
cutoffDistBegin + (m_goal.position - m_current.position) + cutoffDistEnd;
units::second_t accelerationTime =
m_constraints.maxVelocity / m_constraints.maxAcceleration;
@@ -60,15 +65,19 @@ TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints,
template <class Distance>
typename TrapezoidProfile<Distance>::State
TrapezoidProfile<Distance>::Calculate(units::second_t t) const {
State result = m_initial;
if (m_newAPI) {
throw std::runtime_error(
"Cannot use new constructor with deprecated Calculate()");
}
State result = m_current;
if (t < m_endAccel) {
result.velocity += t * m_constraints.maxAcceleration;
result.position +=
(m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
(m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
} else if (t < m_endFullSpeed) {
result.velocity = m_constraints.maxVelocity;
result.position += (m_initial.velocity +
result.position += (m_current.velocity +
m_endAccel * m_constraints.maxAcceleration / 2.0) *
m_endAccel +
m_constraints.maxVelocity * (t - m_endAccel);
@@ -86,12 +95,83 @@ TrapezoidProfile<Distance>::Calculate(units::second_t t) const {
return Direct(result);
}
template <class Distance>
typename TrapezoidProfile<Distance>::State
TrapezoidProfile<Distance>::Calculate(units::second_t t, State goal,
State current) {
m_direction = ShouldFlipAcceleration(current, goal) ? -1 : 1;
m_current = Direct(current);
goal = Direct(goal);
if (m_current.velocity > m_constraints.maxVelocity) {
m_current.velocity = m_constraints.maxVelocity;
}
// Deal with a possibly truncated motion profile (with nonzero initial or
// final velocity) by calculating the parameters as if the profile began and
// ended at zero velocity
units::second_t cutoffBegin =
m_current.velocity / m_constraints.maxAcceleration;
Distance_t cutoffDistBegin =
cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
units::second_t cutoffEnd = goal.velocity / m_constraints.maxAcceleration;
Distance_t cutoffDistEnd =
cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
// Now we can calculate the parameters as if it was a full trapezoid instead
// of a truncated one
Distance_t fullTrapezoidDist =
cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd;
units::second_t accelerationTime =
m_constraints.maxVelocity / m_constraints.maxAcceleration;
Distance_t fullSpeedDist =
fullTrapezoidDist -
accelerationTime * accelerationTime * m_constraints.maxAcceleration;
// Handle the case where the profile never reaches full speed
if (fullSpeedDist < Distance_t{0}) {
accelerationTime =
units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
fullSpeedDist = Distance_t{0};
}
m_endAccel = accelerationTime - cutoffBegin;
m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
State result = m_current;
if (t < m_endAccel) {
result.velocity += t * m_constraints.maxAcceleration;
result.position +=
(m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
} else if (t < m_endFullSpeed) {
result.velocity = m_constraints.maxVelocity;
result.position += (m_current.velocity +
m_endAccel * m_constraints.maxAcceleration / 2.0) *
m_endAccel +
m_constraints.maxVelocity * (t - m_endAccel);
} else if (t <= m_endDeccel) {
result.velocity =
goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
units::second_t timeLeft = m_endDeccel - t;
result.position =
goal.position -
(goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) *
timeLeft;
} else {
result = goal;
}
return Direct(result);
}
template <class Distance>
units::second_t TrapezoidProfile<Distance>::TimeLeftUntil(
Distance_t target) const {
Distance_t position = m_initial.position * m_direction;
Velocity_t velocity = m_initial.velocity * m_direction;
Distance_t position = m_current.position * m_direction;
Velocity_t velocity = m_current.velocity * m_direction;
units::second_t endAccel = m_endAccel * m_direction;
units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;

8
wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
View File

@@ -58,12 +58,12 @@ class LinearSystemLoopTest {
TrapezoidProfile profile;
TrapezoidProfile.State state;
for (int i = 0; i < 1000; i++) {
profile =
new TrapezoidProfile(
constraints,
profile = new TrapezoidProfile(constraints);
state =
profile.calculate(
kDt,
new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)),
new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0)));
state = profile.calculate(kDt);
m_loop.setNextR(VecBuilder.fill(state.position, state.velocity));
updateTwoState(m_plant, m_loop, (random.nextGaussian()) * kPositionStddev);

66
wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
View File

@@ -57,9 +57,9 @@ class TrapezoidProfileTest {
TrapezoidProfile.State goal = new TrapezoidProfile.State(3, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
TrapezoidProfile profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 450; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
}
assertEquals(state, goal);
}
@@ -67,21 +67,21 @@ class TrapezoidProfileTest {
// Tests that decreasing the maximum velocity in the middle when it is already
// moving faster than the new max is handled correctly
@Test
void posContinousUnderVelChange() {
void posContinuousUnderVelChange() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(12, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
TrapezoidProfile profile = new TrapezoidProfile(constraints);
TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
double lastPos = state.position;
for (int i = 0; i < 1600; ++i) {
if (i == 400) {
constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
profile = new TrapezoidProfile(constraints);
}
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
double estimatedVel = (state.position - lastPos) / kDt;
if (i >= 400) {
@@ -105,9 +105,9 @@ class TrapezoidProfileTest {
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
TrapezoidProfile profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 400; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
}
assertEquals(state, goal);
}
@@ -118,16 +118,16 @@ class TrapezoidProfileTest {
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
TrapezoidProfile profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 200; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
}
assertNotEquals(state, goal);
goal = new TrapezoidProfile.State(0.0, 0.0);
profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 550; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
}
assertEquals(state, goal);
}
@@ -139,15 +139,15 @@ class TrapezoidProfileTest {
TrapezoidProfile.State goal = new TrapezoidProfile.State(4, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
TrapezoidProfile profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 200; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
}
assertNear(constraints.maxVelocity, state.velocity, 10e-5);
profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 2000; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
}
assertEquals(state, goal);
}
@@ -158,9 +158,9 @@ class TrapezoidProfileTest {
TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
TrapezoidProfile profile = new TrapezoidProfile(constraints);
for (int i = 0; i < 400; i++) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
assertNear(profile.timeLeftUntil(state.position), 0, 2e-2);
}
}
@@ -170,14 +170,13 @@ class TrapezoidProfileTest {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
TrapezoidProfile profile = new TrapezoidProfile(constraints);
TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
double predictedTimeLeft = profile.timeLeftUntil(goal.position);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
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
@@ -193,14 +192,13 @@ class TrapezoidProfileTest {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
TrapezoidProfile profile = new TrapezoidProfile(constraints);
TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
double predictedTimeLeft = profile.timeLeftUntil(1);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
if (!reachedGoal && Math.abs(state.velocity - 1) < 10e-5) {
assertNear(predictedTimeLeft, i / 100.0, 2e-2);
reachedGoal = true;
@@ -213,14 +211,13 @@ class TrapezoidProfileTest {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
TrapezoidProfile profile = new TrapezoidProfile(constraints);
TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
double predictedTimeLeft = profile.timeLeftUntil(goal.position);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
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
@@ -236,14 +233,13 @@ class TrapezoidProfileTest {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
TrapezoidProfile profile = new TrapezoidProfile(constraints);
TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State());
double predictedTimeLeft = profile.timeLeftUntil(-1);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
state = profile.calculate(kDt, goal, state);
if (!reachedGoal && Math.abs(state.velocity + 1) < 10e-5) {
assertNear(predictedTimeLeft, i / 100.0, 2e-2);
reachedGoal = true;

69
wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
View File

@@ -31,9 +31,9 @@ TEST(TrapezoidProfileTest, ReachesGoal) {
frc::TrapezoidProfile<units::meter>::State goal{3_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
frc::TrapezoidProfile<units::meter> profile{constraints};
for (int i = 0; i < 450; ++i) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
}
EXPECT_EQ(state, goal);
}
@@ -45,17 +45,18 @@ TEST(TrapezoidProfileTest, PosContinousUnderVelChange) {
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{12_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
frc::TrapezoidProfile<units::meter> profile{constraints};
auto state = profile.Calculate(kDt, goal,
frc::TrapezoidProfile<units::meter>::State{});
auto lastPos = state.position;
for (int i = 0; i < 1600; ++i) {
if (i == 400) {
constraints.maxVelocity = 0.75_mps;
profile = frc::TrapezoidProfile<units::meter>{constraints};
}
profile = frc::TrapezoidProfile<units::meter>{constraints, goal, state};
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
auto estimatedVel = (state.position - lastPos) / kDt;
if (i >= 400) {
@@ -79,9 +80,9 @@ TEST(TrapezoidProfileTest, Backwards) {
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
frc::TrapezoidProfile<units::meter> profile{constraints};
for (int i = 0; i < 400; ++i) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
}
EXPECT_EQ(state, goal);
}
@@ -92,16 +93,16 @@ TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
frc::TrapezoidProfile<units::meter> profile{constraints};
for (int i = 0; i < 200; ++i) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
}
EXPECT_NE(state, goal);
goal = {0.0_m, 0.0_mps};
profile = frc::TrapezoidProfile<units::meter>{constraints};
for (int i = 0; i < 550; ++i) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
}
EXPECT_EQ(state, goal);
}
@@ -113,15 +114,15 @@ TEST(TrapezoidProfileTest, TopSpeed) {
frc::TrapezoidProfile<units::meter>::State goal{4_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
frc::TrapezoidProfile<units::meter> profile{constraints};
for (int i = 0; i < 200; ++i) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
}
EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps);
profile = frc::TrapezoidProfile<units::meter>{constraints};
for (int i = 0; i < 2000; ++i) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
}
EXPECT_EQ(state, goal);
}
@@ -132,9 +133,9 @@ TEST(TrapezoidProfileTest, TimingToCurrent) {
frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
frc::TrapezoidProfile<units::meter> profile{constraints};
for (int i = 0; i < 400; i++) {
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
}
}
@@ -146,14 +147,14 @@ TEST(TrapezoidProfileTest, TimingToGoal) {
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
frc::TrapezoidProfile<units::meter> profile{constraints};
auto state = profile.Calculate(kDt, goal,
frc::TrapezoidProfile<units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
if (!reachedGoal && state == 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
@@ -170,14 +171,14 @@ TEST(TrapezoidProfileTest, TimingBeforeGoal) {
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
frc::TrapezoidProfile<units::meter> profile{constraints};
auto state = profile.Calculate(kDt, goal,
frc::TrapezoidProfile<units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
if (!reachedGoal &&
(units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
@@ -193,14 +194,14 @@ TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
frc::TrapezoidProfile<units::meter> profile{constraints};
auto state = profile.Calculate(kDt, goal,
frc::TrapezoidProfile<units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
if (!reachedGoal && state == 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
@@ -217,14 +218,14 @@ TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
frc::TrapezoidProfile<units::meter> profile{constraints};
auto state = profile.Calculate(kDt, goal,
frc::TrapezoidProfile<units::meter>::State{});
auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
state = profile.Calculate(kDt, goal, state);
if (!reachedGoal &&
(units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {
EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);