<?xml version="1.0" encoding="utf-8"?>
<mujoco model="qube">
    <compiler angle="radian" coordinate="local" meshdir="../meshes/" inertiafromgeom="auto"/>
    <option gravity="0 0 -9.81" integrator="Euler"/>

    <!-- Materials & Assets -->
    <asset>
        <!-- Assets -->
        <include file="shared_assets.xml"/>

        <!-- Textures -->
        <texture type="2d" name="texbody" builtin="flat" rgb1="0 0 0" width="1024" height="1024" />
        <texture type="2d" name="texfloor" builtin="flat" rgb1="1 1 1" width="1024" height="1024" />
        <texture type="2d" name="texred" builtin="flat" rgb1="0.721569 0.25098 0.235294" width="1024" height="1024" />
        <texture type="2d" name="texsilver" builtin="flat" rgb1="0.7529 0.7529 0.7529" width="512" height="512" />

        <!-- Materials -->
        <material name="floorMat" texture="texfloor" specular="1.0" shininess="0.1" />
        <material name="bodyMat" texture="texbody" specular="0.75" shininess="0.3" />
        <material name="redMat" texture="texred" specular="1.0" shininess="0.9" reflectance="1.0"/>
        <material name="silverMat" texture="texsilver" specular="0.75" shininess="0.6" reflectance="1.0"/>
    </asset>

    <visual>
        <global fovy="45" />
    </visual>

    <worldbody>
        <light diffuse=".8 .8 .8" dir="0 0 -1" directional="false" pos="0.051036 -0.032181818 0.5" specular="0.9 0.9 0.9" castshadow="true"/>

        <camera pos="-5 -5 15" name="view" euler="-30 0 -45"/>

        <geom conaffinity="0" contype="0" name="ground" pos="0 0 0" size="5 5 1" type="plane" material="floorMat"/>

        <!-- <body name="cube" pos="0 -0.026 0.15">
            <geom name="cube:base" pos="0 0 0" type="mesh" contype="0" conaffinity="0" condim="1" friction="0 0 0" material="bodyMat" mesh="cube" />
        </body> -->

        <!-- Qube -->
        <body name="base" pos="0 0 0" quat="0 0 0 1">
            <inertial pos="0.000323 -0.00132 0.054992" mass="1.0" fullinertia="0.16666666667 0.16666666667 0.16666666667 0.000000000 0.000000000 0.000000000" />
            <geom name="qube0:base" pos="0.000323 -0.00132 0.054992" type="mesh" contype="0" conaffinity="0" condim="1" friction="0 0 0" material="bodyMat" mesh="qube0:base" />
            <geom name="qube0:base:screw:01" pos="0.009938 0.009932 0.10653" type="mesh" contype="0" conaffinity="0" condim="1" friction="0 0 0" material="silverMat" mesh="qube0:base:screw:01" />
            <geom name="qube0:base:screw:02" pos="0.009938 -0.009927 0.10653" type="mesh" contype="0" conaffinity="0" condim="1" friction="0 0 0" material="silverMat" mesh="qube0:base:screw:02" />
            <geom name="qube0:base:screw:03" pos="-0.009938 0.009932 0.10653" type="mesh" contype="0" conaffinity="0" condim="1" friction="0 0 0" material="silverMat" mesh="qube0:base:screw:03" />
            <geom name="qube0:base:screw:04" pos="-0.009938 -0.009931 0.10653" type="mesh" contype="0" conaffinity="0" condim="1" friction="0 0 0" material="silverMat" mesh="qube0:base:screw:04" />
            <geom name="qube0:base:aux:female" pos="0 -0.026394 0.1078" type="mesh" contype="0" conaffinity="0" condim="1" friction="0 0 0" material="silverMat" mesh="qube0:base:aux:female" />


            <body name="cable" pos="0 -0.002359 0.1667">
                <!-- <geom name="qube:cable" type="mesh" contype="0" conaffinity="0" condim="1" friction="0 0 0" material="bodyMat" mesh="qube0:cable" /> -->
                <geom name="qube:cable:aux:male" pos="0 -0.023859 -0.0447" type="mesh" contype="0" conaffinity="0" condim="1" friction="0 0 0" material="bodyMat" mesh="qube0:cable:aux:male" />
            </body>

            <body name="motor" pos="0 0 0.12672">
                <inertial pos="0 0.008561 0.00125" mass="0.088967482" fullinertia="0.00001955483012 0.00000622772374 0.00001955483012 0.000000000 0.000000000 0.000000000" />
                <geom name="qube:motor:main" pos="0 -0.000486 0.00135" type="mesh" contype="0" conaffinity="0" condim="1" friction="0 0 0" material="redMat" mesh="qube0:motor:main" />
                <geom name="qube:motor:drive" pos="0.000009 -0.000053 -0.01472" type="mesh" contype="0" conaffinity="0" condim="1" friction="0 0 0" material="silverMat" mesh="qube0:motor:drive" />
                <geom name="qube:motor:front" pos="0.000031 0.025329 0.00124" type="mesh" contype="0" conaffinity="0" condim="1" friction="0 0 0" material="bodyMat" mesh="qube0:motor:front" />
                <geom name="qube:motor:part" pos="0 0.015289 -0.01559" type="mesh" contype="0" conaffinity="0" condim="1" friction="0 0 0" material="silverMat" mesh="qube0:motor:part" />

                <joint axis="0 0 1" name="base_motor" range="-90 90" type="hinge" damping="3e-04"/>

                <body name="arm" pos="0 0.054882 0">
                    <inertial pos="0 0 0" mass="0.006032518" fullinertia="0.000002500 0.000000030 0.000002500 0.000000000 0.000000000 0.000000000" />
                    <geom name="qube:arm" pos="0 0 0" type="mesh" contype="0" conaffinity="0" condim="1" friction="0 0 0" material="silverMat" mesh="qube0:arm" />

                    <joint axis="0 1 0" name="arm_pole" range="-90 90" type="hinge" damping="3.5e-05"/>

                    <body name="pole" pos="0 0.030562 0">
                        <inertial pos="-0.000022 0 0.05851" mass="0.024" fullinertia="0.000031118 0.000031122 0.000000273 0.000000000 0.000000000 0.000000000" />
                        <geom name="qube:pole" pos="-0.000022 0 0.05851" type="mesh" contype="0" conaffinity="0" condim="1" friction="0 0 0" material="redMat" mesh="qube0:pole" />
                    </body> 
                </body> 

            </body>            
        </body>

        <body name="B0" pos="0 -0.026 0.149" quat="-0.707107 0 0.707107 0">
            <inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="8.12887e-05" diaginertia="9.41594e-10 9.41594e-10 9.14498e-11" />
            <geom name="G0" size="0.0015 0.005" quat="0.707107 0 0.707107 0" type="capsule" />
            <body name="B1" pos="0.01 0 0">
                <inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="8.12887e-05" diaginertia="9.41594e-10 9.41594e-10 9.14498e-11" />
                <joint name="J0_1" pos="-0.005 0 0" axis="0 1 0" group="3" />
                <joint name="J1_1" pos="-0.005 0 0" axis="0 0 1" group="3" />
                <joint name="JT1" pos="-0.005 0 0" axis="1 0 0" group="3" stiffness="1" />
                <geom name="G1" size="0.0015 0.005" quat="0.707107 0 0.707107 0" type="capsule" />
                <body name="B2" pos="0.01 0 0">
                    <inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="8.12887e-05" diaginertia="9.41594e-10 9.41594e-10 9.14498e-11" />
                    <joint name="J0_2" pos="-0.005 0 0" axis="0 1 0" group="3" />
                    <joint name="J1_2" pos="-0.005 0 0" axis="0 0 1" group="3" />
                    <joint name="JT2" pos="-0.005 0 0" axis="1 0 0" group="3" stiffness="1" />
                    <geom name="G2" size="0.0015 0.005" quat="0.707107 0 0.707107 0" type="capsule" />
                    <body name="B3" pos="0.01 0 0">
                        <inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="8.12887e-05" diaginertia="9.41594e-10 9.41594e-10 9.14498e-11" />
                        <joint name="J0_3" pos="-0.005 0 0" axis="0 1 0" group="3" />
                        <joint name="J1_3" pos="-0.005 0 0" axis="0 0 1" group="3" />
                        <joint name="JT3" pos="-0.005 0 0" axis="1 0 0" group="3" stiffness="1" />
                        <geom name="G3" size="0.0015 0.005" quat="0.707107 0 0.707107 0" type="capsule" />
                        <body name="B4" pos="0.009807852804032305 0.0019509032201612826 0.0">
                            <inertial pos="0 0 0" quat="0.6935201372700871 -0.1379497323298584 0.6935201372700871 0.1379497323298584" mass="8.12887e-05" diaginertia="9.41594e-10 9.41594e-10 9.14498e-11" />
                            <joint name="J0_4" pos="-0.005 0 0" axis="0 1 0" group="3" />
                            <joint name="J1_4" pos="-0.005 0 0" axis="0 0 1" group="3" />
                            <joint name="JT4" pos="-0.005 0 0" axis="1 0 0" group="3" stiffness="1" />
                            <geom name="G4" size="0.0015 0.005" quat="0.6935201372700871 -0.1379497323298584 0.6935201372700871 0.1379497323298584" type="capsule" />
                            <body name="B5" pos="0.008314696123025453 0.005555702330196022 0.0">
                                <inertial pos="0 0 0" quat="0.6532816845954584 -0.2705981338093816 0.6532816845954584 0.2705981338093816" mass="8.12887e-05" diaginertia="9.41594e-10 9.41594e-10 9.14498e-11" />
                                <joint name="J0_5" pos="-0.005 0 0" axis="0 1 0" group="3" />
                                <joint name="J1_5" pos="-0.005 0 0" axis="0 0 1" group="3" />
                                <joint name="JT5" pos="-0.005 0 0" axis="1 0 0" group="3" stiffness="1" />
                                <geom name="G5" size="0.0015 0.005" quat="0.6532816845954584 -0.2705981338093816 0.6532816845954584 0.2705981338093816" type="capsule" />
                                <body name="B6" pos="0.005555702330196023 0.008314696123025453 0.0">
                                    <inertial pos="0 0 0" quat="0.5879379831464159 -0.39284760075979186 0.5879379831464159 0.39284760075979186" mass="8.12887e-05" diaginertia="9.41594e-10 9.41594e-10 9.14498e-11" />
                                    <joint name="J0_6" pos="-0.005 0 0" axis="0 1 0" group="3" />
                                    <joint name="J1_6" pos="-0.005 0 0" axis="0 0 1" group="3" />
                                    <joint name="JT6" pos="-0.005 0 0" axis="1 0 0" group="3" stiffness="1" />
                                    <geom name="G6" size="0.0015 0.005" quat="0.5879379831464159 -0.39284760075979186 0.5879379831464159 0.39284760075979186" type="capsule" />
                                    <body name="B7" pos="0.0019509032201612834 0.009807852804032305 0.0">
                                        <inertial pos="0 0 0" quat="0.5000001547244761 -0.500000154724476 0.5000001547244761 0.500000154724476" mass="8.12887e-05" diaginertia="9.41594e-10 9.41594e-10 9.14498e-11" />
                                        <joint name="J0_7" pos="-0.005 0 0" axis="0 1 0" group="3" />
                                        <joint name="J1_7" pos="-0.005 0 0" axis="0 0 1" group="3" />
                                        <joint name="JT7" pos="-0.005 0 0" axis="1 0 0" group="3" stiffness="1" />
                                        <geom name="G7" size="0.0015 0.005" quat="0.5000001547244761 -0.500000154724476 0.5000001547244761 0.500000154724476" type="capsule" />
                                        <body name="B8" pos="-0.001950903220161282 0.009807852804032305 0.0">
                                            <inertial pos="0 0 0" quat="0.3928476007597919 -0.5879379831464159 0.3928476007597919 0.5879379831464159" mass="8.12887e-05" diaginertia="9.41594e-10 9.41594e-10 9.14498e-11" />
                                            <joint name="J0_8" pos="-0.005 0 0" axis="0 1 0" group="3" />
                                            <joint name="J1_8" pos="-0.005 0 0" axis="0 0 1" group="3" />
                                            <joint name="JT8" pos="-0.005 0 0" axis="1 0 0" group="3" stiffness="1" />
                                            <geom name="G8" size="0.0015 0.005" quat="0.3928476007597919 -0.5879379831464159 0.3928476007597919 0.5879379831464159" type="capsule" />
                                            <body name="B9" pos="-0.00555570233019602 0.008314696123025454 0.0">
                                                <inertial pos="0 0 0" quat="0.2705981338093816 -0.6532816845954584 0.2705981338093816 0.6532816845954584" mass="8.12887e-05" diaginertia="9.41594e-10 9.41594e-10 9.14498e-11" />
                                                <joint name="J0_9" pos="-0.005 0 0" axis="0 1 0" group="3" />
                                                <joint name="J1_9" pos="-0.005 0 0" axis="0 0 1" group="3" />
                                                <joint name="JT9" pos="-0.005 0 0" axis="1 0 0" group="3" stiffness="1" />
                                                <geom name="G9" size="0.0015 0.005" quat="0.2705981338093816 -0.6532816845954584 0.2705981338093816 0.6532816845954584" type="capsule" />
                                                <body name="B10" pos="-0.008314696123025454 0.005555702330196022 0.0">
                                                    <inertial pos="0 0 0" quat="0.13794973232985847 -0.6935201372700871 0.13794973232985847 0.6935201372700871" mass="8.12887e-05" diaginertia="9.41594e-10 9.41594e-10 9.14498e-11" />
                                                    <joint name="J0_10" pos="-0.005 0 0" axis="0 1 0" group="3" />
                                                    <joint name="J1_10" pos="-0.005 0 0" axis="0 0 1" group="3" />
                                                    <joint name="JT10" pos="-0.005 0 0" axis="1 0 0" group="3" stiffness="1" />
                                                    <geom name="G10" size="0.0015 0.005" quat="0.13794973232985847 -0.6935201372700871 0.13794973232985847 0.6935201372700871" type="capsule" />
                                                    <body name="B11" pos="-0.009807852804032305 0.001950903220161286 0.0">
                                                        <inertial pos="0 0 0" quat="4.329781621023438e-17 -0.707107 4.329781621023438e-17 0.707107" mass="8.12887e-05" diaginertia="9.41594e-10 9.41594e-10 9.14498e-11" />
                                                        <joint name="J0_11" pos="-0.005 0 0" axis="0 1 0" group="3" />
                                                        <joint name="J1_11" pos="-0.005 0 0" axis="0 0 1" group="3" />
                                                        <joint name="JT11" pos="-0.005 0 0" axis="1 0 0" group="3" stiffness="1" />
                                                        <geom name="G11" size="0.0015 0.005" quat="4.329781621023438e-17 -0.707107 4.329781621023438e-17 0.707107" type="capsule" />
                                                        <body name="B12" pos="-0.009807852804032305 0 0.0">
                                                            <inertial pos="0 0 0" quat="4.329781621023438e-17 -0.707107 4.329781621023438e-17 0.707107" mass="8.12887e-05" diaginertia="9.41594e-10 9.41594e-10 9.14498e-11" />
                                                            <joint name="J0_12" pos="-0.005 0 0" axis="0 1 0" group="3" />
                                                            <joint name="J1_12" pos="-0.005 0 0" axis="0 0 1" group="3" />
                                                            <joint name="JT12" pos="-0.005 0 0" axis="1 0 0" group="3" stiffness="1" />
                                                            <geom name="G12" size="0.0015 0.005" quat="4.329781621023438e-17 -0.707107 4.329781621023438e-17 0.707107" type="capsule" />
                                                            <body name="B13" pos="-0.009807852804032305 0 0.0">
                                                                <inertial pos="0 0 0" quat="4.329781621023438e-17 -0.707107 4.329781621023438e-17 0.707107" mass="8.12887e-05" diaginertia="9.41594e-10 9.41594e-10 9.14498e-11" />
                                                                <joint name="J0_13" pos="-0.005 0 0" axis="0 1 0" group="3" />
                                                                <joint name="J1_13" pos="-0.005 0 0" axis="0 0 1" group="3" />
                                                                <joint name="JT13" pos="-0.005 0 0" axis="1 0 0" group="3" stiffness="1" />
                                                                <geom name="G13" size="0.0015 0.005" quat="4.329781621023438e-17 -0.707107 4.329781621023438e-17 0.707107" type="capsule" />
                                                                <body name="B14" pos="-0.009807852804032305 0 0.0">
                                                                    <inertial pos="0 0 0" quat="4.329781621023438e-17 -0.707107 4.329781621023438e-17 0.707107" mass="8.12887e-05" diaginertia="9.41594e-10 9.41594e-10 9.14498e-11" />
                                                                    <joint name="J0_14" pos="-0.005 0 0" axis="0 1 0" group="3" />
                                                                    <joint name="J1_14" pos="-0.005 0 0" axis="0 0 1" group="3" />
                                                                    <joint name="JT14" pos="-0.005 0 0" axis="1 0 0" group="3" stiffness="1" />
                                                                    <geom name="G14" size="0.0015 0.005" quat="4.329781621023438e-17 -0.707107 4.329781621023438e-17 0.707107" type="capsule" />
                                                                    <body name="B15" pos="-0.009807852804032305 0 0.0">
                                                                        <inertial pos="0 0 0" quat="4.329781621023438e-17 -0.707107 4.329781621023438e-17 0.707107" mass="8.12887e-05" diaginertia="9.41594e-10 9.41594e-10 9.14498e-11" />
                                                                        <joint name="J0_15" pos="-0.005 0 0" axis="0 1 0" group="3" />
                                                                        <joint name="J1_15" pos="-0.005 0 0" axis="0 0 1" group="3" />
                                                                        <joint name="JT15" pos="-0.005 0 0" axis="1 0 0" group="3" stiffness="1" />
                                                                        <geom name="G15" size="0.0015 0.005" quat="4.329781621023438e-17 -0.707107 4.329781621023438e-17 0.707107" type="capsule" />
                                                                        <body name="B16" pos="-0.009807852804032305 0 0.0">
                                                                            <inertial pos="0 0 0" quat="4.329781621023438e-17 -0.707107 4.329781621023438e-17 0.707107" mass="8.12887e-05" diaginertia="9.41594e-10 9.41594e-10 9.14498e-11" />
                                                                            <joint name="J0_16" pos="-0.005 0 0" axis="0 1 0" group="3" />
                                                                            <joint name="J1_16" pos="-0.005 0 0" axis="0 0 1" group="3" />
                                                                            <joint name="JT16" pos="-0.005 0 0" axis="1 0 0" group="3" stiffness="1" />
                                                                            <geom name="G16" size="0.0015 0.005" quat="4.329781621023438e-17 -0.707107 4.329781621023438e-17 0.707107" type="capsule" />
                                                                            <!-- <body name="B17" pos="-0.009807852804032305 0 0.0">
                                                                                <inertial pos="0 0 0" quat="4.329781621023438e-17 -0.707107 4.329781621023438e-17 0.707107" mass="8.12887e-05" diaginertia="9.41594e-10 9.41594e-10 9.14498e-11" />
                                                                                <joint name="J0_17" pos="-0.005 0 0" axis="0 1 0" group="3" />
                                                                                <joint name="J1_17" pos="-0.005 0 0" axis="0 0 1" group="3" />
                                                                                <joint name="JT17" pos="-0.005 0 0" axis="1 0 0" group="3" stiffness="1" />
                                                                                <geom name="G17" size="0.0015 0.005" quat="4.329781621023438e-17 -0.707107 4.329781621023438e-17 0.707107" type="capsule" />
                                                                            </body> -->
                                                                        </body>
                                                                    </body>
                                                                </body>
                                                            </body>
                                                        </body>
                                                    </body>
                                                </body>
                                            </body>
                                        </body>
                                    </body>
                                </body>
                            </body>
                        </body>
                    </body>
                </body>
            </body>
        </body>
    </worldbody>

    <equality>
        <!-- <joint joint1="JT1" polycoef="0 1 0 0 0" />
        <joint joint1="JT2" polycoef="0 1 0 0 0" />
        <joint joint1="JT3" polycoef="0 1 0 0 0" />
        <joint joint1="JT4" polycoef="0 1 0 0 0" />
        <joint joint1="JT5" polycoef="0 1 0 0 0" />
        <joint joint1="JT6" polycoef="0 1 0 0 0" />
        <joint joint1="JT7" polycoef="0 1 0 0 0" />
        <joint joint1="JT8" polycoef="0 1 0 0 0" />
        <joint joint1="JT9" polycoef="0 1 0 0 0" />
        <joint joint1="JT10" polycoef="0 1 0 0 0" />
        <joint joint1="JT11" polycoef="0 1 0 0 0" />
        <joint joint1="JT12" polycoef="0 1 0 0 0" />
        <joint joint1="JT13" polycoef="0 1 0 0 0" />
        <joint joint1="JT14" polycoef="0 1 0 0 0" />
        <joint joint1="JT15" polycoef="0 1 0 0 0" />
        <joint joint1="JT16" polycoef="0 1 0 0 0" /> -->
        <weld body1="motor" body2="B0" />
        <weld body1="cable" body2="B16" />
        <!-- <joint joint1="JT17" polycoef="0 1 0 0 0" /> -->
    </equality>

    <actuator>
        <velocity joint="base_motor" name="motor_rotation" ctrllimited="true" ctrlrange="-18 18" kv="0.0025"/>
        <!-- <motor joint="base_motor" name="motor_rotation" ctrllimited="true" ctrlrange="-18 18"/> -->
    </actuator>
</mujoco>