Finalize implementation of joint tps cost. Needs testing