This repository is for reproducing my stackoverflow question.
All the related code is in the error_reproduce.ipynb. To reproduce the error, you just need to open the Drake Visualizer first and then run the cells in error_reproduce.ipynb in sequence.
init_q_leftis the start position for theiiwarobotmodel/contains the hole's model, including themeshandurdffiles- In the simulation, I firstly command a vertical downward velocity for the peg to make contact with the hole surface and then command a vertical downward force at the end of peg. This applied force locates at the edge of the peg's end and points into the hole's surface (not points into the hollow hole). Since the hole's surface should be able to counteract this force, the peg shouldn't tilt much.