PathPlanner trajectory generated on-the-fly acts robot relative, despite being passed field relative feedback

Hello! I am using the PathPlanner library to generate trajectories on-the-fly. My trajectory currently has only two points, the first one being the robot’s current pose. I’m tracking the trajectory with HolonomicDriveController, and passing field-relative position feedback in calculate().
Despite this, the trajectory seems to be robot-relative: every time I enable in autonomous, the robot moves the same way regardless of it’s position. I am not resetting the odometry in between runs.

Why is this, and how can I solve it? Any help would be appreciated!

Here is a link to our GitHub repository:
https://github.com/HaMosad1657/MiniProject2022/tree/chassis/src/main/java/frc/robot
The said code is in commands/drive/FollowGeneratedTrajectoryCommand on branch chassis.
Thanks!

EDIT: The repository is private, so here it is in pasteBin - https://appp.me/QHgZhG

Your link is private, so we can’t see your code.

3 Likes

Does the robot drive field centric when in TeleOp? If not check to make sure your gyro is reading correctly. If so then check to make sure that you are not resetting it before you run your path.

Yes, the robot is field relative in teleop! I’m not resetting the gyro before running the path (I know for sure because it prints to the RioLog when the gyro resets, and it doesn’t in this case).

2 Likes

If you’re using the robot’s gyro as the start pose “heading” then the path will always go along that heading at the start, which will be a fixed robot-relative direction.

4 Likes

Sorry, unfortunately I’m not in charge of the repository so I can’t make it public. But here is the contents of the file:

package frc.robot.commands.drive;

import java.util.ArrayList;

import com.pathplanner.lib.PathConstraints;
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;
import com.pathplanner.lib.PathPoint;
import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;

import edu.wpi.first.math.controller.HolonomicDriveController;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.CommandBase;

import frc.robot.subsystems.chassis.DrivetrainSubsystem;
import frc.robot.subsystems.chassis.DrivetrainConstants;

/**
 * This command generates a trajectory in the code and follows it
 * (as opposed to getting the trajectory from a JSON file).
 */
public class FollowGeneratedTrajectoryCommand extends CommandBase {
	private DrivetrainSubsystem drivetrain;

	public FollowGeneratedTrajectoryCommand(DrivetrainSubsystem drivetrain) {
		this.drivetrain = drivetrain;
		this.addRequirements(this.drivetrain);

		/**
		 * This is an arraylist of PathPoints objects that are points the trajectory
		 * passes through. The start and end poses are mandatory, and you can also
		 * add additional waypoints in between.
		 */
		this.kTrajectoryWaypointsList = new ArrayList<PathPoint>();

		this.timer = new Timer();

		// The position tolerance in X, Y and angle for HolonomicDriveController
		// is represented in one Pose2d object.
		this.kPositionTolerance = new Pose2d(
				DrivetrainConstants.kPositionToleranceMetersX,
				DrivetrainConstants.kPositionToleranceMetersY,
				Rotation2d.fromDegrees(DrivetrainConstants.kPositionToleranceDegrees));
	}

	private Timer timer;
	private PathPlannerTrajectory trajectory1;

	private PIDController PIDControllerX, PIDControllerY;
	private ProfiledPIDController profiledPIDControllerAngle;

	private ArrayList<PathPoint> kTrajectoryWaypointsList;

	private HolonomicDriveController driveController;
	private Pose2d currentPose;
	private Trajectory.State currentPositionSetpoint;
	private PathPlannerState currentPathPlannerState;
	private Rotation2d currentAngleSetpoint;

	private Pose2d kPositionTolerance;

