Pathplanner Trajectory and setting Yaw

[Bmongar]

We are trying to get our trajectories with PathPlanner working. Trajectories aren’t quite right but that is not the question. I set my position to the initial position of the trajectory. And this was working close to right but one of the drivers wanted to start out facing the other way so the gyro would be correct for field oriented drive. I though, good point, I will set the yaw to the holonomic rotation so it will be correct, so I updated my resetPosition method to set the yaw on the pigeon and that was very bad. it moved opposite. So I won’t set it but why is would tat do it.

GitHub - FRC-5013-Park-Hill-Robotics/5013-RapidReact: 5013 bot for RapidReact

public void resetPosition(Pose2d newPosition, Rotation2d newRotation) {
        m_odometry.resetPosition(newPosition, newRotation);
        m_pose = m_odometry.getPoseMeters();
        m_pigeon.setYaw(newRotation.getDegrees());
    }

public static Command createRightSide3(RobotContainer container){
        DrivetrainSubsystem drivetrain = container.getDrivetrainSubsystem();
        ParallelCommandGroup startup = new ParallelCommandGroup(
                    new InstantCommand(container.getintake()::dropIntake),
                    new InstantCommand(container.getshooter()::spinUp),
                    new AutonomousTurnToTargetCommand(drivetrain, container.getshooterVision(),container.getshooter(), container.getturret(), container.getconveyor()::isAllianceBallNext)
        );
        PathPlannerTrajectory leg1Trajectory =  PathPlanner.loadPath("Right Leg1", DrivetrainGeometry.MAX_VELOCITY_METERS_PER_SECOND, DrivetrainGeometry.MAX_VELOCITY_METERS_PER_SECOND / .33);
        drivetrain.resetPosition(leg1Trajectory.getInitialPose(), leg1Trajectory.getInitialState().holonomicRotation);
        Command leg1 = createSwerveControllerCommand(leg1Trajectory, drivetrain);
        return  new SequentialCommandGroup(
                    startup,
                    new AutonomousFire(container.getshooter(), container.getconveyor()),
                    leg1,
                    new InstantCommand(container.getintake()::raiseIntake),
                    createTurnAndShoot(container)
        );
    }

I

[vargoose84]

Are you sure pigeon is ccw positive? When we do this for navx, we negate the angle before setting initial startup parameters.

[jdaming]

Pigeon is CCW positive NavX is not.

[Quake]

The odometry .resetPosition(Pose2d, Rotation2d) method should not take the new odometry rotation as the second parameter. The Rotation2d parameter there represents the current gyro angle-- the “new” rotation is supplied from the given Pose2d and an offset is calculated internally between the current gyro rotation and the pose’s rotation. This way you don’t have to reset the gyro angle, if you want the robot angle related to the field, just use the odometry pose.

To reset odometry with pathplanner this way, construct a pose with the trajectory’s initial pose translation and initial state holonomic rotation. Your odometry reset should happen right before you follow your first path in autonomous(.beforeStarting(Command))-- the way you have it right now might lead to problems as you reset odometry when the command is created in the method.

[Bmongar]

IT seems the problem stems from the fact that the Rotation in the Pose2d given my PathPlanner is not the same as the holonomic rotation given by pathplanner making an offset like you mentioned. I do want my gyro reset before drivers go so that the 0 is upfield and after execution of the auto it is backward. I can edit my trajectory so the heading and holonomic are the same. That makes sense for starting with swerve. You just have to make sure your facing pointer is pulled to the center of your bot in the ui so it doesn’t drive out and back before the next pose.

[Quake]

@Bmongar

the Rotation in the Pose2d given my PathPlanner is not the same as the holonomic rotation given by pathplanner making an offset like you mentioned

That’s not what I said, I said the odometry .resetPosition(Pose2d, Rotation2d) takes (Pose to reset to, Current gyro angle) and then the odometry calculates an offset between the given gyro angle and the desired new pose’s angle. This way, immediately getting the rotation of the odometry pose after resetting it will return the new pose’s rotation despite the gyro reading some other arbitrary angle, removing the need to reset the gyro. You should use the odometry angle over the gyro angle for things like turning to an angle on the field.

Given some Trajectory you know is a PathPlannerTrajectory, you can correctly reset including the holonomic rotation by finding

PathPlannerTrajectory pTrajectory = (PathPlannerTrajectory)trajectory;
PathPlannerState pInitialState = (PathPlannerState)pTrajectory.sample(0);
Pose2d initialPose = new Pose2d(
  trajectory.getInitialPose().getTranslation(),
  pInitialState.holonomicRotation
);

[Bmongar]

Odometry is horribly unreliable for angle over the course of the entire match especially for swerve field oriented drive, almost every implementation 8 gave see use the gyro for that. If I start my rotation, heading, and gyro all to the same angle everything works out and my gyro is good for the rest of the match.

[bruingineer]

Seconding that this is the way to reset your odometry based on a PathPlannerState. You have to create a new Pose2d object with the Translation from the initial pose and the holonomicRotation of the initial state (instead of the rotation that is part of the initial pose).

[Quake]

How is the odometry more unreliable than the gyro for rotation? The odometry is updated with the gyro angle. Any error with the odometry angle is due to the gyro. The only difference is the calculated offset to correctly place the odometry angle on the field. You don’t have to match your auto heading and holonomic rotation.