Make all pose estimator example projects use Field2d

[calcmogul]

The differential drive pose estimator example uses Field2d, but not the mecanum or swerve pose estimator examples.

[adamgarcia4]

Hey Tyler, first time contributing back to the WPILib main repo.

The DifferentialDrivePoseEstimator example defines both an m_fieldSim as well as an m_fieldApproximation.

m_fieldSim.setRobotPose(m_drivetrainSimulator.getPose()); m_fieldApproximation.setRobotPose(m_poseEstimator.getEstimatedPosition());

It seems to me, being that Swerve doesn't have a Drivetrain Sim (please correct me if I'm wrong), we will only need the m_fieldApproximation for the SwervePoseEstimator example.

Is this correct?

[adamgarcia4]

Additionally, running the simulation with my current code changes results in a Pose2d that "jitters" and doesn't respond to inputs.

Would you mind giving me a bit more direction on the matter?

[calcmogul]

The swerve sim "physics" can just be SwerveDriveKinematics. It would feed m_fieldSim as the pose ground truth. The pose estimator would feed m_fieldApproximation as a noisy approximation of ground truth.

[adamgarcia4]

Okay, so please bear with me, as I'm relatively new to FRC Programming.

If I'd like to work out the translational + rotational displacement of the robot wrt the field, I would need to:

Approximate the field displacement by integrating the ChassisSpeed (new ChassisSpeeds(xSpeed, ySpeed, rot)) by the dt (I guess I can assume as 20ms). Then I guess I could update the m_fieldSim directly from the drive method.

I'd get the dx, dy, and dTheta separately, and from there, I can construct the Pose2d with which to update the m_fieldSim.

Does this approach make sense?

[calcmogul]

Plug the position deltas into Twist2d, then pass that to Pose2d.exp() to update the pose.

The sim update would go in simulationPeriodic(). You can use the diff drive pose estimator as a guide, tho the parts specific to m_drivetrainSimulator will be different.

[adamgarcia4]

I had done the below, which gets the Field2d to update properly:

Transform2d delta = new Transform2d(
	new Translation2d(ySpeed * periodSeconds, xSpeed * periodSeconds),
	new Rotation2d()
);

m_fieldSim.setRobotPose(m_fieldSim.getRobotPose().transformBy(delta));
// m_fieldSim.setRobotPose(m_poseEstimator.getEstimatedPosition());

I see that you just mentioned to use a Twist2d to represent the position deltas.
Let me try this.

[calcmogul]

ySpeed being passed to the x component doesn't seem correct.

[adamgarcia4]

Even from reading the docs a month or two ago, I saw that `x` represents translation up/down, why `y` represents strafing. (This is why ChassisSpeeds for differential drive has y=0).

That made me think that I should update the y on the x-component of the Twist.

However, being that you noted this being weird, I've flipped the direction.

[calcmogul]

The +x direction is to the right on the Field2d, and +y is up. Heading is measured CCW+ from the x axis.

https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html

[adamgarcia4]

Ahh yes, the ol' Frame of Reference. This makes sense now.
How does my current implementation look for the m_fieldSim?

[adamgarcia4]

Last question before I call it a day and continue tomorrow:
Does the below look correct for updating the Approximation Field2d?

// m_fieldApproximation.setRobotPose(m_poseEstimator.getEstimatedPosition());

[calcmogul]

The m_fieldSim update looks fine at a glance.

The m_fieldApproximation also looks correct.

[adamgarcia4]

Great, I'll continue on it tomorrow.
This back/forth helped me quite substantially. Thanks!