Fixed initial commanded position error by setting to zero -- was un-initialized