Addresses https://github.com/robomechanics/quad-sdk/issues/204 by expanding constraint bounds on yaw in NLP from [-pi, pi] to [-10, 10], and unwraps the yaw reference to remove discontinuities. Tested by initializing the robot at x = 2.0, y = 0.0, yaw = 1.57 and requesting a goal to x = 0.0, y = -1.0. This requires the robot to pass through yaw = pi. Can confirm it fails if new function unwrapYawReference is commented out, succeeds if used.
Also verified that just expanding the bounds leads to smooth twist tracking through yaw = pi (previously this resulted in a slight jerk and higher solve times). Parameters were updated for Spirit and A1, any other robots on dev branches would need to update yaw bounds in a similar way.
Addresses https://github.com/robomechanics/quad-sdk/issues/204 by expanding constraint bounds on yaw in NLP from [-pi, pi] to [-10, 10], and unwraps the yaw reference to remove discontinuities. Tested by initializing the robot at x = 2.0, y = 0.0, yaw = 1.57 and requesting a goal to x = 0.0, y = -1.0. This requires the robot to pass through yaw = pi. Can confirm it fails if new function unwrapYawReference is commented out, succeeds if used.
Also verified that just expanding the bounds leads to smooth twist tracking through yaw = pi (previously this resulted in a slight jerk and higher solve times). Parameters were updated for Spirit and A1, any other robots on dev branches would need to update yaw bounds in a similar way.
Before:
After: