<mujoco model="arm3d">

    <compiler inertiafromgeom="true" angle="radian" coordinate="local" />
    <option timestep="0.01" gravity="0 0 0" iterations="20" integrator="Euler" />
    <default>
        <joint armature="0.04" damping="1" limited="true" />
        <geom friction=".5 .1 .1" margin="0.002" condim="1" contype="0" conaffinity="0" />
    </default>
    <visual>
        <map shadowclip="100" shadowscale="0.3"/>
        <headlight ambient="0.8 0.8 0.8" />
    </visual>

    <asset>
        <material name="non-reflect" specular="0" shininess="0"/>
    </asset>

    <worldbody>
        <body name="r_shoulder_pan_link" pos="0 -0.188 0">
            <geom name="e1" type="sphere" rgba="0.6 0.6 0.6 1" pos="-0.06 0.05 0.2" size="0.05" />
            <geom name="e2" type="sphere" rgba="0.6 0.6 0.6 1" pos=" 0.06 0.05 0.2" size="0.05" />
            <geom name="e1p" type="sphere" rgba="0.1 0.1 0.1 1" pos="-0.06 0.09 0.2" size="0.03" />
            <geom name="e2p" type="sphere" rgba="0.1 0.1 0.1 1" pos=" 0.06 0.09 0.2" size="0.03" />
            <geom name="sp" type="capsule" fromto="0 0 -0.4 0 0 0.2" size="0.1" contype="1" conaffinity="1" rgba="0.25 0.25 0.25 1"/>
            <joint name="r_shoulder_pan_joint" type="hinge" pos="0 0 0" axis="0 0 1" range="-2.2854 1.714602" damping="10.0" />


            <body name="r_shoulder_lift_link" pos="0.1 0 0">
                <geom name="sl" type="capsule" fromto="0 -0.1 0 0 0.1 0" size="0.1" contype="1" conaffinity="1" rgba="0.25 0.25 0.25 1"/>
                <joint name="r_shoulder_lift_joint" type="hinge" pos="0 0 0" axis="0 1 0" range="-0.5236 1.3963" damping="10.0" />

                <body name="r_upper_arm_roll_link" pos="0 0 0">
                    <geom name="uar" type="capsule" fromto="-0.1 0 0 0.1 0 0" size="0.02" rgba="0.25 0.25 0.25 1" />
                    <joint name="r_upper_arm_roll_joint" type="hinge" pos="0 0 0" axis="1 0 0" range="-3.9 0.8" damping="0.1" />

                    <body name="r_upper_arm_link" pos="0 0 0">
                        <geom name="ua" type="capsule" fromto="0 0 0 0.4 0 0" size="0.06"  contype="1" conaffinity="1" rgba="0.25 0.25 0.25 1"/>

                        <body name="r_elbow_flex_link" pos="0.4 0 0">
                            <geom name="ef" type="capsule" fromto="0 -0.02 0 0.0 0.02 0" size="0.06" rgba="0.25 0.25 0.25 1"/>
                            <joint name="r_elbow_flex_joint" type="hinge" pos="0 0 0" axis="0 1 0" range="-2.3213 0" damping="1.0" />

                            <body name="r_forearm_roll_link" pos="0 0 0">
                                <geom name="fr" type="capsule" fromto="-0.1 0 0 0.1 0 0" size="0.02" rgba="0.25 0.25 0.25 1"/>
                                <joint name="r_forearm_roll_joint" type="hinge" pos="0 0 0" axis="1 0 0" range="-3.15 3.15" damping=".1" />

                                <body name="r_forearm_link" pos="0 0 0">
                                    <geom name="fa" type="capsule" fromto="0 0 0 0.321 0 0" size="0.05"  contype="1" conaffinity="1" rgba="0.25 0.25 0.25 1"/>

                                    <body name="r_wrist_flex_link" pos="0.321 0 0">
                                        <geom name="wf" type="capsule" fromto="0 -0.02 0 0 0.02 0" size="0.01" rgba="0.25 0.25 0.25 1"/>
                                        <joint name="r_wrist_flex_joint" type="hinge" pos="0 0 0" axis="0 1 0" range="-2.094 0" damping=".1" />

                                        <body name="r_wrist_roll_link" pos="0 0 0">
                                            <geom name="wr" type="capsule" fromto="-0.02 0 0 0.02 0 0" size="0.01" rgba="0.25 0.25 0.25 1"/>
                                            <joint name="r_wrist_roll_joint" type="hinge" pos="0 0 0" axis="1 0 0" range="-3.15 3.15" damping="0.1" />

                                            <body name="r_gripper_palm_link" pos="0 0 0">
                                                <geom name="pl" type="capsule" fromto="0.05 0 -0.02 0.05 0 0.02" size="0.05" rgba="0.25 0.25 0.25 1"/>

                                                <body name="r_gripper_tool_frame" pos="0.18 0 0">
                                                    <site name="leg_bottom" pos="0 0 -0.25" size="0.01" />
                                                    <site name="leg_top" pos="0 0 0.05" size="0.01" />
                                                    <site name="leg_right" pos="0.15 0 -0.1" size="0.01" />
                                                    <body name="key" pos="0 0 -0.0">
                                                        <geom name="key_geom" rgba="0.545098 0.270588 0.07450987 1" type="cylinder" fromto="0 0 -0.25 0 0 0.05" size="0.028" density="2000" contype="1" conaffinity="1" />
                                                    </body>
                                                    <body name="key_head1" pos="0 -0.028 -0.225">
                                                        <geom name="key_head_geom1" rgba="0.545098 0.270588 0.07450987 1" type="cylinder" fromto="0 0 0 0 0 -0.025" size="0.028" density="2000" contype="1" conaffinity="1" />
                                                    </body>
                                                    <body name="key_head2" pos="0 -0.056 -0.225">
                                                        <geom name="key_head_geom2" rgba="0.545098 0.270588 0.07450987 1" type="cylinder" fromto="0 0 0 0 0 -0.025" size="0.028" density="2000" contype="1" conaffinity="1" />
                                                    </body>
                                                </body>

                                                <body name="r_gripper_l_finger_link" pos="0.07691 0.03 0">
                                                    <geom name="gf3" type="capsule" fromto="0 0 0 0.09137 0.00495 0" size="0.01" contype="1" conaffinity="1" rgba="0.25 0.25 0.25 1"/>

                                                    <body name="r_gripper_l_finger_tip_link" pos="0.09137 0.00495 0">
                                                        <geom name="gf4" type="capsule" fromto="0 0 0 0.09137 0.0 0" size="0.01" contype="1" conaffinity="1" rgba="0.25 0.25 0.25 1"/>
                                                    </body>
                                                </body>

                                                <body name="r_gripper_r_finger_link" pos="0.07691 -0.03 0">
                                                    <geom name="gf1" type="capsule" fromto="0 0 0 0.09137 -0.00495 0" size="0.01" contype="1" conaffinity="1" rgba="0.25 0.25 0.25 1"/>

                                                    <body name="r_gripper_r_finger_tip_link" pos="0.09137 -0.00495 0">
                                                        <geom name="gf2" type="capsule" fromto="0 0 0 0.09137 0.0 0" size="0.01" contype="1" conaffinity="1" rgba="0.25 0.25 0.25 1"/>
                                                    </body>
                                                </body>
                                            </body>
                                        </body>
                                    </body>
                                </body>
                            </body>
                        </body>
                    </body>
                </body>
            </body>
        </body>

        <!--<body name="debug" pos="0.0 0.3 -0.55">-->
            <!--<geom name="debug" type="sphere" rgba="0.0 1.0 0.0 0.5" pos="0.0 0.0 0.0" size="0.2" />-->
        <!--</body>-->
        <!--<body name="debug3" pos="0.0 0.3 -0.55">-->
            <!--<geom name="debug3" type="sphere" rgba="0.0 1.0 0.0 0.5" pos="0.0 0.0 0.0" size="0.4" />-->
        <!--</body>-->

        <!--<body name="debug2" pos="0.0 0.3 -0.7">-->
            <!--<geom name="debug2" type="sphere" rgba="1.0 0.0 0.0 0.5" pos="0.0 0.0 0.0" size="0.4" />-->
        <!--</body>-->

        <body name="g1" pos="0.0 0.3 -0.7">
            <geom name="g1" type="sphere" rgba="1.0 0.0 0.0 1" pos="0.0 0.0 0.0" size="0.01" />
        </body>
        <body name="g2" pos="0.0 0.3 -0.4">
            <geom name="g2" type="sphere" rgba="1.0 0.0 0.0 1" pos="0.0 0.0 0.0" size="0.01" />
        </body>
        <body name="g3" pos="-0.15 0.3 -0.55">
            <geom name="g3" type="sphere" rgba="1.0 0.0 0.0 1" pos="0.0 0.0 0.0" size="0.01" />
        </body>

        <body name="w1" pos="0.216 0.3 -0.55">
            <geom name="w1" rgba="0.2 0.2 0.8 1" type="box" size="0.183 0.469 0.025" contype="1" conaffinity="1" />
        </body>

        <body name="w2" pos="-0.216 0.3 -0.55">
            <geom name="w2" rgba="0.2 0.2 0.8 0.5" type="box" size="0.183 0.469 0.025" contype="1" conaffinity="1" />
        </body>

        <body name="w3" pos="0.0 0.586 -0.55">
            <geom name="w3" rgba="0.2 0.2 0.8 0.5" type="box" size="0.032 0.183 0.025" contype="1" conaffinity="1" />
        </body>

        <body name="w4" pos="0.0 0.084 -0.55">
            <geom name="w4" rgba="0.2 0.2 0.8 0.5" type="box" size="0.032 0.19 0.025" contype="1" conaffinity="1" />  <!--<0.19 was 0.183>-->
        </body>

        <body name="wa1" pos="0.256 0.3 -0.62">
            <geom name="wa1" rgba="0.2 1 1 0.5" type="box" size="0.143 0.469 0.025" contype="1" conaffinity="1" />
        </body>

        <body name="wa2" pos="-0.216 0.3 -0.62">
            <geom name="wa2" rgba="0.2 1 1 0.5" type="box" size="0.183 0.469 0.025" contype="1" conaffinity="1" />
        </body>

        <body name="wa3" pos="0.0 0.586 -0.62">
            <geom name="wa3" rgba="0.2 1 1 0.5" type="box" size="0.032 0.183 0.025" contype="1" conaffinity="1" />
        </body>

        <body name="wa4" pos="0.0 0.084 -0.62">
            <geom name="wa4" rgba="0.2 1 1 0.5" type="box" size="0.032 0.19 0.025" contype="1" conaffinity="1" />  <!--<0.19 was 0.183>-->
        </body>

        <body name="wb1" pos="0.0 0.38 -0.648">
            <geom name="wb1" rgba="0.1 0.5 0.1 0.5" type="box" size="0.183 0.025 0.002" contype="1" conaffinity="1" />
        </body>

        <body name="wb2" pos="0.0 0.38 -0.695">
            <geom name="wb2" rgba="0.2 0.8 0.2 0.5" type="box" size="0.183 0.15 0.002" contype="1" conaffinity="1" />
        </body>

        <body name="wb3" pos="-0.216 0.3 -0.6715">
            <geom name="wb3" rgba="0.9 0.2 0.8 0.5" type="box" size="0.183 0.469 0.0205" contype="1" conaffinity="1" />
        </body>

        <body name="background" pos="0 -0.5 0">
            <geom name="background" rgba="1 1 1 1" type="box" size="2 0.01 2" contype="2" conaffinity="2" material="non-reflect"/>
        </body>


    </worldbody>

    <actuator>
        <motor joint="r_shoulder_pan_joint" ctrlrange="-1.0 1.0" ctrllimited="true" />
        <motor joint="r_shoulder_lift_joint" ctrlrange="-1.0 1.0" ctrllimited="true" />
        <motor joint="r_upper_arm_roll_joint" ctrlrange="-1.0 1.0" ctrllimited="true" />
        <motor joint="r_elbow_flex_joint" ctrlrange="-1.0 1.0" ctrllimited="true" />
        <motor joint="r_forearm_roll_joint" ctrlrange="-1.0 1.0" ctrllimited="true" />
        <motor joint="r_wrist_flex_joint" ctrlrange="-1.0 1.0" ctrllimited="true" />
        <motor joint="r_wrist_roll_joint" ctrlrange="-1.0 1.0" ctrllimited="true" />
    </actuator>

</mujoco>

