Pathplanner on the fly trajectories

[Bmongar]

We use pathplanner for auto and it works fine.

We are trying to do a simple on the fly / tele-op trajectory with our swerve drive. Straight line from where we are now to a specific location.

We output to Dashboard outputs reasonable pose data for where we go,

So what happens is when we run the code we go right and then curve around and go towards where we should go but then keep going, the trajectory doesn’t stop. It is odd because we only have 2 points in our trajectory but it seems to do more.

So the command is mapped like this

		new Trigger(m_controller::getLeftTriggerButton)
			.whileTrue(m_drivetrainSubsystem.doubleSubstation());

and the code for the command is

	public Command doubleSubstation(){
				// More complex path with holonomic rotation. Non-zero starting velocity of 2 m/s. Max velocity of 4 m/s and max accel of 3 m/s^2
		PathPlannerTrajectory traj = PathPlanner.generatePath(
    		new PathConstraints(2, 3), 
    		new PathPoint(getPose().getTranslation(),getYawR2d(), getYawR2d(), 0.0),
			new PathPoint(new Translation2d(FieldTrajectoryConstants.fieldLengthMeters - 1.2, FieldTrajectoryConstants.fieldWidthMeters - 0.6), Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), 0.0)
 		);

		//FieldTrajectoryConstants.fieldLengthMeters - 1.2. FieldTrajectoryConstants.fieldWidthMeters - 0.6
			return new PPSwerveControllerCommand(
					 traj, 
					 this::getPose, // Pose supplier
					 this.m_kinematics, // SwerveDriveKinematics
					 new PIDController(TranslationGains.kP, TranslationGains.kI, TranslationGains.kD),
					 new PIDController(TranslationGains.kP, TranslationGains.kI, TranslationGains.kD),
					 new PIDController(ThetaGains.kP, ThetaGains.kI, ThetaGains.kD),		 this::setDesiredStates, // Module states consumer
					 true, // Should the path be automatically mirrored depending on alliance color. Optional, defaults to true
					 this // Requires this drive subsystem
			 );
	}

So what do we need to do to get this right?

[anon37142155]

Have you tried letting the trajectory run till it finishes and seeing what point it decides to stop at? Or does the trajectory just keep going forever?

[Bmongar]

Just gave the bot back to mechanical, but the driver would let off the command when it crossed out of the field

[veg]

I am trying to remember if it is necessary to zero your motor speeds when an auto-trajectory command terminates.

[celttechie]

I am NOT an expert on this, but I think I’m seeing something similar. I’m suspecting that my issue is related to the heading the controller thinks the robot has. If it thinks the robot is traveling in a different direction than straight to the next waypoint, maybe it tries to calculate a curve to get back around to the heading it needs, or the waypoint has a heading that requires moving to the point from a different direction.

[anon37142155]

I believe you are sort of right. IIRC the robot will try to smoothly transition in to the heading that is demanded at every path point, which sometimes means going around in a curve.

[sam.mulvey747]

Have you tried using PPServer to visualize the issue?

[Ben_Griffin]

You set the useAllianceColor to true, which will flip the x values on the trajectory and expect you to use a close origin if you are on red alliance. It is recommended to set useAlllianceColor to false for on the fly trajectories. However, you may need to do your own translation of the target PathPoint depending on your alliance color.

[Bmongar]

Yes, we have already made that fix. We will get the bot again today to test, but it seems the main issue was the creation timing. IT was always using the initial bot pose because that is what was called in construction. We changed to a proxyCommand with a function pointer to the new command constructor and that should give us the current pose.

[arnavthak]

For something like this, I found that using PathPlanner trajectories was a bit annoying to set up. We had some pose and heading issues that were quite annoying to debug. We ended up just using profiled PID controllers for the X, Y, and Theta to drive to a specific pose without a path planner command. Especially if you don’t want to do any complicated paths, it works just as effectively.