Clean thread exit on BFS3D
Allow Motionprimitives in radians
New challenging goal joint configuration