<!-- =========================================================================== #
# Project:        [M3X] Whip Project
# Title:          XML Model File for 2D Upper Limb
# Author:         Moses C. Nah
# Creation Date:  Monday, July 1st, 2020

# Description: 	  This model file is simply for analyzing the forward kinematics of the upper-limb model.
#									This planar 2D_UpperLimb model has 1DOF on the shoulder joint and 1DOF on the elbow joint.
#									Both shoulder and elbow joint is for the flexion/extension movement.
# =========================================================================== -->


<mujoco model="2D_UpperLimb">

  <compiler inertiafromgeom = "auto"
            angle 					= "radian" />

						<option  timestep  = '0.0001'
										iterations = '50'
										    solver = 'PGS'
										integrator = 'Euler'
										   gravity = "0 0 -9.81"
									   collision = 'all' >

							<flag energy = 'enable'/>

						</option>


  <asset>
    <!-- Texture of the surrounding sky box
         setting everything as white-ish color for better visualization -->
     <texture type 		= "skybox"
              builtin = "flat"
              rgb1 		= "1 1 1"
              rgb2		= "1 1 1"
              width 	= "100"
              height 	= "100"/>

		<!-- <texture type		  = "skybox"
             builtin  = "checker"
             rgb1 		= ".1 .2 .3"
             rgb2 		= ".2 .3 .4"
             width	  = "200"
            height 	  = "200"
              mark 	  = "edge"/> -->

     <!-- Joint and Limb Color -->
     <material name = "JointColor" rgba = "1.0 1.0 0.0 1" />
     <material name = "LimbColor"  rgba = "0.7 0.7 0.7 1" />
  </asset>


  <worldbody>

        <body name='cameramover' mocap='true' pos='0 0 0'>
						<camera name='mycamera' pos='0.0 2.0 2.0' euler='1.5707 0 0'/>
      	</body>


    <!-- ===============  MODEL STARTS  =============== -->

    <!-- ============ UPPER ARM START =============== -->
		<body name='upperArm' pos='0 0 0' euler='0 0 0'>
			<inertial pos='0 0 -0.5'     mass='1'         diaginertia='1 1 1'/>
			<geom name='shoulderGEOM' type='sphere' pos='0 0 0' material='JointColor' size='0.1' mass='0'/>
			<joint name='shoulder' ref='0' type='hinge' stiffness='0' pos='0 0 0' axis='0 -1 0' />
			<geom type='cylinder' fromto='0 0 0 0 0 -1' material='LimbColor' mass='0' size='0.05'/>
			<site name='upperArmCOM' pos='0 0 -0.5' />
		<!-- ============ UPPER ARM END   =============== -->

      <!-- ============ FORE ARM START =============== -->
			<body name='foreArm' pos='0 0 -1' euler='0 0 0'>
				<inertial pos='0 0 -0.5' mass='1'             diaginertia='1 1 1'/>
				<geom name='elbowGEOM' type='sphere' pos='0 0 0' material='JointColor' size='0.1' mass='0'/>
				<joint name='elbow' ref='0' type='hinge' stiffness='0' pos='0 0 0' axis='0 -1 0'/>
				<geom type='cylinder' fromto='0 0 0 0 0 -1' material='LimbColor' mass='0' size='0.05'/>
				<geom name='EEGEOM' type='sphere' pos='0 0 -1' material='JointColor' size='0.1' mass='0'/>
				<site name='foreArmCOM' pos='0 0 -0.5' />
			</body>
			<!-- ============ FORE ARM END   =============== -->

	</body>


  </worldbody>

	<!-- This xml model file don't use the actuator, since it simply runs to the exact joint position -->
	<actuator>

		<motor joint = 'shoulder' name = 'shoulderTorqueMotor'  forcelimited = 'false'/>
		<motor joint = 'elbow'    name =    'elbowTorqueMotor'  forcelimited = 'false'/>

  </actuator>


</mujoco>
