Tested new yaw with IK. Works. Test with trajopt tomorrow