	@Override
	public void initialize() {
		// The trajectory is generated in initialize() instead of in the constructor
		// because the starting point is the robot's current position.

		PathConstraints trajectoryConstraints = new PathConstraints(
				DrivetrainConstants.kMaxChassisVelocityMPSAuto,
				DrivetrainConstants.kMaxChassisAccelMPSSquared);

		/**
		 * Each point has a:
		 * -- X and Y in meters as a Translation2d.
		 * -- A heading (aka direction of travel) as a Rotation2d.
		 * -- Optionally the orientation of the robot as a Rotation2d.
		 * -- Optionally a velocity override (aka specifying the velocity
		 * manually instead of PathPlanner calculating it) as a double.
		 */

		// Start point
		this.kTrajectoryWaypointsList.add(new PathPoint(
				this.drivetrain.getCurretnPose().getTranslation(),
				// Calculate the angle between the robot's current point and the next
				new Rotation2d(this.getAngleFromPoints(
						this.drivetrain.getCurretnPose().getX(),
						this.drivetrain.getCurretnPose().getY(),
						DrivetrainConstants.kTrajectoryEndPose_FieldRelativeXMeters,
						DrivetrainConstants.kTrajectoryEndPose_FieldRelativeYMeters)),
				this.drivetrain.getGyroRotation()));

		// Optional: add more points here

		this.kTrajectoryWaypointsList.add(new PathPoint(
				new Translation2d(
						DrivetrainConstants.kTrajectoryEndPose_FieldRelativeXMeters,
						DrivetrainConstants.kTrajectoryEndPose_FieldRelativeYMeters),
				// Calculate the angle between the robot's previous point and this one
				new Rotation2d(180 - this.getAngleFromPoints(
						this.drivetrain.getCurretnPose().getX(),
						this.drivetrain.getCurretnPose().getY(),
						DrivetrainConstants.kTrajectoryEndPose_FieldRelativeXMeters,
						DrivetrainConstants.kTrajectoryEndPose_FieldRelativeYMeters)),
				Rotation2d.fromDegrees(DrivetrainConstants.kTrajectoryEndAngle_FieldRelativeDegrees)));

		// Now create the trajectory...
		this.trajectory1 = PathPlanner.generatePath(
				trajectoryConstraints,
				this.kTrajectoryWaypointsList.get(0),
				this.kTrajectoryWaypointsList.get(1));
		DriverStation.reportError("trajectory successfully generated!", false);

		// The closed-loop controllers should start from scratch every time the command
		// starts, so they're initialised in initialize() instead of the command's
		// constructor.
		this.PIDControllerX = new PIDController(
				DrivetrainConstants.kXControllerP,
				DrivetrainConstants.kXControllerI,
				DrivetrainConstants.kXControllerD);

		this.PIDControllerY = new PIDController(
				DrivetrainConstants.kYControllerP,
				DrivetrainConstants.kYControllerI,
				DrivetrainConstants.kYControllerD);

		this.profiledPIDControllerAngle = new ProfiledPIDController(
				DrivetrainConstants.kAngleControllerP,
				DrivetrainConstants.kAngleControllerI,
				DrivetrainConstants.kAngleControllerD,
				new TrapezoidProfile.Constraints(
						DrivetrainConstants.kAngleControllerMaxVelocity,
						DrivetrainConstants.kAngleControllerMaxAccel));

		// Angle is measured on a circle, so the minimum and maximum values are
		// the same position in reality. Here the angle is measured in radians
		// (I think) so the min and max values are 0 and 2PI.
		this.profiledPIDControllerAngle.enableContinuousInput(0, Math.PI * 2);

		// HolonomicDriveController accepts 3 constructor parameters: two PID
		// controllers for X and Y, and a profiled PID controller (TrapezoidProfile)
		// for controlling the angle.
		this.driveController = new HolonomicDriveController(
				this.PIDControllerX,
				this.PIDControllerY,
				this.profiledPIDControllerAngle);

		this.driveController.setTolerance(this.kPositionTolerance);

		this.driveController.setEnabled(true);
		this.timer.reset();
		this.timer.start();
	}

	@Override
	public void execute() {
		// The trajectory has a position, angle, velocity and acceleration for each
		// point in time since start. This is represented in a Trajectory.State object.
		// We don't use this angle, instead we use the one from PathPlanner.
		this.currentPositionSetpoint = this.trajectory1.sample(this.timer.get() + 0.02);

		// basically the same thing, but with PathPlanner, so it has information about
		// the angle for a holonomic robot (which the WPILib trajectory doesn't).
		// This is possible because PathPlannerTrajectory extends the WPILib Trajectory,
		// and PathPlannerState extends the WPILib Trajectory.State.
		this.currentPathPlannerState = (PathPlannerState) this.trajectory1.sample(this.timer.get() + 0.02);
		this.currentAngleSetpoint = this.currentPathPlannerState.holonomicRotation;

		this.currentPose = this.drivetrain.getCurretnPose();

		// The HolonimicDriveController.calculate() method returns the desired
		// ChassisSpeeds in order to reach the current setpoints. This is then
		// passed to the DrivetrainSubsystem.drive() method.
		this.drivetrain.drive(
				this.driveController.calculate(
						this.currentPose,
						this.currentPositionSetpoint,
						this.currentAngleSetpoint),
				true);
	}

	@Override
	public void end(boolean interrupted) {
		this.timer.stop();
		this.timer.reset();
		this.driveController.setEnabled(false);
	}

