allwpilib
allwpilib copied to clipboard
wpilibsuite
angle of module position delta in odometry?
https://github.com/wpilibsuite/allwpilib/blob/25ad6eafd5eeaf57ed25db2414a20838bf098399/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java#L283
it looks like the logic here is calculating the change in position corresponding to the change in module drive and steer state, but it is using the end angle. assuming a constant curvature (circular) path between start and end states, shouldn't the angle here be the average of the start and end angles, representing the chord between start and end points on the circle?
If I recall correctly, that code used endModule.angle - startModule.angle originally, which broke. endModule.angle was used after that because it was stable.
wait, do you really mean the difference in angle? i'm suggesting that the correct term is the average.
Yes, I meant difference. I was explaining the steps we went through when attempting to implement toTwist2d().
Twists are supposed to use an angle delta. After the twist is exponentiated, the resulting pose delta is added element-wise to the starting pose. Idk why it worked for diff drive but not swerve.
huh, ok. i'm super confused, i guess. the next thing that happens to that angle is that it is used to construct the state vector for the forward kinematics multiplication, which takes the sine and cosine of that angle. it doesn't seem like it's being handled as a twist to me. maybe there's a missing exp-and-add for each module?
The toTwist2d(SwerveModulePosition... moduleDeltas) implementation expects each swerve module position to contain the translation of each wheel in polar coordinates, based on matching the implementation to the derivation in the swerve drive kinematics section of Controls Engineering in FRC (Section 11.7, page 213).
In toTwist2d(SwerveDriveWheelPositions start, SwerveDriveWheelPositions end), we have access to the total distance traveled by each wheel at the start and end of the time period of interest, as well as the angle of each wheel at the start and end of the time period of interest. However, it's impossible to determine the exact translation each module experienced, since the path during the time period of interest has infinite possibilities, so we just have to make our best guess.
yeah, totally, i get it. i think i'm thinking of the case where the control is constant during each period, so the "best guess" is a constant-curvature path. but you're right that in the case where the control is not constant (e.g. it's an outboard motor controller running faster than the main loop) then you can't assume that. thanks!