Now we are back to where we were
MoveIt stuff is working. Did some debugging, walls are not implemented yet but Jons code to avoid collisions is there. Probably ready to pull into the main repo