[Feasible Control Convex] [Jacobian] refactor singularity handle with two unit vectors are almost paralell

[tongtybj]

What is this

To calculate distance from orign to the surface of feasible control convex, we need to consider the norm of cross production between two vector, and use it to devide the third vector. Therefore, the norm should not be zero which is the singualr case when two vectors are almost paralell.

Details

• Please refere to Eq.17 in https://arxiv.org/pdf/2008.05613. The norm of (v_i x v_j) should not be zero. This means it becomes singualr when v_i and v_j are paralell.
• We introduce a small value about 10e-5 to determine whether the norm is almost zero.
• Corresponding rostest is performed in hydrus_xi_jacobian.test