Fix bug in monitor variables and reintroduce impedance control and sinusoidal reference