<!-- =========================================================================== #
# 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"/>

     <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>

		<!-- ============ UPPER ARM START =============== -->
		<body name="upperArm" pos="0 0 0" euler="0 0 0">
			<inertial pos='0 0 -0.129'     mass='1.595'         diaginertia='0.011917 0.011937 0.001325'/>
			<geom name='shoulderGEOM' type='sphere' pos='0 0 0' material='JointColor' size='0.0294' mass='0'/>
			<joint name='shoulderFlexExt' ref='0' type='hinge' stiffness='0' pos='0 0 0' axis='0 -1 0' />
			<joint name='shoulderAddAbd' ref='0'  type='hinge' stiffness='0' pos='0 0 0' axis='-1 0 0' />
			<joint name='shoulderMedLat' ref='0'  type='hinge' stiffness='0' pos='0 0 0' axis=' 0 0 1' />
			<site name='upperArmCOM' pos='0 0 -0.129' />
			<geom type='cylinder' fromto='0 0 0 0 0 -0.294' material='LimbColor' mass='0' size='0.0147'/>
		<!-- ============ UPPER ARM END   =============== -->

      <!-- ============ FORE ARM START =============== -->
			<body name='foreArm' pos='0 0 -0.294' euler='0 0 0'>
				<inertial pos='0 0 -0.112' mass='0.869'             diaginertia='0.004765 0.004855 0.000472'/>
				<geom name='elbowGEOM' type='sphere' pos='0 0 0' material='JointColor' size='0.0294' 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 -0.291' material='LimbColor' mass='0' size='0.0147'/>
				<site name='foreArmCOM' pos='0 0 -0.112' />
				<geom name='EEGEOM' type='sphere' pos='0 0 -0.291' material='JointColor' size='0.0294' mass='0.0'/>
			</body>
		</body>



  </worldbody>

	<actuator>

		<motor joint = 'shoulderFlexExt' name = 'shoulderFlexExtTorqueMotor' forcelimited = 'false'/>
		<motor joint = 'shoulderAddAbd'  name = 'shoulderAddAbdTorqueMotor'  forcelimited = 'false'/>
		<motor joint = 'shoulderMedLat'  name = 'shoulderMedLatTorqueMotor'  forcelimited = 'false'/>
    <motor joint = 'elbow'           name = 'elbowTorqueMotor'           forcelimited = 'false'/>

  </actuator>


</mujoco>
