# Changing max acceleration in between trajectory waypoints

TL;DR: How can I constrain max acceleration/velocity during a WPI Lib trajectory based on progress instead of position?

Hi! I’m the lead programmer for team 3018 and I’m working on implementing a easy-to-use path system based on the WPI Lib trajectory system. I’m using Java.

I have a question/feature request about the trajectory constraints available. I want to have the ability to change max speeds/acceleration in the middle of a trajectory. So at the start it might have max acceleration of 3 m/s, set in the `TrajectoryConfig`. Then I set it to go to waypoints A, then B, then C. Then, I would want to set to change the max acceleration value to 2 m/s, then go to waypoints D and E.

I would think that using a `TrajectoryConstraint` would be good here, except that it does not accept anything related to progress, only the current position.

The interface for TrajectoryConstraint looks like

``````public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
double velocityMetersPerSecond) {
``````

which is good, but what would be great was if there was an additional parameter `double time`. Then it could switch based on how far through the trajectory it is. The `RectangularRegionConstraint` doesn’t work because it applies to that area throughout the whole trajectory, not just after some time.

For example, a path might include driving straight forward, dropping off a heavy load, then driving back all in the same trajectory. A rectangular region would affect speed the whole time, when I just want it on the way back.

My current solution is basically to generate another trajectory while the robot is moving as soon as it hits the end of a certain acceleration area. So the robot begins following a new trajectory that starts right at its current position. I think this has problems however. One is the time taken to generate the new trajectory, but it’s probably OK since the doc says it will only take 25ms. A bigger issue is that it might magnify any current position errors.

So: Is hot-swapping Trajectories the best way, or is there some way to use constraints?

Sorry for the long post, I wanted to make sure to get out all the information that might be useful.

Thinking about this, and having to avoid region constraints, I think your best bet is to generate multiple trajectories that each are a different acceleration zone.
You can concatenate them post-generation using the method shown here:

1 Like

Yep, do this. The concatenate() function will be available for 2022. You can also set nonzero start and end velocities so you don’t have to stop between trajectory segments.

2 Likes

Is there a way to match the start velocity to the end velocity of the previous trajectory? So it would match what the trajectory would “want” to be? If I just set the one to end at x speed and the other to start at x speed, it may do unnecessary accelerations/slowdowns to match when it could just go at a more ideal speed.

Is there a way to figure out the speed that it would “naturally” be going at if it was all one trajectory?

Have you considered having the constraint be a stateful object? Just give it an accumulator and iterate on calculation.

That’s a good idea, but when I looked in the code the calculation will be called a variable number of times depending on how soon the trajectory can be forced into shape. It’s not just once per waypoint.

https://github.com/wpilibsuite/allwpilib/blob/82066946e5f72f30f1256a577cd7dfd01f42502c/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java#L91

I’ll think about some way to concatenate smoothly. Any suggestions are welcome!

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.