holonomic controller - feedforward issue

[WinstonHChou]

          > If -- desired heading - means robot orientation and not direction of travel, the feedforward calculations appear to be incorrect. If the orientation is non-zero, this causes the feedforwards to be off.

Just some clarification of my understanding:

• The current pose contains the current robot orientation as well as X, Y location
• The trajectory state (Pose Ref) contains the desire X, Y, direction of travel, and linear velocity
• The desired heading contains the desired robot orientation.

I think that the feedforwards should then be:

• Xfeedforward = linear velocity * cosine( direction of travel - current actual orientation )
• Yfeedforward = linear velocity * sine( direction of travel - current actual orientation )

Perhaps for a pure feedforward, the desired orientation could be used instead of actual. (I think the actual values will provide better results.)

Hi @jsimpso81 , I happen to notice that in the previous version, X_ff = linear vel cos(angle_ref) = linear vel cos(Pose_Ref) Y_ff = linear vel sin(angle_ref) = linear vel sin(Pose_Ref) if you set [angle_ref = Pose_Ref], then the both FF would be correct. It means that Pose_Ref equals to path's direction of travel.

Thus, I think that X_ff =linear vel cos(Pose_Ref) Y_ff = linear vel sin(Pose_Ref) should be enough. Noticed that X_ff, Y_ff is with respect to Field-Relative Coordinates, you don't have minus current acutual orientation again when you have From_Field-Relative-Speeds.vi at the end of the calculation.

Originally posted by @WinstonHChou in https://github.com/jsimpso81/WPIlibMathLabVIEW/issues/172#issuecomment-1837624395

[jsimpso81]

Thanks. I was essentially applying the same correction twice. The newest release 3.08C contains the correction.