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:![quad_sdk_no_yaw_unwrapping](https://github.com/robomechanics/quad-sdk/assets/25516615/08bfd1f8-0afa-4215-ac1a-eaa160ec93d6)
After:![quad_sdk_yaw_unwrapping](https://github.com/robomechanics/quad-sdk/assets/25516615/5f229a0b-8d5d-470b-8ff8-84d6a048839a)