<!-- Stanford Pupper Model

The state space is populated with joints in the order that they are
defined in this file.

The elements of the control variable s.d.ctrl correspond to:
[fr_x, fr_y, fr_ext,
fl_x, fl_y, fl_ext,
br_x, br_y, br_ext,
bl_x, bl_y, bl_ext]

fr = front-right
fl = front-left
br = back-right
bl = back-left
_x indicates rotation on the forward/back axis of the robot
_y indicates rotation on the left/right axis of the hip module
_ext indicates linear extension of the leg. Positive values = leg goes up
-->
<mujoco model="Pupper">
  <compiler angle="radian" meshdir="mesh/" texturedir="texture/" inertiafromgeom="false"/>

  <default>
    <mesh scale="0.001 0.001 0.001"/>
    <joint type="hinge" limited="true" armature="0.01936" damping="1.049"/>
    <geom conaffinity="0"/>
    <position forcelimited="true"/>
    <default class="2x4">
      <geom type="box" material="Fir" size="0.045 0.30 0.02" conaffinity="1"/>
    </default>
  </default>

  <option>
    <flag sensornoise="disable"/>
  </option>

  <!--<sensor>
    <accelerometer name="accel" noise="0.01" site="imu_site"/>
    <gyro name="gyro" noise="0.02" site="imu_site"/>
