Continued Work on MoveIt Path Planning

Signed-off-by: Andoni Aguirrezabal <aaguirrezaba@mail.usf.edu>