Added start of the RG_RRT implementation