Hi everywhere. I wrote the following robot model in URDF and then I built a controller to teleoptwistkeyboard with it.

The result is not the expected, since they rotate on itself doing strange things (which seems more difficult when the spin is somehow aligned with the axis of coordinates of the world). Any ideas? I tried to change values ​​Mu1, Mu2, Kp, Kd and others like selfCollide with no success.

Here is the src folder that.

https://drive.google.com/drive/folders/1f8a1eyVG45Qg1PQw1mdK9iDRK7QgSsAN?usp=sharing

Thank you!