Working external forces compensation for objects picking
Cleaned up, commentaries added
Merge branch 'test_jointImpedance'