Closes #9 (closed).
Impedance controller working on main robot, but still some work to tune observers which will be tracked in #8.