<!-- ======================================================
	Model 		:: PR2-Arm from Willow Garage 
		Source		: (Provided by Sergey Levine, UC Berkeley)
		Downloaded	: 18July'14
		
	Mujoco		:: Advanced physics simulation engine
		Source		: www.roboti.us
		Version		: 1.50
		Released 	: 23Apr'17
		
	Author		:: Vikash Kumar
		Contacts 	: vikashplus@gmail.com
		Last edits 	: 30Apr16, 10Oct'15, 26July'15, 25Aug'18
		
	Copyright 2018 Vikash Kumar
		Licensed under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
====================================================== -->


<mujoco model="PR2-Arm(v1.51)">
  <compiler coordinate="local" angle="radian" inertiafromgeom="true" meshdir="meshes/"/>
  <option timestep="0.005" iterations='20'/>
  
  <asset>
	<mesh file="shoulder_pan.stl"/>
	<mesh file="shoulder_lift.stl"/>
	<mesh file="upper_arm_roll.stl"/>
	<mesh file="upper_arm.stl"/>
	<mesh file="elbow_flex.stl"/>
	<mesh file="forearm_roll.stl"/>
	<mesh file="forearm.stl"/>
	<mesh file="wrist_flex.stl"/>
	<mesh file="forearm.stl"/>
	<mesh file="wrist_roll.stl"/>
	<mesh file="gripper_palm.stl"/>
	<mesh file="l_finger.stl"/>
	<mesh file="l_finger_tip.stl"/>
	
	<texture name="skybox" type="skybox" builtin="gradient" rgb1=".4 .6 .8" rgb2="0 0 0"
		   width="800" height="800" mark="random" markrgb="1 1 1"/>  
	<texture name="texgeom" type="cube" builtin="flat" mark="cross" width="127" height="1278" 
		rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" random="0.01"/>  
	<texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2" 
		width="512" height="512"/>  

	<material name='MatPlane' reflectance='0.3' texture="texplane" texrepeat="1 1" texuniform="true"/>
	<material name='geom' texture="texgeom" texuniform="true"/>
  </asset>
  
  <default>
    <joint damping="10" limited="true"/>
    <geom friction=".5 .1 .1" margin="0.002" contype="1" conaffinity="0" type="mesh"/>
  </default>
  
  <worldbody>
	<geom name='floor' pos='0 0 0' size='5 5 .125' type='plane' material="MatPlane" condim='3' contype="0" conaffinity="1"/>
	<light mode='targetbodycom' target='r_shoulder_pan_link' directional='false' diffuse='.8 .8 .8' specular='0.3 0.3 0.3' pos='0 0 4.0' dir='0 0 -1'/>
		
    <body name="r_shoulder_pan_link" pos="-.25 0 0.7">
      <inertial pos="-0.001201 0.024513 -0.098231" quat="0.927482 -0.0876561 0.0740595 -0.355821" mass="25.7993" diaginertia="0.934201 0.837701 0.242033" />
      <joint name="r_shoulder_pan_joint" type="hinge" pos="0 0 0" axis="0 0 1" range="-2.2854 0.714602" damping="10.0"/>
      <geom rgba="0 0 0.8 1" mesh="shoulder_pan"/>
      
	  <body name="r_shoulder_lift_link" pos="0.1 0 0">
        <inertial pos="0.02195 -0.02664 -0.03127" quat="0.791779 0.478014 0.238985 0.295762" mass="2.74988" diaginertia="0.0261874 0.0207302 0.0151681" />
        <joint name="r_shoulder_lift_joint" type="hinge" pos="0 0 0" axis="0 1 0" range="-0.5236 1.3963" damping="10.0"/>
        <geom rgba="0.7 0.7 0.7 1" mesh="shoulder_lift"/>
        
		<body name="r_upper_arm_roll_link" pos="0 0 0">
          <inertial pos="0 0 0" mass="0.1" diaginertia="0.01 0.01 0.01" />
          <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"/>
          <geom mesh="upper_arm_roll"/>
          
		  <body name="r_upper_arm_link" pos="0 0 0">
            <inertial pos="0.21398 -0.01621 -0.0002" quat="-0.0450196 0.697462 -0.088537 0.709705" mass="6.01769" diaginertia="0.0761385 0.0749307 0.0151327" />
            <geom rgba="0 0.8 0 1" mesh="upper_arm"/>
            
			<body name="r_elbow_flex_link" pos="0.4 0 0">
              <inertial pos="0.01014 0.00032 -0.01211" quat="0.308784 0.646797 0.639077 0.279083" mass="1.90327" diaginertia="0.00441852 0.00396477 0.00308977" />
              <joint name="r_elbow_flex_joint" type="hinge" pos="0 0 0" axis="0 1 0" range="-2.3213 0" damping="1.0"/>
              <geom rgba="0.7 0.7 0.7 1" mesh="elbow_flex"/>
              
			  <body name="r_forearm_roll_link" pos="0 0 0">
                <inertial pos="0 0 0" mass="0.1" diaginertia="0.01 0.01 0.01" />
                <joint name="r_forearm_roll_joint" type="hinge" limited="false" pos="0 0 0" axis="1 0 0" damping=".1"/>
                <geom mesh="forearm_roll"/>
                
				<body name="r_forearm_link" pos="0 0 0">
                  <inertial pos="0.18791 -0.00017 -0.00912" quat="-0.000586594 0.726286 -0.00396869 0.687381" mass="2.57968" diaginertia="0.0166326 0.0150775 0.00360892" />
                  <geom rgba="0.7 0.7 0.7 1" mesh="forearm"/>
                  
				  <body name="r_wrist_flex_link" pos="0.321 0 0">
                    <inertial pos="-0.00157 0 -0.00075" quat="0.695976 0.696411 -0.12356 0.123942" mass="0.61402" diaginertia="0.000652771 0.000643391 0.000198244" />
                    <joint name="r_wrist_flex_joint" type="hinge" pos="0 0 0" axis="0 1 0" range="-2.094 0" damping=".1"/>
                    <geom rgba="0.7 0.7 0.7 1" mesh="wrist_flex"/>
                    
					<body name="r_wrist_roll_link" pos="0 0 0">
                      <inertial pos="0 0 0" mass="0.1" diaginertia="0.01 0.01 0.01" />
                      <joint name="r_wrist_roll_joint" type="hinge" pos="0 0 0" limited="false" axis="1 0 0" damping="0.1"/>
                      <geom mesh="wrist_roll"/>
                      
					  <body name="r_gripper_palm_link" pos="0 0 0">
                        <inertial pos="0.06623 0.00053 -0.00119" quat="-0.0181842 0.706274 0.0160771 0.707523" mass="0.58007" diaginertia="0.000865637 0.000678178 0.000351471" />
                        <geom rgba="0.8 0 0 1" mesh="gripper_palm"/>
                        
						<!--<body name="r_gripper_led_frame" pos="0.0513 0 0.0244">
                          <inertial pos="0.0513 0 0.0244" mass="0" diaginertia="0 0 0" />
                        </body>-->
                        
						<body name="r_gripper_motor_accelerometer_link" pos="0 0 0">
                          <inertial pos="0 0 0" mass="0.001" diaginertia="0.001 0.001 0.001" />
                          <geom type="box" size="0.0005 0.0005 0.0005" pos="0 0 0" />
                        </body>
                        
						<!--<body name="r_gripper_tool_frame" pos="0.18 0 0">
                          <inertial pos="0.18 0 0" mass="0" diaginertia="0 0 0" />
                          <site name="finger_tip_site" pos="0 0 0"/>
                        </body>-->
                       
					   <body name="r_gripper_l_finger_link" pos="0.07691 0.01 0">
                          <inertial pos="0.03598 0.0173 -0.00164" quat="0.57444 0.429111 0.518782 0.465562" mass="0.17126" diaginertia="0.00019774 0.000181336 7.66239e-005" />
                          <joint name="r_gripper_l_finger_joint" type="hinge" pos="0 0 0" axis="0 0 1" range="0 0.548" damping="0.2"/>
                          <geom rgba="0.7 0.7 0.7 1" mesh="l_finger"/>
                          
						  <body name="r_gripper_l_finger_tip_link" pos="0.09137 0.00495 0">
                            <inertial pos="0.00423 0.00284 0" quat="0.249588 0.661594 -0.249588 0.661594" mass="0.04419" diaginertia="1.54177e-005 1.50049e-005 3.23624e-006" />
                            <joint name="r_gripper_l_finger_tip_joint" type="hinge" pos="0 0 0" axis="0 0 1" range="-0.548 0" damping="0.01"/>
                            <geom rgba="0 0.8 0 1" mesh="l_finger_tip"/>
                          </body>
						</body>
						
                        <body name="r_gripper_r_finger_link" pos="0.07691 -0.01 0" euler="3.14 0 0">
                          <inertial pos="0.03576 -0.01736 -0.00095" quat="0.474684 0.510909 0.446353 0.560727" mass="0.17389" diaginertia="0.000198903 0.00018134 7.66847e-005" />
                          <joint name="r_gripper_r_finger_joint" type="hinge" pos="0 0 0" axis="0 0 1" range="0 0.548" damping="0.2"/>
                          <geom rgba="0.7 0.7 0.7 1" mesh="l_finger"/>
                          
						  <body name="r_gripper_r_finger_tip_link" pos="0.09137 0.00495 0">
                            <inertial pos="0.00423 -0.00284 0" quat="-0.249588 0.661594 0.249588 0.661594" mass="0.04419" diaginertia="1.54177e-005 1.50049e-005 3.23624e-006" />
                            <joint name="r_gripper_r_finger_tip_joint" type="hinge" pos="0 0 0" axis="0 0 1" range="-.548 0" damping="0.01"/>
                            <geom rgba="0 0.8 0 1" mesh="l_finger_tip"/>
                          </body>
                        </body>
                      </body>
                    </body>
                  </body>
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
    </body>
  </worldbody>

<!--  <features>
    <feature name='control'>
      <data field='ctrl' item='all'/>
    </feature>
  </features>
  <costs>

    <physics_options  timestep='0.01'/>

    <optimization_options
        logeps='-30'
        horizon='400'
        logmurange='-5 3'
        />

    <cost name='dummy'>
      <term feature='control'       norm='quadratic'    coef_running='.1' />
    </cost>
  </costs>-->
</mujoco>
