Fix variable initialization on robot force calculation