Update MP.cpp

-ExtendTree() now uses robotRadius as goal condition
-EST weight formula corrected
-Implemented MyApproach