Open
Conversation
calcmogul
reviewed
Jan 9, 2026
wpimath/algorithms.md
Outdated
|
|
||
| The fastest possible profile for the double integrator is one that applies the maximum allowed output in one direction and then the maximum allowed output in the other, also known as bang-bang. If there is an active maximum velocity constraint, this becomes bang-zero-bang. Notice that the plot of velocity versus time for this case resembles a trapezoid, thus the name. | ||
|
|
||
| The subscript m shall represent maximum and the subscript p shall denote peak. Note that each state has a position and a velocity, and the constraints on the profile include a maximum acceleration (`a_m`) and velocity (`v_m`). |
Contributor
There was a problem hiding this comment.
Suggested change
| The subscript m shall represent maximum and the subscript p shall denote peak. Note that each state has a position and a velocity, and the constraints on the profile include a maximum acceleration (`a_m`) and velocity (`v_m`). | |
| We will use maximum to refer to the greatest quantity allowed by the constraint (e.g., "maximum allowed velocity") and peak to refer to the greatest quantity achieved in the profile (e.g., "peak achieved velocity"). The subscript m shall represent maximum and the subscript p shall denote peak. Note that each state has a position and a velocity, and the constraints on the profile include a maximum acceleration (`a_m`) and velocity (`v_m`). |
Feel free to adjust the wording to make it clearer- I just wanted to make sure we define "maximum" and "peak" so that it's clear how they're different.
wpimath/algorithms.md
Outdated
Comment on lines
768
to
769
| x = x_i + (v_i * v - v_i²) / a + (v² + 2 * v * v_i + v_i²) / (2 * a) | ||
| (x - x_i) * (2 * a) = v² - v_i² |
Contributor
There was a problem hiding this comment.
Could you add some of the intermediate steps? Right now this is skipping a lot of them. (Move x_i to the other side, multiply both sides by 2 a, distribute the 2 for the v_i v - v_i² term, and combine like terms on the right hand side.)
wpimath/algorithms.md
Outdated
|
|
||
| ### Determining the sign of the profile. | ||
|
|
||
| For the purposes of this derivation, the sign (`s`) of the profile can be defined as the sign of the initial input or in relation to the calculated peak velocity of the profile in relation to the velocities of the initial and target states: when the value of the peak is at least the maximum of both velocities the sign would be positive, and when the peak is at most the minimum of both states, it would be negative. The optimal sign of the profile can be determined by looking at the distance covered by the shortest profile that can connect the initial and target velocity while respecting the acceleration constraint. A profile that takes more time than this with a positive sign will either increase the displacement of the profile, or has a faster profile in the other direction. Likewise, a profile with a negative sign will either decrease the displacement or has a faster profile with a different sign. This minimum profile takes the form of a straight line in the velocity versus time plot and has an acceleration equal to `sign(v_t - v_i) * a_m`. This threshold distance (`d`) is derived below. |
Contributor
There was a problem hiding this comment.
Right now, this reads a bit like a brain dump of many different thoughts regarding the sign of the profile, which is a great place to start when drafting, but is not a very easy-to-follow end result. Some suggestions:
- Distinguish between defining
sand describings. For example, instead of saying something like "the sign can be defined as A or B," say "the sign is defined as A, which can be described as B" or "the sign is defined as A. This can also be described as B". (The second option lets you split the different description into separate paragraphs, if that would better convey the structure of thought.) - Don't use "in relation to" twice in the same sentence with different meanings- My understanding is that the first one is similar to "in terms of" and the second one is similar to "compared to".
- Break the paragraph into separate sections. This makes it easier to follow the chain of thought.
- Use parallel structure for descriptions of parallel ideas. For example, make "A profile that takes more time than this with a positive sign" and "a profile with a negative sign" have the same structure, just with positive and negative (and increase/decrease, etc.) swapped. (This was something that took me a bit to get used to- For English class, it can be better to avoid dry repetition since it's more interesting, but in technical writing, using direct repetition is better since it conveys the information more clearly.)
| ``` | ||
| Recall that there can occasionally be two signs that will produce a valid profile (at least with the formulation that will be presented here). To understand this, imagine a profile where the initial and target velocities are below zero. A profile that has a positive sign and a time just a little bit over the minimum valid time will still end up with a negative displacement. Now consider if the profile had a negative sign. The same displacement would be possible to cover faster because the average velocity would be lower. Note that for the formulation discussed here, this ambiguity only arises when both the initial and target velocities have the same signs. | ||
|
|
||
| While comparing with the threshold distance will handle the majority of these cases, solely relying on it cannot handle the case where the initial and target states make the minimum profile. While it is not common for two random states to give rise to this, it is relatively common when profiles are being generated from a reference on the final segment of a profile. If floating point error causes the state to be slightly above or below the threshold distance, the sign is properly determined and the next reference is guaranteed to be correct (within floating point tolerances); however, in the case it is equal and the wrong sign is chosen, a reference for a new, longer profile may be generated. This can lead to choatic input sign changes and prevent the profile from coming to rest. This can be avoided by preferring the negative sign when both state velocities are below zero, and a positive sign otherwise. Because the scenario with different initial signs has one valid profile, meaning either sign will lead to valid solutions within floating point tolerance, this preference can be simplified to only check the sign of the target velocity. |
Contributor
There was a problem hiding this comment.
Could you explain why we can't choose to apply 0 acceleration when the initial and target velocities are equal?
Author
There was a problem hiding this comment.
I think this is explained by the beginning of the derivation where I state that the acceleration of the optimal profile has the form of bang-bang or bang-zero-bang and the part where I discuss the optimal sign. If the velocities have the same sign as the displacement, you could just apply no input and wait for the system to reach the required displacement. If you have a zero displacement, or the velocities are equal to the constraint times the sign of the displacement, then this is indeed optimal. In every other case, applying an acceleration can get you there faster, or in the case where the sign of the velocities and displacement is different, there must be an acceleration applied in order to achieve the desired displacement.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
3 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Closes #7917.
This PR rewrites the trapezoid profile implementations in WPILib to use a phase space formulation. In doing so, the API is updated to match the exponential profile implementations. The following changes to functionality have been made:
TimeLeftUntilmethod has been updated to take two states instead of a distance from the last state passed into theCalculatemethod. This functionality matches that of the exponential profile.The tests have been updated to check for these bugs and to reflect the API change. The algorithm used in the new implementation has been documented in algorithms.md.