Issue on running multiple pathweaver trajectories in SequentialCommandGroup

[Brunoo1103]

Hey there! My team is using pathweaver to build trajectories. It’s works pretty and nice when we just need to run a path, however, we want stop the robot to shoot cargos and then continue the autonomous routine. We have tried to set this:

public class AutonomousTrajectoryBuilder extends SequentialCommandGroup {

  public AutonomousTrajectoryBuilder(Intake intake, Buffer buffer, Shooter shooter, TrajectoryBuilder trajectoryBuilder) {

    super.addCommands(

        //doesn't work two run() methods on a SequentialCommandGroup

        trajectoryBuilder.run("reverse"),

        new ShootAutonomous(3, intake, buffer, shooter),

        trajectoryBuilder.run("straight")

        //the parameter in method run() is the name of the pathweaver .json

    );

  }

}

But, when we run it, “reverse” path goes for a second straight and then go reverse. The “straight” goes a second reverse and then go straight". It works but it is not accurate. Looks like an error on odometry, but it didn’t find out.

Here is the class TrajectoryBuilder, which has all the logic to build our pathweaver trajectory.

Team1772/frc-2022/blob/pid-shooter/src/main/java/frc/core/util/TrajectoryBuilder.java

If someone help me to set multiple pathweaver paths on SequentialCommandGroup I would be pleasureeee

[jdaming]

So both the forward and reverse paths run fine if run separately?

You want to reset odometry before the first trajectory but I don’t understand why you are doing it after the trajectory

[Brunoo1103]

Hey thanks to help me! This reset odometry after the trajectory was a attempt to fix it, actually this code shouldn’t be there on the commit, so you may ignore this line.

With or without this line, multiple paths doesn’t work.

About running separately the paths: It works fine. I can also use the concatenation paths with multiple files names on run() method. However, if I pass two run() methods, it doesn’t work

[ksquare]

There could be two issues: When you simply ‘concatenate’ trajectories, the second one needs to start where the first one ends. You can try to work around that by ‘resetting’ the odometry, but sending a ‘set encoder to zero’ message via CAN might only have an effect a little later. I’ve tried to avoid that by using continuous trajectories, by first making the second one start where the first one ends like this:

    /** Re-locate and rotate a trajectory
     *  @param previous Previous trajectory
     *  @param trajectory Trajectory to translates
     *  @return Transformed trajectory where initial state is at end of previous trajectory
     */
    public static Trajectory continueTrajectory(final Trajectory previous, final Trajectory trajectory)
    {
        return makeTrajectoryStartAt(trajectory, getEndPose(previous));
    }

    /** @param trajectory Trajectory
     *  @return End state
     */
    public static State getEnd(final Trajectory trajectory)
    {
        final List<State> states = trajectory.getStates();
        return states.get(states.size()-1);
    }

    /** @param trajectory Trajectory
     *  @return End pose
     */
    public static Pose2d getEndPose(final Trajectory trajectory)
    {
        return getEnd(trajectory).poseMeters;
    }

    /** Re-locate and rotate a trajectory
     *  @param Original trajectory
     *  @param start Startpoint and heading
     *  @return Transformed trajectory where initial state is 'start'
     */
    public static Trajectory makeTrajectoryStartAt(final Trajectory trajectory, final Pose2d start)
    {
        // The relativeTo() methods in Pose2d and Trajectory are basically
        // a "substract" operation for X, Y and the rotation angle.
        // Assume a trajectory that moves from "5" to "6", and start is at "2".
        // The offset becomes "5" - "2" = "3"
        final Transform2d offset = start.minus(trajectory.getInitialPose());
        // Computing the "5 -> 6" trajectory relative to "3" becomes "2 -> 3".
        // So we get a trajectory that moves by "1" unit, starting at "2".
        return trajectory.transformBy(offset);
    }

[jdaming]

When trying to get help I highly recommend you leave the code as you expect us to run and debug it. When you don’t communicate that you are working in a branch, which branch, and that there are actually some changes that maybe should be made to it or not it massively confuses the troubleshooting process and we can’t inspect the code which is having the problem. Details matter! For example I can’t even find the AutonomousTrajectoryBuilder code snippet you posted above, currently it is just a build of the reverse path.

I highly discourage you to ever reset the odometry during a trajectory. You are essentially magically transporting the robot to a different location. It is better to make the trajectories flow together without any modification except for the reset at the beginning.

Instead load the trajectory at the beginning then just run the commands during. This is a pretty easy example to follow.

Why is your createRamsete returning void and privately storing the ramsete command. Instead return the command.

I have made a pull request to your repo adding simulation (so I could test things) and fixing the trajectory problems.

[Brunoo1103]

Hey jdaming, sorry for not responding earlier, we were making sure to test our robot to reply to you the right informations.

We applied your changes on your pull request, and it worked! We tested many times using simple foward and reverse autonomous trajectories, and used multiple paths that also worked.

We started to make an “official” autonomous for competition, and the multiple paths worked. So, we tried to run many times the same autonomous, like 5 times, and the robot did the same track 4 times. In the last attempt, we noticed a issue. On the last attempt, we just renamed a file, and clicked to “Build Path”. We deployed our code, and expected to run successfully as the other 4 attempts. But, we noticed that something occured. Our robot had a very different trajectory than before (We did not change anything on the trajectory in pathweaver). For example, it tried run in reverse and straight, but it was reverse with a curve. Looks like the left side motors receives more velocity.

Since 2 days ago we noticed that sometimes our autonomous robot starts to bug the trajectory. So, we have to delete all the current pathweaver configs folder on our root project and then start again a clean pathweaver project. Sometimes it works, and sometimes no.

We also tested all our sensors, like encoders, and the electric system seems ok. We tested the robot in teleoperated mode, and the robot runs straight, the encoders receives the right values.

When a path is not running ok, I noticed that pathweaver projects .json has a very strange info on the final of trajectory

this print is for foward.path

this print is for foward.wpilib.json

the curvature attribute has an bizarre value. Maybe it shouldn`t have this value.

OUR REPOSITORY (branch pid-shooter): GitHub - Team1772/frc-2022 at pid-shooter

A PATHWEAVER JSON THAT IS A BUGGED TRAJECTORY: https://github.com/Team1772/frc-2022/blob/pid-shooter/src/main/deploy/paths/output/exitTarmac1.wpilib.json

RAMSETE COMMAND (maybe this is not the problem): https://github.com/Team1772/frc-2022/blob/pid-shooter/src/main/java/frc/core/util/TrajectoryBuilder.java