Added initial configuration read and setting, still missing to pass initial configuration to ref for pids so the robot come back to zeros.