MoveIt! integration

Unit tests build but are currently not passing.