applying impedance controller
Merge pull request #253 from hyaguchijsk/feature/add_impedance_control

Feature/add impedance control