Kinematic solutions sorting (using previous solution) and wrist singularity management

[yvanblanchard]

Hello

I know that the lib opw kinematics written in Rust (https://github.com/bourumir-wyngs/rs-opw-kinematics) integrates two interesting features:

• if previous joint positions are provided, the solutions are sorted by proximity to them (closest first)
• for kinematic singularity at J5 = 0° or J5 = ±180° positions this solver provides reasonable J4 and J6 values close to the previous positions of these joints (and not arbitrary that may result in a large jerk of the real robot)

I don't think those features are available in the C++ implementation (this repo), and if not, do you think you could integrate in a near future ? Thank you.