First implementation of a node which focuses a 3d pose with the head. Needs testing and some more functionality.