Adding double pendulum contact model