Motion planning using MoveIt!