Implementing Mimic joint plugin

Hello, i am trying to make a plugin for mimic joints. ( Because a lot of official urdf’s uses them like robotiq 2f_140 which i am trying to make it work.
This is my code repo:

I think the problem is that i am trying to change joint position with JointPositionReset and it brakes physics and then crashes after a while
Is there any other possible way to do this?

Also this plugin has already been implemented in gazeebo classic: