it can cause dividing zero when computing dpsi

[charlie-dog]

Hello,

This work is great, but I have a question about the flatness computation. When you compute the psi (yaw), you adopt arccos(x0^Td_|_ / ||x0|| ||d_|||) * sgn((x0 X d|)^z0). I guess this may cause singularity of dpsi/dt when the d|_ is parallel with x0, especially x0^Td_|_ / ||x0|| ||d_||| = -1. The psi is discontinue at x0^T*d|_ / ||x0|| ||d_||| (d|_ = [-1, 0, 0] for instance). The dpsi/dt will be infinity, due to arccos'(.) = -1/ sqrt(1-.^2), where 1-.^2 = 0, when d_|_ = [-1, 0, 0].

I think a have a way to cope with this problem, we can compute psi:
psi = arccos(x0^Td_|_ / ||x0|| ||d_|||) * sgn((x0 X d|)^z0), if y0^T * d|_ >= sqrt(2),
psi = arcsin(y0^Td_|_ / ||y0|| ||d_|||) * sgn((x0 X d|)^z0) + 0.5*PI, else if x0^T * d|_ > sqrt(2).
Actually psi is still not continuous (psi \in [-PI3/4, PI5/4]), but it avoids dividing zero when computing dpsi/dt.

I am not sure if this can work. Could please help me check its effectiveness.

[charlie-dog]

I dont know why the d | is not shown correctly sometimes, I just want to type dir_xb in your code. Anyway, what I was trying to say are shown in the picture.