Extended RRT-CONNECT Algorithm for Whole-body Motion Planning