We have set up the filed2d widget to display our robot position in the simulation. It shows the robot starting in the bottom left corner which agrees with the json file built in Pathweaver. but, in Pathweaver, our starting waypoint shows the robot starting at x=7.5, y=-22.5
Did we miss something?
The coordinate system of PathWeaver and Field2D are different
It looks as if what Pathweaver builds in the json is robot centric. While we plot the waypoints in Pathweaver as field centric. Do we need to transform the coordinates in the json to be field specific?
Yes, you (usually) want to transform your trajectories to correspond to your robot’s current position. WPILIB has a function that does this for you:
Pose2d bOrigin = m_drivetrain.getPose();
Trajectory bTrajectory = aTrajectory.relativeTo(bOrigin);
Note that this assumes your Trajectory is already based from the origin. If it starts from another point you may need to subtract the initial pose from the Trajectory, or simply move it relativeTo (0, 0, 0).
Also, you might want to initialize your robot’s pose at startup to know where it is positioned on the field. I call m_drivetrain.resetOdometry() with the intended starting pose so that the path following knows where you are, initially. This will make the robot icon appear in the correct spot in the Field2D visualizer, in your case ( x=7.5, y=-22.5) and (heading=0).
This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.