Update RRTConnectPlanner.py