put impedance controller in a separate header
working impedance controller for the arms using wrist F/T sensor, legs don't work