</sensor>-->

  <size nstack="300000" nuser_geom="1"/>

  <option timestep="0.0001"/>

  <asset>
    <texture builtin="gradient" height="100" rgb1="1 1 1" rgb2="0 0 0" type="skybox" width="100"/>
    <mesh file="TCBody.stl"/>
    <mesh file="Upper Leg plus Motor Left Side.stl"/>
    <mesh file="Upper Leg plus Motor Right Side.stl"/>
    <mesh file="Upper Leg Left Side (reduced).stl"/>
    <mesh file="Upper Leg Right Side (reduced).stl"/>
    <mesh file="M2006.stl"/>
    <mesh file="Lower Leg Round Left Side.stl"/>
    <mesh file="Lower Leg Round Right Side.stl"/>

    <texture file="BrushedAluminum.png" type="2d"/>
    <texture file="Concrete.png" type="2d"/>
    <texture file="Fir.png" type="cube" gridsize="3 4" gridlayout=".U..LFRB.D.."/>
    <texture file="Marble.png" type="cube"/>
    <texture file="Oak.png" type="2d"/>

    <material name="ABS" rgba="1 1 1 1"/>
    <material name="BrushedAluminum" texture="BrushedAluminum" specular="0.9" shininess="0.9" reflectance="0.3"/>
    <material name="Concrete" texture="Concrete" specular="0" shininess="0.1" reflectance="0" texrepeat="2 2"/>
    <material name="Fir" texture="Fir" specular="0.1" shininess="0.01" reflectance="0.01" texrepeat="1 3"/>
    <material name="M2006" specular="0.9" shininess="0.9" reflectance="0.3" rgba = ".1 .1 .1 1"/>
    <material name="Marble" texture="Marble" rgba=".7 .65 .65 0.9"/>
    <material name="Oak" texture="Oak" specular="0.1" shininess="0.01" reflectance="0.01" texrepeat="4 4"/>
  </asset>

  <worldbody>
    <!-- <light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/> -->
    <!-- <light dir="-0 0 -1.3" directional="true" exponent=".001" pos="0 0 1.3" specular=".1 .1 .1"/>  castshadow="true" cutoff="35" -->
    <light directional="true" diffuse=".2 .2 .2" specular="0.1 0.1 0.1" pos="0 0 5.0" dir="0 0 -1"/>
    <light directional="true" diffuse=".3 .3 .3" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1"/>
    <!-- <light pos="0.4 -0.2 1.5" dir="-0.4 0.2 -1.5"/> -->
    <!--
        ambient : real(3), "0 0 0" The ambient color of the light.
        diffuse : real(3), "0.7 0.7 0.7" The diffuse color of the light.
        specular : real(3), "0.3 0.3 0.3" The specular color of the light.
    -->
    <!--<geom type="plane" material="Oak" size="10 10 .1" conaffinity="1" name="floor"/>-->
    <geom type="plane" material="Concrete" size="4 4 .4" conaffinity="1" name="floor"/>
    <!-- <body>
      <joint type="free" limited="false" damping=".3"/>
      <geom type="sphere" material="BrushedAluminum" pos="0.7 -0.5 0.07" size="0.07" conaffinity="1" condim="6" name="ball1" rgba="1 0 0 1"/>
    </body> -->
    <site type="box" size="2.5 .5 .001" pos="0 -2.5 0" rgba=".2 .3 .2 .2"/>
    <site type="box" size="2.5 .5 .001" pos="0 2.5 0" rgba=".2 .3 .2 .2"/>
    <site type="box" size=".64 0.34 .001" pos="0.6 0.5 0" rgba=".5 .2 .2 .2"/>
    <geom class="2x4" pos=".25 0.5 0.02"/>
    <geom class="2x4" pos=".59 0.5 0.02"/>
    <geom class="2x4" pos=".93 0.5 0.02"/>

    <body name="torso" pos="0 0 0.2">
      <inertial pos="0 0 0" mass="1.0" diaginertia="0.0003688 0.002508 0.002674"/>
      <camera name="track" mode="trackcom" pos="0 -2 0.3" xyaxes="1 0 0 0 0 1"/>
      <camera name="puppercam" pos="0 0.16 0.11" quat="0.7453559925 0.6666666 0 0"/>
      <freejoint/>
      <geom type="mesh" mesh="TCBody" pos="-0.08 0 0" quat="0.5 0.5 0.5 0.5" material="ABS"/>
      <!--<site name="imu_site" size=".01 .01 .01" type="box"/>-->

      <body name="fr_module" pos="0.1 -0.04 0">
        <inertial pos="0 0 0" mass="0.08" diaginertia="3.698e-05 7.127e-06 4.075e-05"/>
        <joint name="fr_x" axis="1 0 0" range="-1.57 1.57"/>
        <!-- the pos and quat on the motor orient the tab (electrical connection) to point up -->
        <!-- <geom type="mesh" mesh="M2006" material="M2006" pos="-0.015 -0.005 0" quat="-0.9238795 0 0 0.3826834"/> -->
        <geom type="mesh" mesh="M2006" material="M2006" pos="-0.0145 0.008 -0.015" quat="0 0 0.7071068 0.7071068"/>
        <body pos="0 -0.032 -0.08" quat="0 0 0.7071068 0.7071068">
          <inertial pos="0 0 0" mass="0.06" diaginertia="0.0002253 6.493e-05 0.0002502"/>
          <joint name="fr_y" axis="0 0 1" pos="0 0.08 0" range="-1.57 1.57"/>
          <geom type="mesh" mesh="Upper Leg plus Motor Right Side" quat="0.7071068 0 0.7071068 0" material="ABS"/>
          <!-- <geom type="mesh" mesh="Upper Leg Right Side (reduced)" material="ABS"/>
          <geom type="mesh" mesh="M2006" material="M2006" pos=".04 -0.0205 .001" quat="-0.6532815 0.2705981 -0.6532815 0.2705981"/> -->
          <body>
            <inertial pos="0 0 0" mass="0.03" diaginertia="0.0002253 6.493e-05 0.0002502"/>
            <joint name="fr_y2" axis="0 0 1" range="-3.14 0.0"/>
            <geom type="mesh" mesh="Lower Leg Round Right Side" material="ABS"/>
          </body>
        </body>
      </body>

      <body name="fl_module" pos="0.1 0.04 0">
        <inertial pos="0 0 0" mass="0.08" diaginertia="3.698e-05 7.127e-06 4.075e-05"/>
        <joint name="fl_x" axis="1 0 0" range="-1.57 1.57"/>
        <!-- the pos and quat on the motor orient the tab (electrical connection) to point up -->
        <!-- <geom type="mesh" mesh="M2006" material="M2006" pos="-0.015 -0.005 0" quat="-0.9238795 0 0 0.3826834"/> -->
        <geom type="mesh" mesh="M2006" material="M2006" pos="0.0145 -0.008 -0.015" quat="0.7071068 0.7071068 0 0"/>
        <body pos="0 0.032 -0.08" quat="0 0 0.7071068 0.7071068">
          <inertial pos="0 0 0" mass="0.06" diaginertia="0.0002253 6.493e-05 0.0002502"/>
          <joint name="fl_y" axis="0 0 1" pos="0 0.08 0" range="-1.57 1.57"/>
          <geom type="mesh" mesh="Upper Leg plus Motor Left Side" quat="0.7071068 0 -0.7071068 0" material="ABS"/>
          <!-- <geom type="mesh" mesh="Upper Leg Left Side (reduced)" material="ABS"/>
          <geom type="mesh" mesh="M2006" material="M2006" pos=".04 -0.0205 .001" quat="-0.6532815 0.2705981 -0.6532815 0.2705981"/> -->
          <body>
            <inertial pos="0 0 0" mass="0.03" diaginertia="0.0002253 6.493e-05 0.0002502"/>
            <joint name="fl_y2" axis="0 0 1" range="-3.14 0.0"/>
            <geom type="mesh" mesh="Lower Leg Round Left Side" material="ABS"/>
          </body>
        </body>
      </body>
      
      <body name="br_module" pos="-0.1 -0.04 0">
        <inertial pos="0 0 0" mass="0.08" diaginertia="3.698e-05 7.127e-06 4.075e-05"/>
        <joint name="br_x" axis="1 0 0" range="-1.57 1.57"/>
        <!-- the pos and quat on the motor orient the tab (electrical connection) to point up -->
        <!-- <geom type="mesh" mesh="M2006" material="M2006" pos="-0.015 -0.005 0" quat="-0.9238795 0 0 0.3826834"/> -->
        <geom type="mesh" mesh="M2006" material="M2006" pos="-0.0145 0.008 -0.015" quat="0 0 0.7071068 0.7071068"/>
        <body pos="0 -0.032 -0.08" quat="0 0 0.7071068 0.7071068">
          <inertial pos="0 0 0" mass="0.06" diaginertia="0.0002253 6.493e-05 0.0002502"/>
          <joint name="br_y" axis="0 0 1" pos="0 0.08 0" range="-1.57 1.57"/>
          <geom type="mesh" mesh="Upper Leg plus Motor Right Side" quat="0.7071068 0 0.7071068 0" material="ABS"/>
          <!-- <geom type="mesh" mesh="Upper Leg Right Side (reduced)" material="ABS"/>
          <geom type="mesh" mesh="M2006" material="M2006" pos=".04 -0.0205 .001" quat="-0.6532815 0.2705981 -0.6532815 0.2705981"/> -->
          <body>
            <inertial pos="0 0 0" mass="0.03" diaginertia="0.0002253 6.493e-05 0.0002502"/>
            <joint name="br_y2" axis="0 0 1" range="-3.14 0.0"/>
            <geom type="mesh" mesh="Lower Leg Round Right Side" material="ABS"/>
          </body>
        </body>
      </body>

      <body name="bl_module" pos="-0.1 0.04 0">
        <inertial pos="0 0 0" mass="0.08" diaginertia="3.698e-05 7.127e-06 4.075e-05"/>
        <joint name="bl_x" axis="1 0 0" range="-1.57 1.57"/>
        <!-- the pos and quat on the motor orient the tab (electrical connection) to point up -->
        <!-- <geom type="mesh" mesh="M2006" material="M2006" pos="-0.015 -0.005 0" quat="-0.9238795 0 0 0.3826834"/> -->
        <geom type="mesh" mesh="M2006" material="M2006" pos="0.0145 -0.008 -0.015" quat="0.7071068 0.7071068 0 0"/>
        <body pos="0 0.032 -0.08" quat="0 0 0.7071068 0.7071068">
          <inertial pos="0 0 0" mass="0.06" diaginertia="0.0002253 6.493e-05 0.0002502"/>
          <joint name="bl_y" axis="0 0 1" pos="0 0.08 0" range="-1.57 1.57"/>
          <geom type="mesh" mesh="Upper Leg plus Motor Left Side" quat="0.7071068 0 -0.7071068 0" material="ABS"/>
          <!-- <geom type="mesh" mesh="Upper Leg Left Side (reduced)" material="ABS"/>
          <geom type="mesh" mesh="M2006" material="M2006" pos=".04 -0.0205 .001" quat="-0.6532815 0.2705981 -0.6532815 0.2705981"/> -->
          <body>
            <inertial pos="0 0 0" mass="0.03" diaginertia="0.0002253 6.493e-05 0.0002502"/>
            <joint name="bl_y2" axis="0 0 1" range="-3.14 0.0"/>
            <geom type="mesh" mesh="Lower Leg Round Left Side" material="ABS"/>
          </body>
        </body>
      </body>
    </body>
  </worldbody>

  <actuator>
    <!-- gear: This attribute scales the length (and consequently moment arms, velocity and force) of the actuator, for all transmission types.
             It is different from the gain in the force generation mechanism, because the gain only scales the force output and does not affect
             the length, moment arms and velocity. For actuators with scalar transmission, only the first element of this vector is used.
             The remaining elements are needed for joint, jointinparent and site transmissions where this attribute is used to specify 3D force
             and torque axes.  -->
    <!-- joint: This attribute determines the type of actuator transmission. If this attribute is specified, the actuator acts on the given joint.
              For hinge and slide joints, the actuator length equals the joint position/angle times the first element of gear.  -->
    <!-- kp: Position feedback gain.  -->
    <!-- forcerange: Range for clamping the force output. The compiler expects the first value to be no greater than the second value.  -->
    <position gear="1" joint="fr_x" name="fr_x"   kp="300" forcerange="-9.4 9.4"/>
    <position gear="1" joint="fr_y" name="fr_y"   kp="300" forcerange="-9.4 9.4"/>
    <position gear="1" joint="fr_y2" name="fr_y2" kp="300" forcerange="-9.4 9.4"/>

    <position gear="1" joint="fl_x" name="fl_x"   kp="300" forcerange="-9.4 9.4"/>
    <position gear="1" joint="fl_y" name="fl_y"   kp="300" forcerange="-9.4 9.4"/>
    <position gear="1" joint="fl_y2" name="fl_y2" kp="300" forcerange="-9.4 9.4"/>

    <position gear="1" joint="br_x" name="br_x"   kp="300" forcerange="-9.4 9.4"/>
    <position gear="1" joint="br_y" name="br_y"   kp="300" forcerange="-9.4 9.4"/>
    <position gear="1" joint="br_y2" name="br_y2" kp="300" forcerange="-9.4 9.4"/>

    <position gear="1" joint="bl_x" name="bl_x"   kp="300" forcerange="-9.4 9.4"/>
    <position gear="1" joint="bl_y" name="bl_y"   kp="300" forcerange="-9.4 9.4"/>
    <position gear="1" joint="bl_y2" name="bl_y2" kp="300" forcerange="-9.4 9.4"/>
  </actuator>
</mujoco>
