Motion testing code, sends joints and stiffnesses (todo remove debugging code)