fixed bug in impedance control where next commanded position cannot be current msr position