includes first joints offset when moving robot

Previously, the first joint in a robot was not properly offset when moving the robots base it seems. See screenshot in #35 (closed) - this MR includes moving the first joint as well