joint impedance ops and FK fix