Collision resolving and motion algo fixed for stability