Testing on kuka. Need to tune gains and make a better velocity filter