Open Loop Torque and Impedance Control on Servo

I believe that cheaper devices help everyone around the world to access devices and learn state of the art concepts and build cool things. In this video I tried integrating a 20Kg-cm hobby servo with an arduino Nano, INA219 current sensor, AS5048B magnetic encoder and a L298N motor driver to implement open loop torque control and impedance/ position control for unlimited range of motion. Hope this helps hobbyists and people who are interested in learning and building devices.
To integrate the encoder I took apart the back cover of the servo and glued the magnet directly on top of the potentiometer and 3d printed a mount for the encoder board.
Surprisingly the torque that the servo produced was impressive and it could also be due to the high voltage and current setting on the L298N motor driver. Even with a low open loop torque control setting it was hard for me to stop the moving motor with my bare hands.
Code: github.com/lal...
The code above and device is not optimized and was done to test the concept. So I would highly recommend to modify it based on your convenience.