some issues need to be resolved. contact force is wrong
contact force modified
good to go