compute
abstract fun compute(targetPose: Pose2dDual<Time>, actualPose: Pose2d, actualVelActual: PoseVelocity2d): PoseVelocity2dDual<Time>
Computes the velocity and acceleration command. The frame Target is the reference robot, and the frame Actual is the measured, physical robot.
Return
velocity command in the actual frame