Wheel forceAtCarpet math appears to be incorrect

[bhall-ctre]

From what I can tell, the math used to calculate the wheel force feedforwards here (and in the other languages) seems to be incorrect: https://github.com/mjansen4857/pathplanner/blob/64e43f803a1ce48b21aa1a8fe382c6ef66ac01d0/pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java#L330-L331 https://github.com/mjansen4857/pathplanner/blob/64e43f803a1ce48b21aa1a8fe382c6ef66ac01d0/pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java#L353-L362

1. Unlike going from robot angular velocity to module velocities (which is v=ωr), going from robot torque to wheel forces is F=𝜏/r, so using the regular SwerveDriveKinematics directly shouldn't give the correct force output for rotational motion. Instead, I think you can use a SwerveDriveKinematics constructed using the reciprocal of all the swerve module locations, although I haven't tested that.
2. Unlike velocity, force is additive. As an example, if the robot requires a force vector of <8, 0> N and no torque, then each of the four modules should only need to output <2, 0> N.
3. The force that the motor needs to exert through the wheel is not the total magnitude of the force vector. Assuming no wheel slip, the component of the force vector perpendicular to the wheel is centripetal force covered by static friction. As a result, only the component of the force vector in the direction of the wheel's velocity vector needs to be applied. In other words (ignoring the retStates flipping logic):

   forceAtCarpet = wheelForces[m].speedMetersPerSecond * wheelForces[m].angle.minus(retStates[m].angle).getCos();

   This should also cover direction of the force (no need to manually flip the sign).

[mjansen4857]

Yeah, you're right. I basically thought "surely it can't be this easy" then never got to testing or looking further into it. I'll give the ff calculations a redo.

Edit: Although I believe for point 3, friction forces would not be included in any of those wheel forces. The chassis forces are calculated from wheel accelerations, and then once the chassis forces get split into wheel forces, it won't be picking up any additional force covered by friction. Not sure if the angles of those forces states would match up with the actual module states though, all that FF stuff just generally needs more work/testing.

[bhall-ctre]

Although I believe for point 3, friction forces would not be included in any of those wheel forces. The chassis forces are calculated from wheel accelerations, and then once the chassis forces get split into wheel forces, it won't be picking up any additional force covered by friction. Not sure if the angles of those forces states would match up with the actual module states though, all that FF stuff just generally needs more work/testing.

Are chassis forces derived from individual modules, or from the acceleration of the whole robot? To me it looks more like the latter, the code calculates the robot acceleration vector and converts it to a robot force vector (although it needs to calculate <(vx - prevVx) / dt, (vy - prevVy) / dt> instead of (linearVel - prevLinearVel) / dt at the old heading). And that is then combined with torque to get wheel force vectors. In which case the total force on the robot (and by extension the wheels) does include friction, as friction is ultimately what allows the robot to change direction through centripetal force.

Edit: Just as an example of how you could write the Java code for the Chassis forces to pick up all forces:

    double prevLinearVelX = prevSetpoint.robotRelativeSpeeds().vxMetersPerSecond;
    double prevLinearVelY = prevSetpoint.robotRelativeSpeeds().vyMetersPerSecond;
    double linearAccelX = (retSpeeds.vxMetersPerSecond - prevLinearVelX) / dt;
    double linearAccelY = (retSpeeds.vyMetersPerSecond - prevLinearVelY) / dt;
    double linearForceX = linearAccelX * config.massKG;
    double linearForceY = linearAccelY * config.massKG;

    double angularAccel =
        (retSpeeds.omegaRadiansPerSecond - prevSetpoint.robotRelativeSpeeds().omegaRadiansPerSecond)
            / dt;
    double angTorque = angularAccel * config.MOI;

    ChassisSpeeds chassisForces = new ChassisSpeeds(linearForceX, linearForceY, angTorque);

Basically start at the desired whole-robot acceleration vector instead of linear acceleration, then figure out the whole-robot force vector and torque necessary to achieve that acceleration. After that, you can figure out the net force vectors on all the modules to achieve the chassis forces, then pull out the force component aligned with the motor.

[mjansen4857]

Ah ok I see now. I was under the impression that since robot acceleration is calculated with linear vel that wouldn't include friction since if it maintains a constant speed the accel would be 0 while direction changes. But, using the heading to create a force vector would pick up changes from friction.

[mjansen4857]

Ok I think I got it figured out (for java at least) in b1164034321d56c7237d5dac49be2b9fa7975971

I basically just pulled the inverse kinematics stuff out of SwerveDriveKinematics (using 1 / module location X/Y) to make a function to convert the chassis force vector to module force vectors. I still need to test it but it seems much more correct this time around. Thanks for the help!