Robust planning for multi-stage forceful manipulation