Changing max acceleration in between trajectory waypoints

Technical Programming
,
#1

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 curvatureRadPerMeter,
                                                 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.

#2

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
#3

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.

1 Like
#4

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?

#5

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

#6

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!