Problem About Yaw Constraint

[johnsongash]

Hi Sikang, thanks for sharing your code. I'm trying to apply the yaw planning, but some problem with the result. The output yaw angle seems to be the same as velocity direction, any way to change the parameter of theta to relax the hard constraint? I have notice that there is a parameter of "tol_yaw", that's only for goal region right? Thanks for your time, Johnson