Following trajectories in reverse

Technical Java
,
1

Here is the function we are using to generate trajectories:

public final static Trajectory generateTrajectory(Pose2d start, List<Translation2d> waypoints, Pose2d end, boolean reversed, double maxVelocity, double maxAccel, double startVelocity, double endVelocity, double maxVoltage) {
    DifferentialDriveVoltageConstraint autoVoltageConstraint = new DifferentialDriveVoltageConstraint(new SimpleMotorFeedforward(Constants.ks, Constants.kv, Constants.ka), Constants.kDriveKinematics, maxVoltage);
    TrajectoryConfig config = new TrajectoryConfig(maxVelocity, maxAccel).setKinematics(Constants.kDriveKinematics).addConstraint(autoVoltageConstraint);
    config.setStartVelocity(startVelocity);
    config.setEndVelocity(endVelocity);
    config.setReversed(reversed);
    return TrajectoryGenerator.generateTrajectory(start, waypoints, end, config);
}

This Trajectory works well:

generateTrajectory(
  new Pose2d(0,0, new Rotation2d(0)),
  List.of(new Translation2d(1,0.1), new Translation2d(2,1)),
  new Pose2d(3,0, new Rotation2d(0)),
  false, 2, 1, 0, 0, 5
  ))

However, when we try to make a trajectory for the robot to drive backward, it fails.

TrajectoryHelper.generateTrajectory(new Pose2d(3,0, new Rotation2d(0)), 
List.of(new Translation2d(2,1), new Translation2d(1,1)),
new Pose2d(0,1, new Rotation2d(0)),
true, 2, 1, 0, 0, 5))

The error we get is “The trajectory’s minimum velocity was greater than its maximum velocity.”

How do we follow trajectories backward? What are we doing wrong?

2

Try adding TrajectoryConfig.setReversed(true)

3

Sorry, just noticed you do call setReversed… Hmm…

4

Try changing the signs of your waypoints. Driving “forward”, you would go from for example x=0 to x=1. Driving in reverse, you need to go from x=0 to x=-1.

6

But I’m going from (3,0) to (0,0), which is a negative movement. Shouldn’t it work?
Is this my fault or is this a bug?

7

It doesn’t know if you want to back up to make that movement or drive around in a circle to make that movement. setReversed lets it know that you want the robot to drive backwards to get to the destination rather than only driving forwards to get to the destination.

8

I tell it the trajectory is reversed, but it crashes anyway.

9

We need a link to your complete code to really debug this. Let us know exactly what you are doing to reproduce the issue.

10

code.zip (81.1 MB)
Here is our code. The trajectory is created in RobotContainer and making the command is handled by our trajectory helper class.

11

That is 81 MB zipped what is in there?

I highly suggest you use github or similar.

12

Try instead going from (0,0) to (-3, 0).

13

Okay so I put them on github, but it won’t let me upload the build folder due to file count restrictions. I think the src folder is enough though.

14

That would require us to zero our odometry at each reversal. The goal is to drive forward to pick up balls and then drive backward to line up to score them. I’ll give it a try next time I have access to the robot, but it seems like a workaround for something that should work anyway.

15

You createTrajectoryCommand is reseting the odometry for every new command it looks like twice.

16

Yes, but it resets it to the initial pose of the trajectory, not to 0,0. Is this causing the issue with the trajectory generation? Should I try removing it?
Edit: Oh, about it doing that twice, that is a good point. We had issues with not being zeroed when the trajectory started so I set it to do that at run time, but then never removed the other one.

17

You set your odometry at the very beginning and then you don’t change it from there. When you change your odometry your robot thinks it has just “warped” to a new place.

Issue with trajectory generation? I run your robot in simulation and I don’t see any errors.

18

That’s possible?

Yesterday when I ran it it threw an error. The error was something along the lines of “the min acceleration is greater than the max acceleration”, and it resulted in robot code crashing. I guess I’ll check again tonight and try to develop a better understanding of which trajectories trigger the error and which ones don’t and update this thread as needed.

19

Simulation - You need to do a little work to get it actually showing the robot but I HIGHLY recommend all teams do simulation so that they can test their robots. At a minimum you can debug the code and check for errors.

We would really need:

  1. The code you were running.
  2. The exact text of the complete error message to try and figure out what that is / was.
1 Like
20

If you’re still having problems, you can try writing two different TrajectoryConfig objects. One for going forward and one for going reverse. You can have a look at our Infinite Recharge robot’s code here. You can see that there are two config objects on line 43 and 52.

21

We aren’t having issues anymore, but thank you very much your help anyway!