compute
open override fun compute(targetPose: Pose2dDual<Time>, actualPose: Pose2d, actualVelActual: PoseVelocity2d): PoseVelocity2dDual<Time>
fun compute(s: DualNum<Time>, targetPose: Pose2dDual<Arclength>, actualPose: Pose2d): 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