Fixed bug in backing out contact forces