Motion planning a custom path using MoveIt! C++ APIs