Changing max acceleration in between trajectory waypoints

lborowy · November 27, 2021, 12:01am

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.

Amicus1 · November 27, 2021, 2:49am

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:

github.com

wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java#L216-L250


public Trajectory concatenate(Trajectory other) {
  // If this is a default constructed trajectory with no states, then we can
  // simply return the rhs trajectory.
  if (m_states.isEmpty()) {
    return other;
  }

  // Deep copy the current states.
  List<State> states =
      m_states.stream()
          .map(
              state ->
                  new State(
                      state.timeSeconds,
                      state.velocityMetersPerSecond,
                      state.accelerationMetersPerSecondSq,
                      state.poseMeters,
                      state.curvatureRadPerMeter))
          .collect(Collectors.toList());

This file has been truncated. show original

calcmogul · November 27, 2021, 3:08am

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.

lborowy · November 27, 2021, 3:43am

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?

Oblarg · November 27, 2021, 3:49am

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

lborowy · November 27, 2021, 3:59am

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!

system · November 27, 2022, 4:00am

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