<mujoco model="hive_core_r0">
  <option iterations="50" timestep="0.001" solver="PGS" gravity="0 0 -9.81" />

  <compiler angle="radian" meshdir="meshes" eulerseq="zyx" autolimits="true" />

  <!-- <default>
    <joint limited="true" damping="0.01" armature="0.01" frictionloss="0.01" />
    <geom condim="4" contype="1" conaffinity="15" friction="0.9 0.2 0.2" solref="0.001 2" />
    <motor ctrllimited="false" />
    <equality solref="0.001 2" />
    <default class="visualgeom">
      <geom material="visualgeom" condim="1" contype="0" conaffinity="0" />
    </default>
  </default> -->

  <default>
    <!-- 关节参数：减小关节摩擦 -->
    <joint limited="true" damping="0.005" armature="0.01" frictionloss="0.001" />  <!-- 降低frictionloss和damping -->
    
    <!-- 几何体参数：减小摩擦和碰撞响应 -->
    <geom condim="4" contype="1" conaffinity="15" 
          friction="0.3 0.01 0.05" 
          solref="0.001 3"  
          solimp="0.005 0.9 0.1" />  <!-- 增加穿透容忍度（第一个值），减少碰撞力 -->
    
    <motor ctrllimited="false" />
    <equality solref="0.01 3" />  <!-- 同步调整equality的碰撞参数 -->
    
    <default class="visualgeom">
      <geom material="visualgeom" condim="1" contype="0" conaffinity="0" />
    </default>
  </default>


  <asset>
    <mesh name="base_link" content_type="model/stl" file="base_link.STL" />
    <mesh name="left_leg1_link" content_type="model/stl" file="left_leg1_link.STL" />
    <mesh name="left_wheel1_link" content_type="model/stl" file="left_wheel1_link.STL" />
    <mesh name="right_leg1_link" content_type="model/stl" file="right_leg1_link.STL" />
    <mesh name="right_wheel1_link" content_type="model/stl" file="right_wheel1_link.STL" />
    <mesh name="pitch_body_link" content_type="model/stl" file="pitch_body_link.STL" />
    <mesh name="left_shoulder_link" content_type="model/stl" file="left_shoulder_link.STL" />
    <mesh name="left_arm1_link" content_type="model/stl" file="left_arm1_link.STL" />
    <mesh name="left_arm2_link" content_type="model/stl" file="left_arm2_link.STL" />
    <mesh name="right_shoulder_link" content_type="model/stl" file="right_shoulder_link.STL" />
    <mesh name="right_arm1_link" content_type="model/stl" file="right_arm1_link.STL" />
    <mesh name="right_arm2_link" content_type="model/stl" file="right_arm2_link.STL" />
    <mesh name="left_leg2_link" content_type="model/stl" file="left_leg2_link.STL" />
    <mesh name="left_wheel2_link" content_type="model/stl" file="left_wheel2_link.STL" />
    <mesh name="right_leg2_link" content_type="model/stl" file="right_leg2_link.STL" />
    <mesh name="right_wheel2_link" content_type="model/stl" file="right_wheel2_link.STL" />
    <texture name="texplane" type="2d" builtin="checker" rgb1=".0 .0 .0" rgb2=".8 .8 .8" width="100" height="100" />
    <material name="matplane" reflectance="0." texture="texplane" texrepeat="1 1" texuniform="true" />
    <material name="visualgeom" rgba="0.5 0.9 0.2 1" />
  </asset>

  <worldbody>
    <light directional="true" diffuse="0.4 0.4 0.4" specular="0.1 0.1 0.1" pos="0 0 5.0" dir="0 0 -1" castshadow="false" />
    <light directional="true" diffuse="0.6 0.6 0.6" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1" />
    <geom name="ground" type="plane" pos="0 0 0" size="100 100 0.001" quat="1 0 0 0" material="matplane" condim="3" conaffinity="15" />
    <camera name="fixed" pos="0 -3.0 0.9062448722851582" xyaxes="1 0 0 0 0 1" />
    <camera name="track" mode="trackcom" pos="0 -3.0 0.9062448722851582" xyaxes="1 0 0 0 0 1" />
    <body name="root" pos="0 0 0.4062448722851581" quat="1 0 0 0">
      <freejoint name="root" />
      <site name="imu" size="0.01" pos="0 0 0" />
      <geom type="mesh" rgba="0.592157 0.619608 0.65098 1" mesh="base_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
      <geom type="mesh" rgba="0.592157 0.619608 0.65098 1" mesh="base_link" />
      <body name="left_leg1_link" pos="-0.025573 0.143 0.031674" quat="0.685177 0.728377 0 0">
        <inertial pos="0.0365132 -0.100285 -0.000439159" quat="0.701159 -0.118757 -0.126604 0.691552" mass="0.687222" diaginertia="0.0112809 0.011113 0.000216194" />
        <joint name="left_leg1_joint" pos="0 0 0" axis="0 0 -1" range="-1.57 1.57" actuatorfrcrange="-60 60" />
        <geom type="mesh" rgba="0.686275 0.686275 0.686275 1" mesh="left_leg1_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
        <geom type="mesh" rgba="0.686275 0.686275 0.686275 1" mesh="left_leg1_link" />
        <body name="left_wheel1_link" pos="0.14246 -0.39141 0.03757">
          <inertial pos="3.63169e-07 2.62194e-06 -0.00076961" quat="0.706246 0.0348808 -0.0348808 0.706246" mass="0.75" diaginertia="0.00042099 0.00032 0.00031901" />
          <joint name="left_wheel1_joint" pos="0 0 0" axis="0 0 -1" range="-1000 1000" actuatorfrcrange="-30 30" />
          <geom type="mesh" rgba="0.780392 0.780392 0.780392 1" mesh="left_wheel1_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
          <geom type="mesh" rgba="0.780392 0.780392 0.780392 1" mesh="left_wheel1_link" />
        </body>
      </body>
      <body name="right_leg1_link" pos="-0.025573 -0.143 0.031674" quat="0.728374 0.685179 0 0">
        <inertial pos="0.036491 -0.100293 0.000439159" quat="0.691664 -0.126482 -0.118674 0.701084" mass="0.687222" diaginertia="0.0112809 0.0111118 0.000207362" />
        <joint name="right_leg1_joint" pos="0 0 0" axis="0 0 -1" range="-1.57 1.57" actuatorfrcrange="-60 60" />
        <geom type="mesh" rgba="0.686275 0.686275 0.686275 1" mesh="right_leg1_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
        <geom type="mesh" rgba="0.686275 0.686275 0.686275 1" mesh="right_leg1_link" />
        <body name="right_wheel1_link" pos="0.142461 -0.391408 -0.0375696" quat="0 1 0 0">
          <inertial pos="-2.01863e-06 -1.71218e-06 -0.00076961" quat="0.706246 -0.0348808 0.0348808 0.706246" mass="0.75" diaginertia="0.00042099 0.00032 0.00031901" />
          <joint name="right_wheel1_joint" pos="0 0 0" axis="0 0 -1" range="-1000 1000" actuatorfrcrange="-30 30" />
          <geom type="mesh" rgba="1 1 1 1" mesh="right_wheel1_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
          <geom type="mesh" rgba="1 1 1 1" mesh="right_wheel1_link" />
        </body>
      </body>
      <body name="pitch_body_link" pos="-0.022999 0.039 0.048" quat="0.707105 0.707108 0 0">
        <inertial pos="0.000405026 0.164222 0.038213" quat="0.999026 0.0383753 0.0211692 -0.00503889" mass="4.31439" diaginertia="0.0256579 0.0186955 0.0159865" />
        <joint name="pitch_body_joint" pos="0 0 0" axis="0 0 -1" range="-0.5236 0.5236" actuatorfrcrange="-20 20" />
        <geom type="mesh" rgba="0.592157 0.619608 0.65098 1" mesh="pitch_body_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
        <geom type="mesh" rgba="0.592157 0.619608 0.65098 1" mesh="pitch_body_link" />
        <body name="left_shoulder_link" pos="0.0039637 0.22096 -0.0095" quat="0.707105 -0.707108 0 0">
          <inertial pos="0.0139818 -0.000283388 0.0469667" quat="0.706719 0.706719 0.0234277 -0.0234277" mass="1.12175" diaginertia="0.000990664 0.000839336 0.00082" />
          <joint name="left_shoulder_joint" pos="0 0 0" axis="-0.017935 0 -0.999839" range="-1.57 1.57" actuatorfrcrange="-60 60" />
          <geom type="mesh" rgba="0.501961 0.501961 0.501961 1" mesh="left_shoulder_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
          <geom type="mesh" rgba="0.501961 0.501961 0.501961 1" mesh="left_shoulder_link" />
          <body name="left_arm1_link" pos="0.040338 -0.00076903 0.046784" quat="0.915825 -0.401579 0 0">
            <inertial pos="-0.0222765 0.116446 0.130396" quat="0.713034 0.69112 0.104336 0.0552205" mass="0.97181" diaginertia="0.00209185 0.0020505 0.000677649" />
            <joint name="left_arm1_joint" pos="0 0 0" axis="0.99965 0 -0.026469" range="-1.57 1.57" actuatorfrcrange="-60 60" />
            <geom type="mesh" rgba="1 1 1 1" mesh="left_arm1_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
            <geom type="mesh" rgba="1 1 1 1" mesh="left_arm1_link" />
            <body name="left_arm2_link" pos="0.0085407 0.12037 0.13366" quat="-3.67321e-06 -1 0 0">
              <inertial pos="-0.00344285 -0.0871901 -0.09473" quat="0.668762 0.691991 -0.148059 0.228003" mass="0.153345" diaginertia="0.000866593 0.000844058 8.93494e-05" />
              <joint name="left_arm2_joint" pos="0 0 0" axis="-0.99965 0 -0.026469" range="-1.57 1.57" actuatorfrcrange="-20 20" />
              <geom type="mesh" rgba="1 1 1 1" mesh="left_arm2_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
              <geom type="mesh" rgba="1 1 1 1" mesh="left_arm2_link" />
            </body>
          </body>
        </body>
        <body name="right_shoulder_link" pos="0.0039637 0.22096 0.0865" quat="0.707105 -0.707108 0 0">
          <inertial pos="0.0139829 0.000183964 0.0469281" quat="0.706719 0.706719 0.0234277 -0.0234277" mass="1.12175" diaginertia="0.000990664 0.000839336 0.00082" />
          <joint name="right_shoulder_joint" pos="0 0 0" axis="-0.017935 0 -0.999839" range="-1.57 1.57" actuatorfrcrange="-60 60" />
          <geom type="mesh" rgba="0.898039 0.921569 0.929412 1" mesh="right_shoulder_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
          <geom type="mesh" rgba="0.898039 0.921569 0.929412 1" mesh="right_shoulder_link" />
          <body name="right_arm1_link" pos="0.040343 0.000445 0.046784" quat="0.960932 0.276783 0 0">
            <inertial pos="-0.0237185 -0.146707 0.0947584" quat="0.700546 0.70096 -0.0393265 -0.127846" mass="0.97181" diaginertia="0.00208774 0.00204969 0.000682566" />
            <joint name="right_arm1_joint" pos="0 0 0" axis="0.999776 0 -0.0211789" range="-1.57 1.57" actuatorfrcrange="-60 60" />
            <geom type="mesh" rgba="1 1 1 1" mesh="right_arm1_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
            <geom type="mesh" rgba="1 1 1 1" mesh="right_arm1_link" />
            <body name="right_arm2_link" pos="0.00696183 -0.151368 0.0972794" quat="0 -1 0 0">
              <inertial pos="-0.00446701 0.108002 -0.0700232" quat="0.707099 0.702409 0.00330707 0.0813729" mass="0.153345" diaginertia="0.00086092 0.00085 9.90801e-05" />
              <joint name="right_arm2_joint" pos="0 0 0" axis="-0.999776 0 -0.0211789" range="-1.57 1.57" actuatorfrcrange="-20 20" />
              <geom type="mesh" rgba="1 1 1 1" mesh="right_arm2_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
              <geom type="mesh" rgba="1 1 1 1" mesh="right_arm2_link" />
            </body>
          </body>
        </body>
      </body>
      <body name="left_leg2_link" pos="-0.090467 0.14336 0.031579" quat="0.685177 0.728377 0 0">
        <inertial pos="-0.0333228 -0.0915849 0.000701841" quat="0.693033 0.128657 0.116355 0.699724" mass="0.758445" diaginertia="0.0118728 0.0117401 0.000237059" />
        <joint name="left_leg2_joint" pos="0 0 0" axis="0 0 -1" range="-1.57 1.57" actuatorfrcrange="-60 60" />
        <geom type="mesh" rgba="0.686275 0.686275 0.686275 1" mesh="left_leg2_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
        <geom type="mesh" rgba="0.686275 0.686275 0.686275 1" mesh="left_leg2_link" />
        <body name="left_wheel2_link" pos="-0.14242 -0.39129 0.037923">
          <inertial pos="1.94616e-06 1.79413e-06 -0.00076961" quat="0.706246 0.0348808 -0.0348808 0.706246" mass="0.75" diaginertia="0.00042099 0.00032 0.00031901" />
          <joint name="left_wheel2_joint" pos="0 0 0" axis="0 0 -1" range="-1000 1000" actuatorfrcrange="-30 30" />
          <geom type="mesh" rgba="0.780392 0.780392 0.780392 1" mesh="left_wheel2_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
          <geom type="mesh" rgba="0.780392 0.780392 0.780392 1" mesh="left_wheel2_link" />
        </body>
      </body>
      <body name="right_leg2_link" pos="-0.090467 -0.14336 0.031579" quat="0.707105 0.707108 0 0">
        <inertial pos="-0.0333429 -0.0914497 0.00489015" quat="0.702629 0.116043 0.129569 0.68997" mass="0.758445" diaginertia="0.0118729 0.0117377 0.00022938" />
        <joint name="right_leg2_joint" pos="0 0 0" axis="0 0 -1" range="-1.57 1.57" actuatorfrcrange="-60 60" />
        <geom type="mesh" rgba="0.501961 0.501961 0.501961 1" mesh="right_leg2_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
        <geom type="mesh" rgba="0.501961 0.501961 0.501961 1" mesh="right_leg2_link" />
        <body name="right_wheel2_link" pos="-0.14242 -0.39288 -0.013965" quat="0.0305416 0.999533 0 0">
          <inertial pos="-2.61861e-06 -3.86428e-07 -0.00076961" quat="0.706246 -0.0348808 0.0348808 0.706246" mass="0.75" diaginertia="0.00042099 0.00032 0.00031901" />
          <joint name="right_wheel2_joint" pos="0 0 0" axis="0 0 -1" range="-1000 1000" actuatorfrcrange="-30 30" />
          <geom type="mesh" rgba="0.780392 0.780392 0.780392 1" mesh="right_wheel2_link" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
          <geom type="mesh" rgba="0.780392 0.780392 0.780392 1" mesh="right_wheel2_link" />
        </body>
      </body>
    </body>
  </worldbody>


  <actuator>
    <!-- 腿部关节 -->
    <position name="left_leg1_joint_ctrl" joint="left_leg1_joint" kp="500" kv="0.6" />
    <position name="right_leg1_joint_ctrl" joint="right_leg1_joint" kp="500" kv="0.6" />
    <position name="left_leg2_joint_ctrl" joint="left_leg2_joint" kp="500" kv="0.6" />
    <position name="right_leg2_joint_ctrl" joint="right_leg2_joint" kp="500" kv="0.6" />
    
    <!-- 车轮关节 -->
    <position name="left_wheel1_joint_ctrl" joint="left_wheel1_joint" kp="500" kv="0.6" />
    <position name="right_wheel1_joint_ctrl" joint="right_wheel1_joint" kp="500" kv="0.6" />
    <position name="left_wheel2_joint_ctrl" joint="left_wheel2_joint" kp="500" kv="0.6" />
    <position name="right_wheel2_joint_ctrl" joint="right_wheel2_joint" kp="500" kv="0.6" />
    
    <!-- 身体关节 -->
    <position name="pitch_body_joint_ctrl" joint="pitch_body_joint" kp="500" kv="0.6" />
    
    <!-- 手臂关节 -->
    <position name="left_shoulder_joint_ctrl" joint="left_shoulder_joint" kp="500" kv="0.6" />
    <position name="right_shoulder_joint_ctrl" joint="right_shoulder_joint" kp="500" kv="0.6" />
    <position name="left_arm1_joint_ctrl" joint="left_arm1_joint" kp="500" kv="0.6" />
    <position name="left_arm2_joint_ctrl" joint="left_arm2_joint" kp="500" kv="0.6" />
    <position name="right_arm1_joint_ctrl" joint="right_arm1_joint" kp="500" kv="0.6" />
    <position name="right_arm2_joint_ctrl" joint="right_arm2_joint" kp="500" kv="0.6" />
  </actuator>

  <sensor>
    <actuatorpos name="left_leg1_joint_p" actuator="left_leg1_joint_ctrl" />
    <actuatorvel name="left_leg1_joint_v" actuator="left_leg1_joint_ctrl" />
    <actuatorfrc name="left_leg1_joint_f" actuator="left_leg1_joint_ctrl" noise="0.001" />
    <actuatorpos name="left_wheel1_joint_p" actuator="left_wheel1_joint_ctrl" />
    <actuatorvel name="left_wheel1_joint_v" actuator="left_wheel1_joint_ctrl" />
    <actuatorfrc name="left_wheel1_joint_f" actuator="left_wheel1_joint_ctrl" noise="0.001" />
    <actuatorpos name="right_leg1_joint_p" actuator="right_leg1_joint_ctrl" />
    <actuatorvel name="right_leg1_joint_v" actuator="right_leg1_joint_ctrl" />
    <actuatorfrc name="right_leg1_joint_f" actuator="right_leg1_joint_ctrl" noise="0.001" />
    <actuatorpos name="right_wheel1_joint_p" actuator="right_wheel1_joint_ctrl" />
    <actuatorvel name="right_wheel1_joint_v" actuator="right_wheel1_joint_ctrl" />
    <actuatorfrc name="right_wheel1_joint_f" actuator="right_wheel1_joint_ctrl" noise="0.001" />
    <actuatorpos name="pitch_body_joint_p" actuator="pitch_body_joint_ctrl" />
    <actuatorvel name="pitch_body_joint_v" actuator="pitch_body_joint_ctrl" />
    <actuatorfrc name="pitch_body_joint_f" actuator="pitch_body_joint_ctrl" noise="0.001" />
    <actuatorpos name="left_shoulder_joint_p" actuator="left_shoulder_joint_ctrl" />
    <actuatorvel name="left_shoulder_joint_v" actuator="left_shoulder_joint_ctrl" />
    <actuatorfrc name="left_shoulder_joint_f" actuator="left_shoulder_joint_ctrl" noise="0.001" />
    <actuatorpos name="left_arm1_joint_p" actuator="left_arm1_joint_ctrl" />
    <actuatorvel name="left_arm1_joint_v" actuator="left_arm1_joint_ctrl" />
    <actuatorfrc name="left_arm1_joint_f" actuator="left_arm1_joint_ctrl" noise="0.001" />
    <actuatorpos name="left_arm2_joint_p" actuator="left_arm2_joint_ctrl" />
    <actuatorvel name="left_arm2_joint_v" actuator="left_arm2_joint_ctrl" />
    <actuatorfrc name="left_arm2_joint_f" actuator="left_arm2_joint_ctrl" noise="0.001" />
    <actuatorpos name="right_shoulder_joint_p" actuator="right_shoulder_joint_ctrl" />
    <actuatorvel name="right_shoulder_joint_v" actuator="right_shoulder_joint_ctrl" />
    <actuatorfrc name="right_shoulder_joint_f" actuator="right_shoulder_joint_ctrl" noise="0.001" />
    <actuatorpos name="right_arm1_joint_p" actuator="right_arm1_joint_ctrl" />
    <actuatorvel name="right_arm1_joint_v" actuator="right_arm1_joint_ctrl" />
    <actuatorfrc name="right_arm1_joint_f" actuator="right_arm1_joint_ctrl" noise="0.001" />
    <actuatorpos name="right_arm2_joint_p" actuator="right_arm2_joint_ctrl" />
    <actuatorvel name="right_arm2_joint_v" actuator="right_arm2_joint_ctrl" />
    <actuatorfrc name="right_arm2_joint_f" actuator="right_arm2_joint_ctrl" noise="0.001" />
    <actuatorpos name="left_leg2_joint_p" actuator="left_leg2_joint_ctrl" />
    <actuatorvel name="left_leg2_joint_v" actuator="left_leg2_joint_ctrl" />
    <actuatorfrc name="left_leg2_joint_f" actuator="left_leg2_joint_ctrl" noise="0.001" />
    <actuatorpos name="left_wheel2_joint_p" actuator="left_wheel2_joint_ctrl" />
    <actuatorvel name="left_wheel2_joint_v" actuator="left_wheel2_joint_ctrl" />
    <actuatorfrc name="left_wheel2_joint_f" actuator="left_wheel2_joint_ctrl" noise="0.001" />
    <actuatorpos name="right_leg2_joint_p" actuator="right_leg2_joint_ctrl" />
    <actuatorvel name="right_leg2_joint_v" actuator="right_leg2_joint_ctrl" />
    <actuatorfrc name="right_leg2_joint_f" actuator="right_leg2_joint_ctrl" noise="0.001" />
    <actuatorpos name="right_wheel2_joint_p" actuator="right_wheel2_joint_ctrl" />
    <actuatorvel name="right_wheel2_joint_v" actuator="right_wheel2_joint_ctrl" />
    <actuatorfrc name="right_wheel2_joint_f" actuator="right_wheel2_joint_ctrl" noise="0.001" />
    <framequat name="orientation" objtype="site" noise="0.001" objname="imu" />
    <gyro name="angular-velocity" site="imu" noise="0.005" cutoff="34.9" />
  </sensor>
</mujoco>