<!-- ======================================================
	This file is part of MuJoCo.     
	Copyright 2009-2016 Roboti LLC.
	
	Model 		:: WAM Arm from Barrett technologies
		Source		: https://github.com/jhu-lcsr/barrett_model
		Downloaded	: 10Oct'15
		
	Mujoco		:: Advanced physics simulation engine
		Source		: www.roboti.us
		Version		: 1.31
		Released 	: 23Apr'16
		
	Author		:: Vikash Kumar
		Contacts 	: kumar@roboti.us
		Last edits 	: 30Apr16, 6Dec'15
====================================================== -->

<mujoco model="wam(v1.31)">
    <compiler coordinate="local" angle="radian" meshdir="meshes/" />	
	<default>
		<joint type="hinge" limited="true" pos="0 0 0" axis="0 0 1" frictional="true" frictionloss=".001"/>
		<default class="viz">
			<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1"/>
		</default>
		<default class="col">
			<geom type="mesh" contype="0" conaffinity="1" group="0" rgba="0.5 0.6 0.7 1"/>
		</default>
	</default>
	<asset>
		<mesh file="base_link_fine.stl"/>
		<mesh file="base_link_convex.stl"/>
		<mesh file="shoulder_link_fine.stl"/>
		<mesh file="shoulder_link_convex_decomposition_p1.stl"/>
		<mesh file="shoulder_link_convex_decomposition_p2.stl"/>
		<mesh file="shoulder_link_convex_decomposition_p3.stl"/>
		<mesh file="shoulder_pitch_link_fine.stl"/>
		<mesh file="shoulder_pitch_link_convex.stl"/>
		<mesh file="upper_arm_link_fine.stl"/>
		<mesh file="upper_arm_link_convex_decomposition_p1.stl"/>
		<mesh file="upper_arm_link_convex_decomposition_p2.stl"/>
		<mesh file="elbow_link_fine.stl"/>
		<mesh file="elbow_link_convex.stl"/>
		<mesh file="forearm_link_fine.stl"/>
		<mesh file="forearm_link_convex_decomposition_p1.stl"/>
		<mesh file="forearm_link_convex_decomposition_p2.stl"/>
		<mesh file="wrist_yaw_link_fine.stl"/>
		<mesh file="wrist_yaw_link_convex_decomposition_p1.stl"/>
		<mesh file="wrist_yaw_link_convex_decomposition_p2.stl"/>
		<mesh file="wrist_pitch_link_fine.stl"/>
		<mesh file="wrist_pitch_link_convex_decomposition_p1.stl"/>
		<mesh file="wrist_pitch_link_convex_decomposition_p2.stl"/>
		<mesh file="wrist_pitch_link_convex_decomposition_p3.stl"/>
		<mesh file="wrist_palm_link_fine.stl"/>
		<mesh file="wrist_palm_link_convex.stl"/>
		
		<texture name="groundplane" type="2d" builtin="checker" rgb1=".25 .26 .25" 
			rgb2=".22 .22 .22" width="100" height="100" mark="edge" markrgb=".3 .3 .3"/>
		<material name="MatGnd" texture="groundplane"  texrepeat="5 5" specular="1" shininess=".3" reflectance="0.00001"/>
	</asset>
	<include file="include_bhandDependencies.xml"/>
	
    <worldbody>
		
		<light directional="true" cutoff="60" exponent="1" diffuse="1 1 1" specular=".1 .1 .1" pos=".1 .2 1.3" dir="-.1 -.2 -1.3"/>
		<geom name="ground" type="plane" pos="0 0 0" size="1.5 2 1" conaffinity="1" contype="1" material="MatGnd"/>

        <body name="wam/base_link" pos="0 0 .6">
            <inertial pos="0 0 0" mass="1" diaginertia="0.1 0.1 0.1" />
            <geom class="viz" mesh="base_link_fine"/>
            <geom class="col" mesh="base_link_convex"/>
            <body name="wam/shoulder_yaw_link" pos="0 0 0.346">
                <inertial pos="-0.00443422 -0.00066489 -0.128904" quat="0.69566 0.716713 -0.0354863 0.0334839" mass="5" diaginertia="0.135089 0.113095 0.0904426" />
                <joint name="wam/base_yaw_joint" range="-2.6 2.6" damping="1.98"/>
                <geom class="viz" mesh="shoulder_link_fine"/>
                <geom class="col" mesh="shoulder_link_convex_decomposition_p1"/>
                <geom class="col" mesh="shoulder_link_convex_decomposition_p2"/>
                <geom class="col" mesh="shoulder_link_convex_decomposition_p3"/>
                <body name="wam/shoulder_pitch_link" pos="0 0 0" quat="0.707107 -0.707107 0 0">
                    <inertial pos="-0.00236981 -0.0154211 0.0310561" quat="0.961794 0.273112 -0.0169316 0.00866592" mass="3.87494" diaginertia="0.0214195 0.0167127 0.0126452" /> <!--seems off-->
                    <joint name="wam/shoulder_pitch_joint" range="-1.985 1.985" damping="0.55"/>
                    <geom class="viz" mesh="shoulder_pitch_link_fine"/>
                    <geom class="col" mesh="shoulder_pitch_link_convex"/>
                    <body name="wam/upper_arm_link" pos="0 0 0" quat="0.707107 0.707107 0 0">
                        <inertial pos="0.00683259 3.309e-005 0.392492" quat="0.647136 0.0170822 0.0143038 0.762049" mass="2.20228" diaginertia="0.0592718 0.0592207 0.00313419" />
                        <joint name="wam/shoulder_yaw_joint" range="-2.8 2.8" damping="1.65"/>
                        <geom class="viz" mesh="upper_arm_link_fine"/>
                        <geom class="col" mesh="upper_arm_link_convex_decomposition_p1"/>
                        <geom class="col" mesh="upper_arm_link_convex_decomposition_p2"/>
                        <body name="wam/forearm_link" pos="0.045 0 0.55" quat="0.707107 -0.707107 0 0">
                            <inertial pos="-0.0400149 -0.142717 -0.00022942" quat="0.704281 0.706326 0.0180333 0.0690353" mass="0.500168" diaginertia="0.0151047 0.0148285 0.00275805" />
                            <joint name="wam/elbow_pitch_joint" range="-0.9 3.14159" damping="0.88"/>
                            <geom class="viz" mesh="elbow_link_fine"/>
                            <geom class="col" mesh="elbow_link_convex"/>
                            <geom class="viz" mesh="forearm_link_fine" pos="-.045 -0.0730 0" euler="1.57 0 0"/>
                            <geom class="col" mesh="forearm_link_convex_decomposition_p1" pos="-0.045 -0.0730 0" euler="1.57 0 0"/>
                            <geom class="col" mesh="forearm_link_convex_decomposition_p2" pos="-.045 -0.0730 0" euler="1.57 0 0"/>
                            <body name="wam/wrist_yaw_link" pos="-0.045 -0.3 0" quat="0.707107 0.707107 0 0">
                                <inertial pos="8.921e-005 0.00435824 -0.00511217" quat="0.630602 0.776093 0.00401969 -0.002372" mass="1.05376" diaginertia="0.000555168 0.00046317 0.000234072" /> <!--this is an approximation-->
                                <joint name="wam/wrist_yaw_joint" range="-4.55 1.25" damping="0.55"/>
                                <geom class="viz" mesh="wrist_yaw_link_fine"/>
                                <geom class="col" mesh="wrist_yaw_link_convex_decomposition_p1"/>
                                <geom class="col" mesh="wrist_yaw_link_convex_decomposition_p2"/>
                                <body name="wam/wrist_pitch_link" pos="0 0 0" quat="0.707107 -0.707107 0 0">
                                    <inertial pos="-0.00012262 -0.0246834 -0.0170319" quat="0.630602 0.776093 0.00401969 -0.002372" mass="0.517974" diaginertia="0.000555168 0.00046317 0.000234072" />
                                    <joint name="wam/wrist_pitch_joint" range="-1.5707 1.5707" damping="0.11"/>
                                    <geom class="viz" mesh="wrist_pitch_link_fine"/>
                                    <geom class="col" mesh="wrist_pitch_link_convex_decomposition_p1"/>
                                    <geom class="col" mesh="wrist_pitch_link_convex_decomposition_p2"/>
                                    <geom class="col" mesh="wrist_pitch_link_convex_decomposition_p3"/>
                                    <body name="wam/wrist_palm_link" pos="0 0 0" quat="0.707107 0.707107 0 0">
                                        <inertial pos="0 0 0.055" quat="0.707107 0 0 0.707107" mass="0.0828613" diaginertia="0.00020683 0.00010859 0.00010851" />
                                        <joint name="wam/palm_yaw_joint" range="-3 3" damping="0.11"/>
                                        <geom class="viz" mesh="wrist_palm_link_fine"/>
                                        <geom class="col" mesh="wrist_palm_link_convex"/>
										<include file="include_bhandChain.xml"/>
                                    </body>
                                </body>
                            </body>
                        </body>
                    </body>
                </body>
            </body>
        </body>
    </worldbody>
</mujoco>
