Very interesting work!
I did not really go into the code, so these questions might be trivial:
- What part of the MoveIt! framework do you still use? You write that you bypass it by interacting with joint states directly. Could your algorithm be an alternative for the RRT algorithm, or one of the other planning algorithms?
- Would it be easy to extend this to obstacle avoidance as well?