Fix to compute the previous position as it was the current one
Fix to compute the previous position as it was the current one

lluissalord/romeo_grasper,lluissalord/romeo_grasper,lluissalord/romeo_grasper