[SDFormat->MJCF] Support kinematic loops

[azeey]

Currently, only kinematic trees are supported in the converter. We need to detect if a kinematic loop exists in an SDFormat file and add MJCF equality constraints (see https://mujoco.readthedocs.io/en/latest/XMLreference.html#equality)