Unexpected random rotation of trunk in unitree AI

[gokulmk-12]

Intro

Hi!

I am an undergraduate student, using mujoco for my project on quadruped locomotion on uneven terrains.

My setup

I am using mujoco with python bindings, and the xml files were cloned from mujoco menagerie

My question

I am trying to implement a torque controller for swing leg and stance leg in trot gait. I modified the unitree A1 xml to suit for torque control mode. After writing the code for the trot gait, the tracking is pretty good, but i am stuck in a situation where the torso rotates randomly . I have attached a video below for your reference. The red marker is the thing which is supposed to be tracked.

Initially i felt that it was something to do with the freejoint component in the xml. So i tried increasing the stiffness and damping to compensate this rotation, which worked. my question, is this the way to remove the random roation of the torso ? Also i feel i messed up the xml conversion from position to torque control, i just copied it from go2 with minimal changes. Please do let me know where i have went wrong. I have attached the xml below.

torso_failure.webm

Minimal model and/or code that explain my question

If you encountered the issue in a complex model, please simplify it as much as possible (while still reproducing the issue).

Model:

Full A1 XML

<mujoco model="a1">
  <include file="common/visual.xml"/>
  <include file="common/skybox.xml"/>
  <include file="common/materials.xml"/>
  
  <compiler angle="radian" meshdir="assets" texturedir="assets" autolimits="true"/>

  <option cone="elliptic" impratio="100"/>

  <default>
    <default class="a1">
      <geom friction="0.6" margin="0.001"/>
      <joint axis="0 1 0" damping="2" armature="0.01" frictionloss="0.2"/>
      <motor ctrlrange="-33.5 33.5"/> 
      <default class="abduction">
        <joint axis="1 0 0" damping="1" range="-0.802851 0.802851"/>
        <motor ctrlrange="-33.5 33.5"/>
      </default>
      <default class="hip">
        <joint range="-1.0472 4.18879"/>
        <motor ctrlrange="-33.5 33.5"/>
      </default>
      <default class="knee">
        <joint range="-2.69653 -0.916298" actuatorgravcomp="true"/>
        <motor ctrlrange="-33.5 33.5"/> 
      </default>
      <!-- <position kp="500" kv="5" forcerange="-33.5 33.5"/>
      <default class="abduction">
        <joint axis="1 0 0" damping="1" range="-0.802851 0.802851"/>
        <position ctrlrange="-0.802851 0.802851"/>
      </default>
      <default class="hip">
        <joint range="-1.0472 4.18879" actuatorgravcomp="true"/>
        <position ctrlrange="-1.0472 4.18879"/>
      </default>
      <default class="knee">
        <joint range="0 3.14" axis="0 -1 0" actuatorgravcomp="true"/>
        <position ctrlrange="0 3.14"/>
      </default> -->
      <default class="visual">
        <geom type="mesh" contype="0" conaffinity="0" group="2" material="dark"/>
      </default>
      <default class="collision">
        <geom group="3" type="capsule"/>
        <default class="hip_left">
          <geom size="0.04 0.04" quat="1 1 0 0" type="cylinder" pos="0 0.055 0"/>
        </default>
        <default class="hip_right">
          <geom size="0.04 0.04" quat="1 1 0 0" type="cylinder" pos="0 -0.055 0"/>
        </default>
        <default class="thigh1">
          <geom size="0.015" fromto="-0.02 0 0 -0.02 0 -0.16"/>
        </default>
        <default class="thigh2">
          <geom size="0.015" fromto="0 0 0 -0.02 0 -0.1"/>
        </default>
        <default class="thigh3">
          <geom size="0.015" fromto="-0.02 0 -0.16 0 0 -0.2"/>
        </default>
        <default class="calf1">
          <geom size="0.01" fromto="0 0 0 0.02 0 -0.13"/>
        </default>
        <default class="calf2">
          <geom size="0.01" fromto="0.02 0 -0.13 0 0 -0.2"/>
        </default>
        <default class="foot">
          <geom type="sphere" size="0.02" pos="0 0 -0.2" priority="1" solimp="0.015 1 0.02" condim="6"
            friction="0.8 0.02 0.01"/>
        </default>
      </default>
    </default>
  </default>

  <asset>
    <material name="dark" specular="0" shininess="0.25" rgba="0.2 0.2 0.2 1"/>
    <texture type="2d" name="trunk_A1" file="trunk_A1.png"/>
    <material name="carbonfibre" texture="trunk_A1" specular="0" shininess="0.25"/>

    <mesh class="a1" file="calf.obj"/>
    <mesh class="a1" file="hip.obj"/>
    <mesh class="a1" file="thigh.obj"/>
    <mesh class="a1" file="thigh_mirror.obj"/>
    <mesh class="a1" file="trunk.obj"/>
    
    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
      markrgb="0.8 0.8 0.8" width="300" height="300"/>
    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
    
  </asset>

  <worldbody>
    <light name="spotlight" mode="targetbodycom" target="trunk" pos="0 -1 2" />
    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" solref="0.02 1" solimp="0.8 0.99 0.001"/>
    <body name="trunk" pos="0 0 0.43" childclass="a1">
      <camera name="track" pos="0.846 -1.3 0.316" xyaxes="0.866 0.500 0.000 -0.171 0.296 0.940" mode="trackcom"/>
      <camera name="top" pos="-1 0 1" xyaxes="0 -1 0 0.7 0 0.7" mode="trackcom"/>
      <camera name="side" pos="0 -1 .3" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
      <camera name="back" pos="-1 0 .3" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
      <freejoint/>
      <!-- <joint type="free" stiffness="250" damping="20" frictionloss="0" armature="0.02"/> -->
      <inertial mass="6.0" pos="0 0.0041 -0.0005"
        fullinertia="0.0158533 0.0377999 0.0456542 -3.66e-05 -6.11e-05 -2.75e-05"/>
      <geom class="visual" mesh="trunk" material="carbonfibre"/>
      <geom class="collision" size="0.125 0.04 0.057" type="box"/>
      <geom class="collision" quat="1 0 1 0" pos="0 -0.04 0" size="0.058 0.125" type="cylinder"/>
      <geom class="collision" quat="1 0 1 0" pos="0 +0.04 0" size="0.058 0.125" type="cylinder"/>
      <geom class="collision" pos="0.25 0 0" size="0.005 0.06 0.05" type="box"/>
      <geom class="collision" pos="0.25 0.06 -0.01" size="0.009 0.035"/>
      <geom class="collision" pos="0.25 -0.06 -0.01" size="0.009 0.035"/>
      <geom class="collision" pos="0.25 0 -0.05" size="0.005 0.06" quat="1 1 0 0"/>
      <geom class="collision" pos="0.255 0 0.0355" size="0.021 0.052" quat="1 1 0 0"/>
      <body name="FR_hip" pos="0.183 -0.047 0">
        <inertial mass="0.696" pos="-0.003311 -0.000635 3.1e-05" quat="0.507528 0.506268 0.491507 0.494499"
          diaginertia="0.000807752 0.00055293 0.000468983"/>
        <joint class="abduction" name="FR_hip_joint"/>
        <geom class="visual" mesh="hip" quat="0 1 0 0"/>
        <geom class="hip_right"/>
        <body name="FR_thigh" pos="0 -0.08505 0">
          <inertial mass="1.013" pos="-0.003237 0.022327 -0.027326" quat="0.999125 -0.00256393 -0.0409531 -0.00806091"
            diaginertia="0.00555739 0.00513936 0.00133944"/>
          <joint class="hip" name="FR_thigh_joint"/>
          <geom class="visual" mesh="thigh_mirror"/>
          <geom class="thigh1"/>
          <geom class="thigh2"/>
          <geom class="thigh3"/>
          <body name="FR_calf" pos="0 0 -0.2">
            <inertial mass="0.226" pos="0.00472659 0 -0.131975" quat="0.706886 0.017653 0.017653 0.706886"
              diaginertia="0.00340344 0.00339393 3.54834e-05"/>
            <joint class="knee" name="FR_calf_joint"/>
            <geom class="visual" mesh="calf"/>
            <geom class="calf1"/>
            <geom class="calf2"/>
            <geom name="FR_foot" class="foot"/>
            <site name="FR" pos="0 0 -0.2" type="sphere" size="0.023" rgba="0.5 0.5 0.5 1"/>
          </body>
        </body>
      </body>
      <body name="FL_hip" pos="0.183 0.047 0">
        <inertial mass="0.696" pos="-0.003311 0.000635 3.1e-05" quat="0.494499 0.491507 0.506268 0.507528"
          diaginertia="0.000807752 0.00055293 0.000468983"/>
        <joint class="abduction" name="FL_hip_joint"/>
        <geom class="visual" mesh="hip"/>
        <geom class="hip_left"/>
        <geom class="collision" size="0.04 0.04" pos="0 0.055 0" quat="1 1 0 0" type="cylinder"/>
        <body name="FL_thigh" pos="0 0.08505 0">
          <inertial mass="1.013" pos="-0.003237 -0.022327 -0.027326" quat="0.999125 0.00256393 -0.0409531 0.00806091"
            diaginertia="0.00555739 0.00513936 0.00133944"/>
          <joint class="hip" name="FL_thigh_joint"/>
          <geom class="visual" mesh="thigh"/>
          <geom class="thigh1"/>
          <geom class="thigh2"/>
          <geom class="thigh3"/>
          <body name="FL_calf" pos="0 0 -0.2">
            <inertial mass="0.226" pos="0.00472659 0 -0.131975" quat="0.706886 0.017653 0.017653 0.706886"
              diaginertia="0.00340344 0.00339393 3.54834e-05"/>
            <joint class="knee" name="FL_calf_joint"/>
            <geom class="visual" mesh="calf"/>
            <geom class="calf1"/>
            <geom class="calf2"/>
            <geom name="FL_foot" class="foot"/>
            <site name="FL" pos="0 0 -0.2" type="sphere" size="0.023" rgba="0.5 0.5 0.5 1"/>
          </body>
        </body>
      </body>
      <body name="RR_hip" pos="-0.183 -0.047 0">
        <inertial mass="0.696" pos="0.003311 -0.000635 3.1e-05" quat="0.491507 0.494499 0.507528 0.506268"
          diaginertia="0.000807752 0.00055293 0.000468983"/>
        <joint class="abduction" name="RR_hip_joint"/>
        <geom class="visual" quat="0 0 0 -1" mesh="hip"/>
        <geom class="hip_right"/>
        <body name="RR_thigh" pos="0 -0.08505 0">
          <inertial mass="1.013" pos="-0.003237 0.022327 -0.027326" quat="0.999125 -0.00256393 -0.0409531 -0.00806091"
            diaginertia="0.00555739 0.00513936 0.00133944"/>
          <joint class="hip" name="RR_thigh_joint"/>
          <geom class="visual" mesh="thigh_mirror"/>
          <geom class="thigh1"/>
          <geom class="thigh2"/>
          <geom class="thigh3"/>
          <body name="RR_calf" pos="0 0 -0.2">
            <inertial mass="0.226" pos="0.00472659 0 -0.131975" quat="0.706886 0.017653 0.017653 0.706886"
              diaginertia="0.00340344 0.00339393 3.54834e-05"/>
            <joint class="knee" name="RR_calf_joint"/>
            <geom class="visual" mesh="calf"/>
            <geom class="calf1"/>
            <geom class="calf2"/>
            <geom name="RR_foot" class="foot"/>
            <site name="RR" pos="0 0 -0.2" type="sphere" size="0.023" rgba="0.5 0.5 0.5 1"/>
          </body>
        </body>
      </body>
      <body name="RL_hip" pos="-0.183 0.047 0">
        <inertial mass="0.696" pos="0.003311 0.000635 3.1e-05" quat="0.506268 0.507528 0.494499 0.491507"
          diaginertia="0.000807752 0.00055293 0.000468983"/>
        <joint class="abduction" name="RL_hip_joint"/>
        <geom class="visual" quat="0 0 1 0" mesh="hip"/>
        <geom class="hip_left"/>
        <body name="RL_thigh" pos="0 0.08505 0">
          <inertial mass="1.013" pos="-0.003237 -0.022327 -0.027326" quat="0.999125 0.00256393 -0.0409531 0.00806091"
            diaginertia="0.00555739 0.00513936 0.00133944"/>
          <joint class="hip" name="RL_thigh_joint"/>
          <geom class="visual" mesh="thigh"/>
          <geom class="thigh1"/>
          <geom class="thigh2"/>
          <geom class="thigh3"/>
          <body name="RL_calf" pos="0 0 -0.2">
            <inertial mass="0.226" pos="0.00472659 0 -0.131975" quat="0.706886 0.017653 0.017653 0.706886"
              diaginertia="0.00340344 0.00339393 3.54834e-05"/>
            <joint class="knee" name="RL_calf_joint"/>
            <geom class="visual" mesh="calf"/>
            <geom class="calf1"/>
            <geom class="calf2"/>
            <geom name="RL_foot" class="foot"/>
            <site name="RL" pos="0 0 -0.2" type="sphere" size="0.023" rgba="0.5 0.5 0.5 1"/>
          </body>
        </body>
      </body>
    </body>
  </worldbody>

  <actuator>
    <!-- <position class="abduction" name="FR_hip" joint="FR_hip_joint"/>
    <position class="hip" name="FR_thigh" joint="FR_thigh_joint"/>
    <position class="knee" name="FR_calf" joint="FR_calf_joint"/>
    <position class="abduction" name="FL_hip" joint="FL_hip_joint"/>
    <position class="hip" name="FL_thigh" joint="FL_thigh_joint"/>
    <position class="knee" name="FL_calf" joint="FL_calf_joint"/>
    <position class="abduction" name="RR_hip" joint="RR_hip_joint"/>
    <position class="hip" name="RR_thigh" joint="RR_thigh_joint"/>
    <position class="knee" name="RR_calf" joint="RR_calf_joint"/>
    <position class="abduction" name="RL_hip" joint="RL_hip_joint"/>
    <position class="hip" name="RL_thigh" joint="RL_thigh_joint"/>
    <position class="knee" name="RL_calf" joint="RL_calf_joint"/> -->
    <motor class="abduction" name="FR_hip" joint="FR_hip_joint"/>
    <motor class="hip" name="FR_thigh" joint="FR_thigh_joint"/>
    <motor class="knee" name="FR_calf" joint="FR_calf_joint"/>
    <motor class="abduction" name="FL_hip" joint="FL_hip_joint"/>
    <motor class="hip" name="FL_thigh" joint="FL_thigh_joint"/>
    <motor class="knee" name="FL_calf" joint="FL_calf_joint"/>
    <motor class="abduction" name="RR_hip" joint="RR_hip_joint"/>
    <motor class="hip" name="RR_thigh" joint="RR_thigh_joint"/>
    <motor class="knee" name="RR_calf" joint="RR_calf_joint"/>
    <motor class="abduction" name="RL_hip" joint="RL_hip_joint"/>
    <motor class="hip" name="RL_thigh" joint="RL_thigh_joint"/>
    <motor class="knee" name="RL_calf" joint="RL_calf_joint"/>
  </actuator>

  <sensor>
    <framelinvel objtype="body" objname="trunk" name="global_lincomvel"/>
    <frameangvel objtype="body" objname="trunk" name="global_angcomvel"/>
    <framelinacc objtype="body" objname="trunk" name="global_lincomacc"/>
    <framelinvel objtype="site" objname="FR" name="FR_foot_velocity"/>
    <framelinvel objtype="site" objname="FL" name="FL_foot_velocity"/>
    <framelinvel objtype="site" objname="RR" name="RR_foot_velocity"/>
    <framelinvel objtype="site" objname="RL" name="RL_foot_velocity"/>
    <touch name="FR_touch" site="FR"/>
    <touch name="FL_touch" site="FL"/>
    <touch name="RR_touch" site="RR"/>
    <touch name="RL_touch" site="RL"/>
  </sensor>

  <keyframe>
    <key name="home" qpos="0 0 0.304 1 0 0 0 0 0.8 -1.5 0 0.8 -1.5 0 0.8 -1.5 0 0.8 -1.5"
      ctrl="0 0.8 -1.5 0 0.8 -1.5 0 0.8 -1.5 0 0.8 -1.5"/>
  </keyframe>
</mujoco>


### Confirmations

- [X] I searched the [latest documentation](https://mujoco.readthedocs.io/en/latest/overview.html) thoroughly before posting.
- [X] I searched previous [Issues](https://github.com/google-deepmind/mujoco/issues) and [Discussions](https://github.com/google-deepmind/mujoco/discussions), I am certain this has not been raised before.