Motion planning using the move_group C++ interface