<mujoco model="simple_pusher_gripper">
    <include file="./common/skybox.xml"/>
    <include file="./common/materials.xml"/>
    <include file="./common/visual.xml"/>

    <compiler angle="radian" inertiafromgeom="true" meshdir="../meshes" />
    <option gravity="0 0 -9.81" integrator="RK4" timestep="0.01"/>
    <default>
        <joint armature="1" damping="1" limited="true"/>
        <geom contype="0" friction="1 0.1 0.1"/>
        <default class="ground">
            <geom material="grid" type="plane" contype="0" conaffinity="0"/>
        </default>
        <default class="link">
            <geom contype="1" conaffinity="1" fromto="0 0 0 0.1 0 0" rgba="0.0 0.4 0.6 1" size=".01" type="capsule" />
        </default>
        <default class="link-dummy">
            <geom contype="0" conaffinity="0" fromto="0 0 0 0.1 0 0" rgba="0.0 0.4 0.6 0.0" size=".01" type="capsule" />
        </default>
        <default class="link-goal">
            <geom contype="0" conaffinity="0" fromto="0 0 0 0.1 0 0" rgba="0.6 0.4 0. 0.0" size=".01" type="capsule" />
        </default>

    </default>

    <worldbody>
        <!-- Arena -->
        <light cutoff="100" diffuse="0.05 0.05 0.05" dir="0 -0.5 -2.0" directional="true" exponent="1" pos="0 0.5 2.0" specular=".1 .1 .1" />
        <geom name="ground" class="ground" pos="0 0 0" size="4 4 1" friction="1 0.005 0.0001"/>
        <camera name="cam0" pos="-0.1 0.1 0.0" quat="0.707 0.707 0 0" />
        <camera name="cam1" pos="-0.1 0.1 0.0" quat="0.707 0.707 0 0" />
        <!--
        <geom conaffinity="0" contype="0" name="ground" pos="0 0 0" rgba="0.9 0.9 0.9 1" size="1 1 10" type="plane"/>
        -->
        <!-- Arm -->
        <geom conaffinity="0" contype="0" fromto="0 0 0 0 0 0.05" name="root" rgba="0.9 0.4 0.6 1" size=".011" type="cylinder"/>
        <body name="body0" pos="0 0 .02">
            <geom class='link' name='link0'/>
            <joint axis="0 0 1" limited="false" name="joint0" pos="0 0 0" type="hinge"/>
            <body name="body1" pos="0.1 0 0">
                <joint axis="0 0 1" limited="true" name="joint1" pos="0 0 0" range="-3.0 3.0" type="hinge"/>
                <geom class='link' name='link1'/>
                <body name="body2" pos="0.1 0 0">
                    <joint axis="0 0 1" limited="true" name="joint2" pos="0 0 0" range="-3.0 3.0" type="hinge"/>
                    <geom class='link' name='link2'/>
                    <body name="body3" pos="0.1 0 0">
                        <joint axis="0 0 1" limited="true" name="joint3" pos="0 0 0" range="-3.0 3.0" type="hinge"/>
                        <geom class="link" name="link3" />
                        <body name="fingertip" pos="0.11 0 0">
                                <site name='fingertip' />
                                <geom fromto="0 -0.03 0 0 0.03 0" contype="1" conaffinity="1" name="fingertip0" pos="0 0 0" rgba="0.0 0.4 0.6 1" size=".01" type="capsule"/>
                                <geom fromto="0 -0.03 0 0.03 -0.03 0" contype="1" conaffinity="1" name="fingertip1" pos="0 0 0" rgba="0.0 0.4 0.6 1" size=".01" type="capsule"/>
                                <geom fromto="0 0.03 0 0.03 0.03 0" contype="1" conaffinity="1" name="fingertip2" pos="0 0 0" rgba="0.0 0.4 0.6 1" size=".01" type="capsule"/>
                        </body>
                    </body>
                </body>
            </body>
        </body>

        <body name="body0-goal" pos="0 0 .02">
            <geom class='link-goal' name='link0-goal'/>
            <joint axis="0 0 1" limited="false" name="joint0-goal" pos="0 0 0" type="hinge"/>
            <body name="body1-goal" pos="0.1 0 0">
                <joint axis="0 0 1" limited="true" name="joint1-goal" pos="0 0 0" range="-3.0 3.0" type="hinge"/>
                <geom class='link-goal' name='link1-goal'/>
                <body name="body2-goal" pos="0.1 0 0">
                    <joint axis="0 0 1" limited="true" name="joint2-goal" pos="0 0 0" range="-3.0 3.0" type="hinge"/>
                    <geom class='link-goal' name='link2-goal'/>
                    <body name="body3-goal" pos="0.1 0 0">
                        <joint axis="0 0 1" limited="true" name="joint3-goal" pos="0 0 0" range="-3.0 3.0" type="hinge"/>
                        <geom class='link-goal' name='link3-goal'/>
                        <body name="fingertip-goal" pos="0.11 0 0">
                                <geom fromto="0 -0.03 0 0 0.03 0" contype="0" conaffinity="0" name="fingertip0-goal" pos="0 0 0" rgba="0.6 0.4 0. 0" size=".01" type="capsule"/>
                                <geom fromto="0 -0.03 0 0.03 -0.03 0" contype="0" conaffinity="0" name="fingertip1-goal" pos="0 0 0" rgba="0.6 0.4 0. 0." size=".01" type="capsule"/>
                                <geom fromto="0 0.03 0 0.03 0.03 0" contype="0" conaffinity="0" name="fingertip2-goal" pos="0 0 0" rgba="0.6 0.4 0. 0." size=".01" type="capsule"/>
                        </body>
                    </body>
                </body>
            </body>
        </body>

        <body name="body0-dummy" pos="0 0 .02">
            <geom class='link-dummy' name='link0-dummy'/>
            <joint axis="0 0 1" limited="false" name="joint0-dummy" pos="0 0 0" type="hinge"/>
            <body name="body1-dummy" pos="0.1 0 0">
                <joint axis="0 0 1" limited="true" name="joint1-dummy" pos="0 0 0" range="-3.0 3.0" type="hinge"/>
                <geom class='link-dummy' name='link1-dummy'/>
                <body name="body2-dummy" pos="0.1 0 0">
                    <joint axis="0 0 1" limited="true" name="joint2-dummy" pos="0 0 0" range="-3.0 3.0" type="hinge"/>
                    <geom class='link-dummy' name='link2-dummy'/>
                    <body name="body3-dummy" pos="0.1 0 0">
                        <joint axis="0 0 1" limited="true" name="joint3-dummy" pos="0 0 0" range="-3.0 3.0" type="hinge"/>
                        <geom class='link-dummy' name='link3-dummy'/>
                        <body name="fingertip-dummy" pos="0.11 0 0">
                                <geom fromto="0 -0.03 0 0 0.03 0" contype="0" conaffinity="0" name="fingertip0-dummy" pos="0 0 0" rgba="0. 0.4 0.6 0" size=".01" type="capsule"/>
                                <geom fromto="0 -0.03 0 0.03 -0.03 0" contype="0" conaffinity="0" name="fingertip1-dummy" pos="0 0 0" rgba="0. 0.4 0.6 0" size=".01" type="capsule"/>
                                <geom fromto="0 0.03 0 0.03 0.03 0" contype="0" conaffinity="0" name="fingertip2-dummy" pos="0 0 0" rgba="0. 0.6 0.4 0" size=".01" type="capsule"/>
                        </body>
                    </body>
                </body>
            </body>
        </body>


        <body name="target" pos=".1 -.1 .005">
            <joint armature="0" axis="1 0 0" damping="0" limited="true" name="target_x" pos="0 0 0" range="-.4 .4" ref=".1" stiffness="0" type="slide"/>
            <joint armature="0" axis="0 1 0" damping="0" limited="true" name="target_y" pos="0 0 0" range="-.4 .4" ref="-.1" stiffness="0" type="slide"/>
            <geom conaffinity="1" contype="0" name="target" pos="0 0 0" rgba="0.2 0.2 0.2 1" size=".020 .001 .002" type="cylinder"/>
        </body>
        <body name="subgoal" pos=".1 -.1 .005">
            <geom conaffinity="0" contype="0" name="subgoal_geom" pos="0 0 0" rgba="0.2 0.9 0.2 0" size=".020 .005 .002" type="cylinder"/>
        </body>

        <body name='box' pos=".1 -.1 .02" euler="0 0 0">
            <inertial pos="0 0 0" mass="1" diaginertia="0.0001296 0.0001296 0.0001296" />
            <joint armature="0" axis="1 0 0" damping="1" limited="true" name="box_x" pos="0 0 0" range="-.4 .4" ref=".1" stiffness="0" type="slide" />
            <joint armature="0" axis="0 1 0" damping="1" limited="true" name="box_y" pos="0 0 0" range="-.4 .4" ref="-.1" stiffness="0" type="slide" />
            <geom conaffinity="1" contype="1" name="box" pos="0 0 0" rgba="0.2 0.9 0.2 1" size=".01 .01 .01" type="box" friction="1 0.005 0.0001"/>
        </body>
    </worldbody>

    <actuator>
        <velocity ctrllimited="true" ctrlrange="-1. 1." gear="10" joint="joint0"/>
        <velocity ctrllimited="true" ctrlrange="-1. 1." gear="10" joint="joint1"/>
        <velocity ctrllimited="true" ctrlrange="-1. 1." gear="10" joint="joint2"/>
        <velocity ctrllimited="true" ctrlrange="-1. 1." gear="10" joint="joint3"/>
        <!-- <position joint="joint0" kp="4." /> -->
        <!-- <position ctrllimited="true" ctrlrange="&#45;3. 3."  joint="joint1" kp="1."/> -->
        <!-- <position ctrllimited="true" ctrlrange="&#45;3. 3." joint="joint2" kp="1."/> -->
        <!-- <position ctrllimited="true" ctrlrange="&#45;3. 3." joint="joint3" kp="1."/> -->
    </actuator>
</mujoco>