	@Override
	public boolean isFinished() {
		// Returns true if the time the trajectory takes to drive
		// has passed, and driveController is at it's setpoint or
		// within the position tolerance for it.
		return (this.trajectory1.getTotalTimeSeconds() < this.timer.get()
				&& this.driveController.atReference());
	}

	private double getAngleFromPoints(double x1, double y1, double x2, double y2) {
		return Math.PI / 2 - Math.atan2(y2 - y1, x2 - x1);
	}
}
1 Like

@Bmongar I posted the file above. Sorry for the mess!

1 Like

Nothing is screaming wrong at me, I checked for the rotation in the heading that @Amicus1 mentioned. Have you considered PPHolonomicDriveController instead of WPILIB? That is all i have off the top of my head

Oh I didn’t know that existed, but I’ll definitely look into it. Thank you!

Oh yeah, if you’re using WPILIB HolonomicDriveController on a PathPlanner Trajectory, the robot’s always going to face along the direction of travel.

Actually not- I separated the position setpoint and the angle setpoint. The position setpoint is with a wpilib Trajectory.State, and the angle setpoint is derived from a PathPlannerState (it’s possible because the PathPlanner classes extend their equivalent WPILib classes).
You can see that in the body of execute() method.

The orientation of the robot isn’t a problem, it rotated very nicely to the angle I told it - Rather, my problem is that the robot makes the trajectory the same every time regardless of it’s position on the “field”. Aka, it’s robot-relative instead of field-relative.

P.S here is a link to the code on pastebin, which might be easier to read: https://appp.me/QHgZhG

You could try spining the robot during the trajectory.
If the robot drives straight, but with a spin, then you know you have a problem with geting the starting angle.
If the robot drives in a curve path, then you might have a problem in your adometry. This probably isn’t the case but I think it’s worth checking.
Does the “drive” method moves "field oriented"ly? because that might also be the issue.

1 Like

I don’t understand- why would the robot spinning be a problem, if I’m telling it to do so?

My odometry is alright, I had it printed to the shuffleboard and it looks pretty accurate (although I didn’t really measure it, just with the eyes).

The drive() method takes a ChassisSpeeds object, which is robot-relative. To my understanding that’s exactly what HolonomicDriveController.calculate() returns, but I may be wrong- next time I’m there, I’ll try to use it with the fromFieldRelativeSpeeds() method from class ChassisSpeeds and see what happens.

I am not sure what is wrong my guess would be somewhere they gyro isn’t being taken into account. The reason I say this is because we would start driving robot relative as when our gyro would go off line.

Are you writing this to make it work or to understand trajectory following specifically? IF you just want to make on the fly trajectories work I would recommend PPSwerveControllerCommand which uses PPHolonomicDriveController and understands Path planner trajectories and states. IF you really want to understand what is going on though I would recommend lots of different outputs of your angles in different stages, also maybe compare the logical differences between PPSwerveControllerCommand and your command.

Something we found very helpful when trying to implement trajectory following was to plot the generated paths in the Field2d element. Even just comparing what the trajectory is saying vs. what the robot is actually doing can be a useful diagnostic.

2 Likes

Oh this will be very helpful, thank you!!! I’ll try it tomorrow and update with new information.

Also, just making sure: PathPlanner uses meters, and the same coordinate system as WPILib, right? because I didn’t see it mentioned anywhere.

1 Like

I have always used meters as my units of measurement, but you need to make sure your units are consistent throughout.

I guess I’m writing this code for both reasons. I do need it to work in the end, but we have plenty of time and this is my team’s first time seriously trying path planning, so really understanding it would be cool.

I’d rather not use PPSweveControllerCommand because it outputs raw SwerveModuleStates, and we designed our drivetrain subsystem to take a ChassisSpeeds from commands. To make it work with SwerveModuleStates we would have to rewrite a big part of our subsystem specifically for that.

Also, I understand that PPHolonomicDriveController is a new addition? Where can I find the wiki for the beta versions (if there is one)? I found the class itself by clicking “go to type definition” in VSCode, but a wiki would be helpful if one exists.

You should not need to rewrite a large part of your drivetrain. ChassisSpeeds and arrays of swerveModuleStates are easily transformable to each other. I can’t tell you what object/method right now as I am at the service station waiting to drop my car off, but I know we have drivetrain code that accepts both types of inputs. You would just need to a new method that takes swervemodulestates and transforms it to ChassisSpeeds and calls your other method

Edit SwerveDriveKinematics.toChassisSpeeds will convert, so you can drive with one new method and not a substantial update

Oh good to know, thanks!

1 Like