Problem w/ more than one RamseteCommand

Hi!
So my team created two Trajectory objects and added to a RamseteCommand and add it all to a SequentialCommandGroup. When just one RamseteCommand is running, it works just fine, but when I added both of the commands, the robot seems to go out of control.

Ex:

public class AutoExample extends SequentialCommandGroup {

  public AutoExample(TrajectoryBuilder trajectoryBuilder) {
    super.addCommands(
      trajectoryBuilder.buildTrajectory("auto_1"), //this returns a RamseteCommand
      trajectoryBuilder.buildTrajectory("auto_2")
    );
  }
}

I thought that maybe this happen because both of the commands use the drivetrain subsystem… So I was thinking that maybe adding “clearGroupedCommands” it would work… Something like this (but this does not seem right):

public class AutoExample extends SequentialCommandGroup {

  public AutoExample(TrajectoryBuilder trajectoryBuilder) {
    super.addCommands(
      trajectoryBuilder.buildTrajectory("auto_1") //this returns a RamseteCommand
    );
    super.clearGroupedCommands();
    super.addCommands(
      trajectoryBuilder.buildTrajectory("auto_2")
    );
  }
}

or maybe

public class AutoExample extends SequentialCommandGroup {

 public AutoExample(TrajectoryBuilder trajectoryBuilder) {
    super.addCommands(
      trajectoryBuilder.buildTrajectory("auto_1") //this returns a RamseteCommand
       .andThen(trajectoryBuilder.buildTrajectory("auto_2")
    );
  }
}

Any ideas of what we could do?

Can you link your entire code? Specifically the buildTrajectory

This can definitely be done with SequentialCommandGroup. I recommend that you load the paths first and then run them but I don’t think it is required. See our code.

Yes!

yes, the first thing we do is to load the paths, otherwise it would have a big delay.

so, I was searching and I actually think a found a solution… I saw that FIRST merged a pull request that has a new method to concatenate trajectories… but it was not released it yet. So I tried to copy it, but I didn’t test yet, so I’m not sure if it works. (GitHub - Team1772/frc-2021 at concatenate-example)
the last code was based on: [wpimath] Add methods to concatenate trajectories by prateekma · Pull Request #3139 · wpilibsuite/allwpilib · GitHub

It looks like you are resetting your odometry every time you are calling createRamsete instead of just once at the beginning.

I would also just have createRamsete return the command rather than calling it with a void return and then getting the variable that it set. Seems like more room for error.

Yeah, but I think that’s just like “architecture”… we decided to use a void method as createRamsete because we use the odometry… I think the odometry is being call just once, because the createRamsete is call just once to create the command… so I’m not sure that would make a difference

You missed the comment about resetting the odometry each time. I am guessing that is your problem. If you sequence 2 trajectories, then the robot will think it is starting at the first point of the 2nd one, and go crazy.

I see… But that wouldn’t happen for the second trajectory? Because when this happens, the robot doesn’t follow any Trajectory, not even the first one… Anyways, I’ll removed the odometry from the method. Thanks

Can you try to describe more what is happening here? Is it actually running a trajectory? Is it being commanded to a setpoint? Is it following? Can you inspect the trajectory that is running to see if it is actually what you think it is?

what happens if you do the same trajectory twice?

So, what I mean is something like: everytime something different happens, sometimes it goes toward and start spinning or just at the beginning start spinning, it’s always something different, but usually the robot gets speed really quickly…
It doesn’t seem to be following any Trajectory…
But like, the trajectories alone, they work, so I don’t think it’s a problem with them, just when I add a new one, seems like a conflict.

It works… Repeating doesn’t seem the problem

As someone who does a lot of customer support, I want to offer some helpful advice. Avoid using the phrases “it works” or “it doesn’t work” when describing an issue. Always say exactly what it is, or isn’t doing, and how that differs from your expectations.

I’m sorry, sometimes it’s hard to express myself since english it’s not my native language. I’ll try to be more clear next time.

1 Like

No worries. You will get better answers more quickly when you provide as much detail as you can.

I suspect that your problems are related to an odometry mismatch between your trajectories and your robot’s starting pose. The sample Ramsete Trajectory code takes a shortcut of setting the drivetrain odometry to match the initial pose from your trajectory. If you remove the code that does that, then the robot will try to drive to the beginning of your trajectory before it begins following the prescribed path.

1 Like

I’ll try to do that. Thanks

I have a different idea of what might be going on. I am wondering if the trajectory you are asking for is not there or misspelled or anything else. If so you would still get the old ramsete command (this is the reason I suggested directly returning the command instead of writing it to a local and then reading later) when you called getRamsete.

I don’t think it could be that, because if the trajectory is misspelled or is not there, the robot code receive a null pointer and then it crashes (I had this problem before hahaah)

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