using deltas instead of distances, fixed case of getter/setting/init, change RobotPose to be tPose, and added angular and linear velocity to pose struct