Update to urdf changes, update path number for ploting, Add PM debug info
Update script/test_rrt.py.
Cosmetic changes