Add stop-on-force-threshold behavior to TaskSpaceDMPController