<?xml version="1.0" ?>
<mujoco model="so101">
  <option integrator="implicitfast" timestep="0.005" cone="elliptic"
   iterations="10" ls_iterations="20" impratio="10"/>

  <compiler angle="radian" meshdir="assets" autolimits="true"/>

  <visual>
    <scale contactwidth="0.1" contactheight="0.05" forcewidth="0.02"
     framelength="0.75" framewidth="0.025" camera="0.2"/>
    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
    <rgba haze="0.15 0.25 0.35 1"/>
    <global azimuth="160" elevation="-20"/>
  </visual>

  <default>
    <mesh maxhullvert="128"/>
    <default class="so101">
      <joint damping="1" frictionloss="0.1" armature="0.005"/>
      <position kp="50"/>
      <default class="visual">
        <geom type="mesh" contype="0" conaffinity="0" group="2" density="50"/>
      </default>
      <default class="collision">
        <geom type="mesh" contype="1" conaffinity="1" group="3" rgba="0.2 0.7 0.3 0.25" density="0"/>
      </default>
    </default>
    <default class="sts3215">
      <geom contype="0" conaffinity="0"/>
      <joint damping="0.60" frictionloss="0.052" armature="0.028"/>
      <position kp="998.22" kv="2.731" forcerange="-2.94 2.94"/>
    </default>
  </default>

  <asset>
    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0"
             width="512" height="3072"/>
    <mesh name="arm_redo_thin_long_pretty_medium_size_base" file="arm_redo_thin_long_pretty_medium_size_base.stl" scale="0.001 0.001 0.001"/>
    <mesh name="arm_redo_thin_long_pretty_medium_size_base_clamp" file="arm_redo_thin_long_pretty_medium_size_base_clamp.stl" scale="0.001 0.001 0.001"/>
    <mesh name="arm_redo_thin_long_pretty_medium_size_link2_clamp" file="arm_redo_thin_long_pretty_medium_size_link2_clamp.stl" scale="0.001 0.001 0.001"/>
    <mesh name="arm_redo_thin_long_pretty_medium_size_link3_clamp" file="arm_redo_thin_long_pretty_medium_size_link3_clamp.stl" scale="0.001 0.001 0.001"/>
    <mesh name="arm_redo_thin_long_pretty_medium_size_link4_clamp_redo" file="arm_redo_thin_long_pretty_medium_size_link4_clamp_redo.stl" scale="0.001 0.001 0.001"/>
    <mesh name="eef_base_with_handle" file="eef_base_with_handle.stl" scale="0.001 0.001 0.001"/>
    <mesh name="eef_thumb_with_handle" file="eef_thumb_with_handle.stl" scale="0.001 0.001 0.001"/>
    <mesh name="link2_)stronger" file="link2_)stronger.stl" scale="0.001 0.001 0.001"/>
    <texture name="aruco_tex_base" type="2d" file="assets/aruco_base.png"/>
    <material name="aruco_mat_base" texture="aruco_tex_base" rgba="1 1 1 1"/>
    <texture name="aruco_tex_shoulder" type="2d" file="assets/aruco_base_clamp.png"/>
    <material name="aruco_mat_shoulder" texture="aruco_tex_shoulder" rgba="1 1 1 1"/>
    <texture name="aruco_tex_shoulder2" type="2d" file="assets/aruco_link1_clamp.png"/>
    <material name="aruco_mat_shoulder2" texture="aruco_tex_shoulder2" rgba="1 1 1 1"/>
    <texture name="aruco_tex_shoulder3" type="2d" file="assets/aruco_link2_clamp.png"/>
    <material name="aruco_mat_shoulder3" texture="aruco_tex_shoulder3" rgba="1 1 1 1"/>
    <texture name="aruco_tex_shoulder4" type="2d" file="assets/aruco_link3_clamp.png"/>
    <material name="aruco_mat_shoulder4" texture="aruco_tex_shoulder4" rgba="1 1 1 1"/>
    <texture name="aruco_tex_shoulder5" type="2d" file="assets/aruco_link4_clamp.png"/>
    <material name="aruco_mat_shoulder5" texture="aruco_tex_shoulder5" rgba="1 1 1 1"/>
    <texture name="aruco_tex_shoulder6" type="2d" file="assets/aruco_link5_clamp.png"/>
    <material name="aruco_mat_shoulder6" texture="aruco_tex_shoulder6" rgba="1 1 1 1"/>
    <mesh file="ours_base_holder.stl" scale="0.001 0.001 0.001"/>
    <mesh file="ours_base_servo.stl" scale="0.001 0.001 0.001"/>
    <mesh file="sts3215_03a_v1.stl"/>
    <material name="yellow_mat" rgba="1 0.82 0.12 1"/>
    <material name="red_mat" rgba="1 0.0 0.0 1"/>
    <material name="green_mat" rgba="0.0 0.6 0.2 1"/>
    <material name="blue_mat" rgba="0.0 0.0 1.0 1"/>
    <material name="black_mat" rgba="0.1 0.1 0.1 1"/>
    <material name="orange_mat" rgba="1 0.5 0.0 1"/>
  </asset>

  <worldbody>
    <light pos="0 0 3.5" dir="0 0 -1" directional="true"/>
    <!-- Link 0: base -->
    <body name="base" pos="0 0 0" quat="1 0 0 0" childclass="so101">
      <geom type="mesh" class="visual" pos="-0.006365 0 -0.0024" quat="1 1 1 1" mesh="arm_redo_thin_long_pretty_medium_size_base" material="yellow_mat"/>
      <geom type="box" class="visual" contype="0" conaffinity="0" pos="-0.03286 -0.07 0.0142" quat="1 0 0 0" size="0.05 0.05 0.0005" material="aruco_mat_base"/>
      <geom type="mesh" class="visual" pos=" 0.0263353 0 0.0437" quat="1 0 0 0" mesh="sts3215_03a_v1" material="black_mat"/>
      <geom type="mesh" class="collision" pos="-0.006365 0 -0.0024" quat="1 1 1 1" mesh="arm_redo_thin_long_pretty_medium_size_base"/>
      <body name="shoulder" pos="0.0388353 0 0.0624" quat="0 0 -1 0">
        <joint axis="0 0 1" name="shoulder_pan" type="hinge" range="-2.0 2.0" class="sts3215"/>
        <geom type="mesh" class="visual" pos="0.012201 2.2e-05 0.0464" quat="1 -1 0 0" mesh="arm_redo_thin_long_pretty_medium_size_base_clamp" material="yellow_mat"/>
        <geom type="mesh" class="collision" pos="0.012201 2.2e-05 0.0464" quat="1 -1 0 0" mesh="arm_redo_thin_long_pretty_medium_size_base_clamp"/>
        <geom type="box" class="visual" contype="0" conaffinity="0" pos="-0.0550647 -0.0223 0.01838" quat="0 0 -0.707107 0.707107" size="0.015 0.015 0.0005" material="aruco_mat_shoulder"/>

                    <body name="base2" pos="-0.0611647 0.042 -0.01606" quat="0.5 0.5 0.5 -0.5" childclass="so101">
              <geom type="mesh" class="visual" pos=" 0.0263353 0 0.0437" quat="1 0 0 0" mesh="sts3215_03a_v1" material="black_mat"/>

                      <body name="shoulder2" pos="0.0388353 0 0.0624" quat="0 0 -1 0">
                <joint axis="0 0 1" name="shoulder_pan2" type="hinge" range="-2.0 2.0" class="sts3215"/>
                        <geom type="mesh" class="visual" pos="0.012201 2.2e-05 0.0464" quat="1 -1 0 0" mesh="link2_)stronger" material="red_mat"/>
                        <geom type="mesh" class="collision" pos="0.012201 2.2e-05 0.0464" quat="1 -1 0 0" mesh="link2_)stronger"/>
                        <geom type="box" class="visual" contype="0" conaffinity="0" pos="-0.0555647 0.00156 -0.0019" quat="0 0 1 0" size="0.015 0.015 0.0005" material="aruco_mat_shoulder2"/>

                        <body name="base3" pos="-0.122705 -0.017 0.0604" quat="0 -0.707107 0.707107 0" childclass="so101">
                  <geom type="mesh" class="visual" pos=" 0.0263353 0 0.0437" quat="1 0 0 0" mesh="sts3215_03a_v1" material="black_mat"/>

                          <body name="shoulder3" pos="0.0388353 0 0.0624" quat="0 0 -1 0">
                    <joint axis="0 0 1" name="shoulder_pan3" type="hinge" range="-2.0 2.0" class="sts3215"/>
                            <geom type="mesh" class="visual" pos="0.012201 2.2e-05 0.0464" quat="1 -1 0 0" mesh="arm_redo_thin_long_pretty_medium_size_link2_clamp" material="green_mat"/>
                            <geom type="mesh" class="collision" pos="0.012201 2.2e-05 0.0464" quat="1 -1 0 0" mesh="arm_redo_thin_long_pretty_medium_size_link2_clamp"/>
                            <geom type="box" class="visual" contype="0" conaffinity="0" pos="-0.0641647 0.0009 0.0024" quat="0 0 1 0" size="0.015 0.015 0.0005" material="aruco_mat_shoulder3"/>

                            <body name="base4" pos="-0.104165 -6.93889e-18 0.0624" quat="0 0 1 0" childclass="so101">
                      <geom type="mesh" class="visual" pos=" 0.0263353 0 0.0437" quat="1 0 0 0" mesh="sts3215_03a_v1" material="black_mat"/>

                              <body name="shoulder4" pos="0.0388353 0 0.0624" quat="0 0 -1 0">
                        <joint axis="0 0 1" name="shoulder_pan4" type="hinge" range="-2.0 2.0" class="sts3215"/>
                                <geom type="mesh" class="visual" pos="0.012201 2.2e-05 0.0464" quat="1 -1 0 0" mesh="arm_redo_thin_long_pretty_medium_size_link3_clamp" material="blue_mat"/>
                                <geom type="mesh" class="collision" pos="0.012201 2.2e-05 0.0464" quat="1 -1 0 0" mesh="arm_redo_thin_long_pretty_medium_size_link3_clamp"/>
                                <geom type="box" class="visual" contype="0" conaffinity="0" pos="-0.0246047 0.00092 -0.0119" quat="0 0 1 0" size="0.015 0.015 0.0005" material="aruco_mat_shoulder4"/>

                                <body name="base5" pos="-0.0261647 -0.041 0.0224" quat="0 0 0.707107 0.707107" childclass="so101">
                          <geom type="mesh" class="visual" pos=" 0.0263353 0 0.0437" quat="1 0 0 0" mesh="sts3215_03a_v1" material="black_mat"/>

                                  <body name="shoulder5" pos="0.0388353 0 0.0624" quat="0 0 -1 0">
                            <joint axis="0 0 1" name="shoulder_pan5" type="hinge" range="-2.0 2.0" class="sts3215"/>
                                    <geom type="mesh" class="visual" pos="0.012201 2.2e-05 0.0464" quat="1 -1 0 0" mesh="arm_redo_thin_long_pretty_medium_size_link4_clamp_redo" material="orange_mat"/>
                                    <geom type="mesh" class="collision" pos="0.012201 2.2e-05 0.0464" quat="1 -1 0 0" mesh="arm_redo_thin_long_pretty_medium_size_link4_clamp_redo"/>
                                    <geom type="box" class="visual" contype="0" conaffinity="0" pos="-0.0326647 -0.0214 0.0204" quat="0 0 0.707107 -0.707107" size="0.015 0.015 0.0005" material="aruco_mat_shoulder5"/>

                                    <body name="base6" pos="-0.0791647 -0.0055 0.0504" quat="0 -0.707107 0 -0.707107" childclass="so101">
                              <geom type="mesh" class="visual" pos=" 0.0263353 0 0.0437" quat="1 0 0 0" mesh="sts3215_03a_v1" material="black_mat"/>

                                      <body name="shoulder6" pos="0.0388353 0 0.0624" quat="0 0 -1 0">
                                <joint axis="0 0 1" name="shoulder_pan6" type="hinge" range="-2.0 2.0" class="sts3215"/>
                                        <geom type="mesh" class="visual" pos="0.012201 2.2e-05 0.0464" quat="1 -1 0 0" mesh="eef_base_with_handle" material="yellow_mat"/>
                                        <geom type="mesh" class="collision" pos="0.012201 2.2e-05 0.0464" quat="1 -1 0 0" mesh="eef_base_with_handle"/>
                                        <geom type="box" class="visual" contype="0" conaffinity="0" pos="-0.0356647 0.018 0.0294" quat="-0.5 0.5 -0.5 -0.5" size="0.015 0.015 0.0005" material="aruco_mat_shoulder6"/>
                                        <body name="virtual_gripper_keypoint" pos="-.1 -.02 .015">
                                          <geom type="sphere" size=".01" rgba="1 0 0 1" group="2" contype="0" conaffinity="0"/>
                                        </body>

                                        <body name="base7" pos="-0.0066647 -0.038 0.0624" quat="0 0 1 0" childclass="so101">
                                  <geom type="mesh" class="visual" pos=" 0.0263353 0 0.0437" quat="1 0 0 0" mesh="sts3215_03a_v1" material="black_mat"/>

                                          <body name="shoulder7" pos="0.0388353 0 0.0624" quat="0 0 -1 0">
                                    <joint axis="0 0 1" name="shoulder_pan7" type="hinge" range="-2.0 2.0" class="sts3215"/>
                                            <geom type="mesh" class="visual" pos="0.012201 2.2e-05 0.0464" quat="1 -1 0 0" mesh="eef_thumb_with_handle" material="red_mat"/>
                                            <geom type="mesh" class="collision" pos="0.012201 2.2e-05 0.0464" quat="1 -1 0 0" mesh="eef_thumb_with_handle"/>
                                        </body>
                                      </body>
                                    </body>
                                  </body>
                                </body>
                              </body>
                            </body>
                          </body>
                        </body>
                      </body>
                    </body>
                  </body>
                </body>
    </body>
  </worldbody>

  <!-- Adjacent shoulder excludes removed: verified that collision clamps
       do not actually overlap (sts3215 servo geom is class="visual" so
       contype=conaffinity=0). Real self-collisions only occur at extreme
       joint angles, which is exactly what we want IK to avoid. If
       gen_and_compare.sh is rerun, update generate_example_twolink_custom.py
       to skip emit_contact_excludes() to preserve this. -->



  <actuator>
    <position class="sts3215" name="shoulder_pan" joint="shoulder_pan" ctrlrange="-2.0 2.0"/>
    <position class="sts3215" name="shoulder_pan2" joint="shoulder_pan2" ctrlrange="-2.0 2.0"/>
    <position class="sts3215" name="shoulder_pan3" joint="shoulder_pan3" ctrlrange="-2.0 2.0"/>
    <position class="sts3215" name="shoulder_pan4" joint="shoulder_pan4" ctrlrange="-2.0 2.0"/>
    <position class="sts3215" name="shoulder_pan5" joint="shoulder_pan5" ctrlrange="-2.0 2.0"/>
    <position class="sts3215" name="shoulder_pan6" joint="shoulder_pan6" ctrlrange="-2.0 2.0"/>
    <position class="sts3215" name="shoulder_pan7" joint="shoulder_pan7" ctrlrange="-2.0 2.0"/>
  </actuator>

  <keyframe>
    <key name="home" qpos="0 -1.5 1.5 0.75 0 0 0" qvel="0 0 0 0 0 0 0" ctrl="0 -1.5 1.5 0.75 0 0 0"/>
  </keyframe>
</mujoco>
