<!--Copyright 2020 Google LLC-->

<!--Licensed under the Apache License, Version 2.0 (the "License");-->
<!--you may not use this file except in compliance with the License.-->
<!--You may obtain a copy of the License at-->

    <!--https://www.apache.org/licenses/LICENSE-2.0-->

<!--Unless required by applicable law or agreed to in writing, software-->
<!--distributed under the License is distributed on an "AS IS" BASIS,-->
<!--WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.-->
<!--See the License for the specific language governing permissions and-->
<!--limitations under the License.-->

<mujoco model="franka_mocap_studyTable_buttons">

    <size njmax='1000' nconmax='1000'/>

    <!-- <include file="../../../../adept_models/scenes/basic_scene.xml"/> -->
    <include file="../../../../third_party/franka/assets/assets.xml"/>
    <!-- <include file="../../../../third_party/franka/assets/actuator0.xml"/> -->
    <include file="../../../../third_party/franka/assets/teleop_actuator.xml"/>
    <include file="../../../../adept_models/kitchen/assets/oven_asset.xml"/>
    <include file="../../../../adept_models/kitchen/assets/counters_asset.xml"/>
    <include file="../../../../adept_models/kitchen/assets/backwall_asset.xml"/>
    <include file="../../../../adept_models/kitchen/assets/slidecabinet_asset.xml"/>
    <include file="../../../../adept_models/kitchen/assets/hingecabinet_asset.xml"/>
    <include file="../../../../adept_models/kitchen/assets/microwave_asset.xml"/>
    <include file="../../../../adept_models/kitchen/assets/kettle_asset.xml"/>

    <!-- <visual>
    <global offwidth="2560" offheight="1920" />
    <quality shadowsize="4096" offsamples="8" />
    <map force="0.1" fogend="5" />
    </visual> -->

    <compiler inertiafromgeom='auto' inertiagrouprange='3 5' angle="radian"
              meshdir="../../../../adept_models/kitchen"
              texturedir="../../../../adept_models/kitchen"/>

    <equality>
        <!-- original constraints -->
        <!-- <weld body1="vive_controller" body2="world" solref="0.02 1" solimp=".7 .95 0.050"/>  -->
        <!-- <weld body1="vive_controller" body2="panda0_link7" solref="0.02 1" solimp="0.7 0.95 0.050"/> -->

        <!-- Set the impedance to constant 0.9, with width 0, seems to reduce penetration (ie. gripper finger w/ microwave handle) -->
        <weld body1="vive_controller" body2="panda0_link7" solref="0.02 1" solimp="0.7 0.9 0"/>

        <!-- from franka_panda_teleop.xml-->
        <!-- <weld body1="vive_controller" body2="panda0_link7" solref="0.01 1" solimp=".25 .25 0.001"/>  -->

        <!-- from Abhishek's code -->
        <!-- <weld body1="vive_controller" body2="panda0_link7" solref="0.02 1" solimp=".4 .85 .1"/> -->
    </equality>

    <worldbody>

        <!-- Mocap -->
        <!-- <body name="vive_controller" mocap="true" pos="0 0 2.89" euler="-1.57 0 -.785"> -->
        <body name="vive_controller" mocap="true" pos="-0.440 -0.092 2.026" euler="-1.57 0 -.785">
            <geom type="box" group="2" pos='0 0 .142' size="0.02 0.10 0.03" contype="0" conaffinity="0" rgba=".9 .7 .95 0.5" euler="0 0 -.785"/>
        </body>

        <site name='target' pos='0 0 0' size='0.1' rgba='0 2 0 .2'/>
        <!-- <camera name='left_cap' pos='-1.2 -0.5 1.8' quat='0.78 0.49 -0.22 -0.32' />
        <camera name='right_cap' pos='1.2 -0.5 1.8' quat='0.76 0.5 0.21 0.35'/> -->

        <!-- Robot -->
        <body pos='0. 0 1.8' euler='0 0 1.57'>
            <!-- <geom type='cylinder' size='.120 .90' pos='-.04 0 -0.90' class='panda_viz'/> -->
            <include file="../../../../third_party/franka/assets/chain0.xml"/>
        </body>

        <body name='desk' pos='-0.1 0.75 0'>

            <body name="counters1" pos="0 0 0" >
                <include file="../../../../adept_models/kitchen/assets/counters_chain.xml"/>
            </body>
            <body name="oven" pos="0 0 0" >
                <include file="../../../../adept_models/kitchen/assets/oven_chain.xml"/>
            </body>
            <body name="backwall" pos="0 0 0" >
                <include file="../../../../adept_models/kitchen/assets/backwall_chain.xml"/>
            </body>
            <body name="slidecabinet" pos="0.4 0.3 2.6" >
                <include file="../../../../adept_models/kitchen/assets/slidecabinet_chain.xml"/>
            </body>
            <body name="hingecabinet" pos="-0.504 0.28 2.6" >
                <include file="../../../../adept_models/kitchen/assets/hingecabinet_chain.xml"/>
            </body>
            <body name="microwave" pos="-0.750 -0.025 1.6" euler="0 0 0.3">
                <include file="../../../../adept_models/kitchen/assets/microwave_chain.xml"/>
            </body>
        </body>
        <body name="kettle" pos="-0.269 0.35 1.626">
            <freejoint/>
            <include file="../../../../adept_models/kitchen/assets/kettle_chain.xml"/>
        </body>

    </worldbody>


    <keyframe>
        <key qpos='0.16 -1.76 1.84 -2.51 0.36 0.79 1.55 0.00 0.0 1.25561e-05 1.57437e-07 1.25561e-05 1.57437e-07 1.25561e-05 1.57437e-07 1.25561e-05 1.57437e-07 8.24417e-05 9.48283e-05 0 0 0 0 -0.269 0.35 1.61523 1 1.34939e-19 -3.51612e-05 -7.50168e-19'/>
    </keyframe>

</mujoco>
