RobotState re-implementation, using urdf instead of KDL tree since it will be change over time and KDL tree doesn't have an interface which allows chances