<mujoco model="iiwa_workcell">
    <compiler angle="radian" meshdir="./mesh/iiwa/" />
    <option collision="predefined" timestep="0.005" integrator="RK4" />
    <asset>
        <mesh name="link_0" file="link_0.stl" />
        <mesh name="link_1" file="link_1.stl" />
        <mesh name="link_2" file="link_2.stl" />
        <mesh name="link_3" file="link_3.stl" />
        <mesh name="link_4" file="link_4.stl" />
        <mesh name="link_5" file="link_5.stl" />
        <mesh name="link_6" file="link_6.stl" />
        <mesh name="link_7" file="link_7.stl" />
        <texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2" width="512" height="512" />
        <material name="MatPlane" reflectance="0" texture="texplane" texrepeat="1 1" texuniform="true" />
    </asset>
    <worldbody>
        <!-- Lights -->
        <light name="light0" diffuse=".5 .5 .5" pos="1.5 1.5 2"   dir="-1 -1 -.5" castshadow="false" />
        <light name="light1" diffuse=".5 .5 .5" pos="-1.5 1.5 2"  dir="1 -1 -.5" castshadow="false" />
        <light name="light2" diffuse=".5 .5 .5" pos="1.5 -1.5 2"  dir="-1 1 -.5" castshadow="false" />
        <light name="light3" diffuse=".5 .5 .5" pos="-1.5 -1.5 2" dir="1 1 -.5" castshadow="false" />
        <!-- Floor -->
        <geom name="floor" pos="0 0 0" size="5 5 .125" type="plane" material="MatPlane" condim="3" />
        <geom name="tplane" pos="0.3 0 0.77153" quat="0.707 0 0 0.707" type="plane" size="0.4 0.625 1e-3" group = "1" rgba="0 0 0 0" />
        <!-- Environment: Workcell -->
        <body name="workcell" pos="0 0 0" quat="0.707 0 0 -0.707">
            <geom name="table" size="0.4 0.625 0.385765" pos="0 0.3 0.385765" type="box" rgba="0.7 0.7 0.7 1" />
            <!-- <geom name="col0"  size="0.05 0.05 0.625"    pos="-0.3302 -0.2794 1.37153" type="box" rgba="0.7 0.7 0.7 .5" />
            <geom name="col1"  size="0.05 0.05 0.625"    pos="0.3302 -0.2794 1.37153"  type="box" rgba="0.7 0.7 0.7 .5" />
            <geom name="col2"  size="0.05 0.05 0.625"    pos="-0.3302 0.8382 1.37153"  type="box" rgba="0.7 0.7 0.7 .5" />
            <geom name="col3"  size="0.05 0.05 0.625"    pos="0.3302 0.8382 1.37153"   type="box" rgba="0.7 0.7 0.7 .5" /> -->
            <!-- <geom name="top"   size="0.475 0.7 0.1"      pos="0 0.3 1.97153"           type="box" rgba="0.7 0.7 0.7 1" /> -->
            <!-- <geom name="backwall" size="0.29845 0.0127 0.381" pos="-0.0063 1.0033 1.16206" type="box" /> -->
            <!-- <geom name="sidewall0" size="0.0127 0.1524 0.381" pos="-0.2921 0.8636 1.16206" type="box" /> -->
            <!-- <geom name="sidewall1" size="0.0127 0.1524 0.381" pos="0.2921 0.8636 1.16206" type="box" /> -->
            <!-- <geom name="shelf0" size="0.29845 0.1524 0.0127" pos="-0.0063 0.8636 0.793755" type="box" /> -->
            <!-- <geom name="shelf1" size="0.29845 0.1524 0.0127" pos="-0.0063 0.8636 1.03506" type="box" /> -->
            <!-- <geom name="shelf2" size="0.29845 0.1524 0.0127" pos="-0.0063 0.8636 1.28906" type="box" /> -->
            <!-- <geom name="shelf3" size="0.29845 0.1524 0.0127" pos="-0.0063 0.8636 1.53036" type="box" /> -->
        </body>
        <!-- Object: Red cardboard -->
        <body name="object" pos="0.5 0 0.815" quat="1 0 0 0" >
            <inertial pos="0 0 0" mass=".155" diaginertia="0.0003686049833333333 0.001255996129167 0.001460916979167" />
            <joint name="object" type="free" limited="false" />
            <geom name="object" type="box" size="0.15075 0.0745 0.0398" group="1" rgba=".9 0 0 1" />
            <site name="obj_front" pos="-0.15074 0 0" quat="1 0 0 0"          type="box" size="1e-5 0.04 0.02" group="2" rgba="1 1 0 .1" />
            <site name="obj_right" pos="0 -0.07449 0" quat="0.707 0 0 0.707"  type="box" size="1e-5 0.10 0.02" group="2" rgba="1 1 0 .1" />
            <site name="obj_left"  pos="0 +0.07449 0" quat="0.707 0 0 -0.707" type="box" size="1e-5 0.10 0.02" group="2" rgba="1 1 0 .1" />
            <site name="obj_rear"  pos="+0.15074 0 0" quat="0 0 0 1"          type="box" size="1e-5 0.04 0.02" group="2" rgba="1 1 0 .1" />
        </body>
        <!-- Object: imaginary blue cube -->
        <!-- <body name="object" pos="0.5 0 0.875" quat="1 0 0 0" >
            <inertial pos="0 0 0" mass="2." diaginertia="0.00750000000 0.00750000000 0.00750000000" />
            <joint name="object" type="free" limited="false" />
            <geom name="object" type="box" size="0.075 0.075 0.075" group="1" rgba="0 0 .8 1" />
            <site name="obj_front" pos="-0.075 0 0" quat="1 0 0 0"          type="box" size="1e-5 0.02 0.02" group="2" rgba="0 1 0 .1" />
            <site name="obj_right" pos="0 -0.075 0" quat="0.707 0 0 0.707"  type="box" size="1e-5 0.02 0.02" group="2" rgba="0 1 0 .1" />
            <site name="obj_left"  pos="0 +0.075 0" quat="0.707 0 0 -0.707" type="box" size="1e-5 0.02 0.02" group="2" rgba="0 1 0 .1" />
            <site name="obj_rear"  pos="+0.075 0 0" quat="0 0 0 1"          type="box" size="1e-5 0.02 0.02" group="2" rgba="0 1 0 .1" />
        </body> -->
        <!-- Robot: LWR iiwa 7 R820 -->
        <body name="iiwa_base" pos="0 0 0.0" quat="0.707 0 0 -0.707">
            <inertial pos="0 0 0" mass="0" diaginertia="0 0 0" />
            <body name="iiwa_link_0" pos="0 0 0" quat="1 0 0 0">
                <geom name="link0" pos="0 -0.0381 0.78106" quat="0.707105 0 0 0.707108" type="mesh" rgba="0.4 0.4 0.4 1" mesh="link_0" />
                <body name="iiwa_link_1" pos="0 -0.0381 0.93106" quat="0.707105 0 0 0.707108">
                    <inertial pos="0 -0.03 0.12" quat="0.428904 0.562175 -0.562175 0.428904" mass="3.4525" diaginertia="0.0218946 0.02183 0.00663837" />
                    <joint name="iiwa_joint_0" pos="0 0 0" axis="0 0 1" limited="true" range="-2.96706 2.96706" damping="0.5" />
                    <geom name="link1" pos="0 0 0.0075" type="mesh" rgba="1 0.423529 0.0392157 1" mesh="link_1" />
                    <body name="iiwa_link_2" pos="0 0 0.19" quat="0 0 0.707107 0.707107">
                        <inertial pos="0.0003 0.059 0.042" quat="0.701371 0.0898824 0.0898824 0.701371" mass="3.4821" diaginertia="0.02179 0.0217049 0.00684512" />
                        <joint name="iiwa_joint_1" pos="0 0 0" axis="0 0 1" limited="true" range="-2.0944 2.0944" damping="0.5" />
                        <geom name="link2" type="mesh" rgba="1 0.423529 0.0392157 1" mesh="link_2" />
                        <body name="iiwa_link_3" pos="0 0.21 0" quat="0 0 0.707107 0.707107">
                            <inertial pos="0 0.03 0.13" quat="0.56292 0.427927 -0.427927 0.56292" mass="4.05623" diaginertia="0.0321488 0.03204 0.00799117" />
                            <joint name="iiwa_joint_2" pos="0 0 0" axis="0 0 1" limited="true" range="-2.96706 2.96706" damping="0.5" />
                            <geom name="link3" pos="0 0 -0.026" type="mesh" rgba="1 0.423529 0.0392157 1" mesh="link_3" />
                            <body name="iiwa_link_4" pos="0 0 0.19" quat="0.707107 0.707107 0 0">
                                <inertial pos="0 0.067 0.034" quat="0.991887 -0.127124 0 0" mass="3.4822" diaginertia="0.02178 0.0216947 0.00684029" />
                                <joint name="iiwa_joint_3" pos="0 0 0" axis="0 0 1" limited="true" range="-2.0944 2.0944" damping="0.5" />
                                <geom name="link4" type="mesh" rgba="1 0.423529 0.0392157 1" mesh="link_4" />
                                <body name="iiwa_link_5" pos="0 0.21 0" quat="0 0 0.707107 0.707107">
                                    <inertial pos="0.0001 0.021 0.076" quat="0.365352 0.605407 -0.605407 0.365352" mass="2.1633" diaginertia="0.0131987 0.01287 0.0036293" />
                                    <joint name="iiwa_joint_4" pos="0 0 0" axis="0 0 1" limited="true" range="-2.96706 2.96706" damping="0.5" />
                                    <geom name="link5" pos="0 0 -0.026" type="mesh" rgba="1 0.423529 0.0392157 1" mesh="link_5" />
                                    <body name="iiwa_link_6" pos="0 0.0607 0.19" quat="0.707107 0.707107 0 0">
                                        <inertial pos="0 0.0006 0.0004" quat="0.996112 0.088097 0 0" mass="2.3466" diaginertia="0.006509 0.00631585 0.00447015" />
                                        <joint name="iiwa_joint_5" pos="0 0 0" axis="0 0 1" limited="true" range="-2.0944 2.0944" damping="0.5" />
                                        <geom name="link6" type="mesh" rgba="1 0.423529 0.0392157 1" mesh="link_6" />
                                        <body name="iiwa_link_7" pos="0 0.081 0.0607" quat="0 0 0.707107 0.707107">
                                            <inertial pos="0 0 0.02" quat="0.923068 0 0 0.384636" mass="3.129" diaginertia="0.0152362 0.0140538 0.002872" />
                                            <!-- <joint name="iiwa_joint_6" pos="0 0 0" axis="0 0 1" limited="true" range="-3.05433 3.05433" damping="0.5" /> -->
                                            <!-- <geom name="link7" pos="0 0 -0.0005" type="mesh" rgba="0.4 0.4 0.4 1" mesh="link_7" /> -->
                                            <geom name="link7"  pos="0 0 0.0235" type="cylinder" size="0.050 0.0235" rgba=".5 .5 .5 1" />
                                            <geom name="flange" pos="0 0 0.0595" type="cylinder" size="0.032 0.0125" rgba=".5 .5 .5 1" />
                                            <site name="end_effector" pos="0 0 0.072" type="sphere" size="1e-4" group="2" rgba="0 1 0 1" />
                                            <!-- <site name="end_effector" pos="0 0 0.0645" type="cylinder" size="0.032 0.01" group="2" rgba="0 1 0 1" /> -->
                                        </body>
                                    </body>
                                </body>
                            </body>
                        </body>
                    </body>
                </body>
            </body>
        </body>
        <!-- Ghost representing the desired pose -->
        <body name="ghost" pos="0 0 0" >
            <geom name="ghost" pos="0 0 0" type="box" size="0.15075 0.0745 0.038" group="0" rgba="0 .9 0 0.3" />
        </body>
    </worldbody>

    <!-- Actuators -->
    <actuator>
        <!-- Reduced torque limits for hardware experiments -->
        <motor name="joint0" joint="iiwa_joint_0" ctrllimited="true" ctrlrange="-80 80" />
        <motor name="joint1" joint="iiwa_joint_1" ctrllimited="true" ctrlrange="-80 80" />
        <motor name="joint2" joint="iiwa_joint_2" ctrllimited="true" ctrlrange="-60 60" />
        <motor name="joint3" joint="iiwa_joint_3" ctrllimited="true" ctrlrange="-50 50" />
        <motor name="joint4" joint="iiwa_joint_4" ctrllimited="true" ctrlrange="-40 40" />
        <motor name="joint5" joint="iiwa_joint_5" ctrllimited="true" ctrlrange="-5 5" />
        <!-- Original torque limits -->
        <!-- <motor name="joint0" joint="iiwa_joint_0" ctrllimited="true" ctrlrange="-176 176" />
        <motor name="joint1" joint="iiwa_joint_1" ctrllimited="true" ctrlrange="-176 176" />
        <motor name="joint2" joint="iiwa_joint_2" ctrllimited="true" ctrlrange="-110 110" />
        <motor name="joint3" joint="iiwa_joint_3" ctrllimited="true" ctrlrange="-110 110" />
        <motor name="joint4" joint="iiwa_joint_4" ctrllimited="true" ctrlrange="-110 110" />
        <motor name="joint5" joint="iiwa_joint_5" ctrllimited="true" ctrlrange="-40 40" /> -->
        <!-- <motor name="joint6" joint="iiwa_joint_6" ctrllimited="true" ctrlrange="-40 40" /> -->
    </actuator>

    <!-- Contacts -->
    <contact>
        <pair geom1="link6"  geom2="tplane" />
        <pair geom1="link7"  geom2="tplane" />
        <pair geom1="flange" geom2="tplane" />
        <pair geom1="link6"  geom2="object" friction=".6 .6" />
        <pair geom1="link7"  geom2="object" friction=".6 .6" />
        <pair geom1="flange" geom2="object" friction=".6 .6" />
        <pair geom1="tplane" geom2="object" friction="1.2 1.2" />
    </contact>

    <!-- Key frames -->
    <keyframe>
        <!-- <key name="init_pose" qpos="0.5 0 0.8125 1 0 0 0 0 0.86 0 -2.05 0 -1.1" /> -->
        <!-- <key name="init_pose" qpos="0.5 0 0.8125 1 0 0 0 0 0.8 0 -2.05 0 -0.85" /> -->
        <!-- <key name="init_pose" qpos="0.5 0 0.8125 1 0 0 0 0 0.7 0 -2.05 0 -0.75" /> -->
        <!-- <key name="init_pose" qpos="0.5 0 0.8125 1 0 0 0 0 0.7 0 -1.75 0 0.65" /> -->
        <key name="init_pose" qpos="0.7 0 0.8125 1 0 0 0 0 1 0 -2.05 0 -1.05" />
    </keyframe>
</mujoco>
