Permalink
Cannot retrieve contributors at this time
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
210 lines (194 sloc)
13.5 KB
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
<mujoco model="ur5gripper"> | |
<compiler angle="radian" | |
inertiafromgeom="true" | |
texturedir="textures/" | |
eulerseq="xyz" | |
meshdir="mesh/visual/"/> | |
<option timestep="2e-3" | |
iterations="10" | |
tolerance="1e-10" | |
impratio="20"/> | |
<size njmax="600" nconmax="150" nstack="300000"/> | |
<default> | |
<site rgba="0.8 0.6 0.7 0.3" type="ellipsoid" group="3"/> | |
<geom contype='1' conaffinity='1' condim='4' margin="1e-3" solref=".01 1" solimp=".99 .99 .01" /> | |
<joint armature="0.01" damping=".1"/> | |
<default class="UR5"> | |
<joint damping='65'/> | |
</default> | |
<default class="UR5e"> | |
<joint damping='45'/> | |
</default> | |
<default class="GRIPPER"> | |
<joint damping='5'/> | |
</default> | |
</default> | |
<visual> | |
<map fogstart="3" fogend="5" znear="0.05"/> | |
<quality shadowsize="2048"/> | |
<scale framelength="0.1" | |
framewidth="0.005" | |
contactwidth="0.1" | |
contactheight="0.005" | |
forcewidth="0.01" | |
/> | |
</visual> | |
<!--Assets--> | |
<asset> | |
<!--Meshes--> | |
<mesh name="base" file="base.stl" /> | |
<mesh name="shoulder" file="shoulder.stl" /> | |
<mesh name="upperarm" file="upperarm.stl" /> | |
<mesh name="forearm" file="forearm.stl" /> | |
<mesh name="wrist1" file="wrist1.stl" /> | |
<mesh name="wrist2" file="wrist2.stl" /> | |
<mesh name="wrist3" file="wrist3.stl" /> | |
<mesh name="link_0" file="link_0.STL" /> | |
<mesh name="link_1" file="link_1.STL" /> | |
<mesh name="link_2" file="link_2.STL" /> | |
<mesh name="link_3" file="link_3.STL" /> | |
<mesh name="ee_mount" file="end_effector_mount.stl" scale="0.001 0.001 0.001"/> | |
<mesh name="handle" file="handle_c.STL" scale="0.001 0.001 0.001"/> | |
<mesh name="main_tray" file="tray_ass.STL" scale="0.001 0.001 0.001"/> | |
<!--Textures--> | |
<texture name="ur5_tex" type="2d" builtin="flat" height="32" width="32" rgb1="0.45 0.45 0.45" /> | |
<texture name="bench_tex" type="2d" builtin="flat" height="32" width="32" rgb1="0.7 0.7 0.7" /> | |
<texture name="sky_tex" type="skybox" builtin="gradient" width="100" height="100" rgb1="1 1 1" rgb2="0.3 0.3 0.8" /> | |
<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="floor_tex" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 .2 .3" width="100" height="100" /> | |
<texture name="wood" type="cube" file="wood2.png"/> | |
<texture name="plywood" type="cube" file="wood3.png"/> | |
<texture name="green_pla" type="cube" file="pla.png"/> | |
<texture name="metal" type="cube" file="metal2.png"/> | |
<!--Materials--> | |
<material name="ur5_mat" texture="ur5_tex" shininess="0.9" specular="0.75" reflectance="0.3"/> | |
<material name='floor_mat' texture="floor_tex" shininess="0.8" texrepeat="10 10" specular="0.7" reflectance="0.5" /> | |
<material name='geom' texture="texgeom" texuniform="true" /> | |
<material name="bench_mat" texture="metal" shininess="0.1" specular="0.5" reflectance="0.2"/> | |
<material name="tablecube" texture="wood" rgba=".8 .8 .8 1"/> | |
<material name="plywood" texture="plywood" rgba=".8 .8 .8 1"/> | |
<material name="pla" texture="green_pla" rgba=".8 .8 .8 1"/> | |
</asset> | |
<!--Contacts--> | |
<contact> | |
<exclude body1="box_link" body2="base_link"/> | |
<exclude body1="base_link" body2="shoulder_link"/> | |
<exclude body1="shoulder_link" body2="upper_arm_link"/> | |
<exclude body1="upper_arm_link" body2="forearm_link"/> | |
<exclude body1="forearm_link" body2="wrist_1_link"/> | |
<exclude body1="wrist_1_link" body2="wrist_2_link"/> | |
<exclude body1="wrist_2_link" body2="wrist_3_link"/> | |
<exclude body1="ee_link" body2="wrist_3_link"/> | |
<exclude body1="ee_link" body2="handle_link"/> | |
<exclude body1="main_tray_link" body2="handle_link"/> | |
</contact> | |
<!--MODEL DESCRIPTION--> | |
<worldbody> | |
<!--Lighting parameters--> | |
<light name="light3" mode="targetbody" target="box_link" directional="true" pos="1 1 3" /> | |
<!--Camera parameters.--> | |
<camera name="main1" mode="fixed" target="main_tray_link" pos="1 1 3" euler="-0.4 0.4 0"/> | |
<!--Floor Property--> | |
<geom name='floor' material="floor_mat" pos='0 0 0' size='2.5 2.5 0.1' type='plane' /> | |
<!--Robot UR5--> | |
<body name="box_link" pos="0 0 0.435"> | |
<!--<geom size=".3 .2 .435" type="box" material="bench_mat"/>--> | |
<geom size=".29 .19 .03" pos ="0 0 0.4" type="box" material="bench_mat"/> | |
<geom size=".29 .19 .03" pos ="0 0 -0.2" type="box" material="bench_mat"/> | |
<geom size=".04 .04 .435" pos =".26 .16 0" type="box" material="bench_mat"/> | |
<geom size=".04 .04 .435" pos ="-.26 .16 0" type="box" material="bench_mat"/> | |
<geom size=".04 .04 .435" pos =".26 -.16 0" type="box" material="bench_mat"/> | |
<geom size=".04 .04 .435" pos ="-.26 -.16 0" type="box" material="bench_mat"/> | |
<inertial pos="0 0 0" mass="1000" diaginertia="0 0 0" /> | |
<body name="base_link" pos="0 0 0.435"> | |
<inertial pos="0 0 0" quat="0.5 0.5 -0.5 0.5" mass="4" diaginertia="0.0072 0.00443333 0.00443333" /> | |
<geom type="mesh" mesh="base" material="ur5_mat"/> | |
<body name="shoulder_link" pos="0 0 0.089159"> | |
<inertial pos="0 0 0" mass="3.7" diaginertia="0.0102675 0.0102675 0.00666" /> | |
<joint name="shoulder_pan_joint" class="UR5" pos="0 0 0" axis="0 0 1" limited="true" range="-3.14159 3.14159" /> | |
<geom type="mesh" mesh="shoulder" material="ur5_mat"/> | |
<body name="upper_arm_link" pos="0 0.13585 0" quat="0.707107 0 0.707107 0"> | |
<inertial pos="0 0 0.28" mass="8.393" diaginertia="0.226891 0.226891 0.0151074" /> | |
<joint name="shoulder_lift_joint" class="UR5" pos="0 0 0" axis="0 1 0" limited="true" range="-3.14159 0" /> <!--Range= -3.14159 3.14159--> | |
<geom type="mesh" mesh="upperarm" material="ur5_mat"/> | |
<body name="forearm_link" pos="0 -0.1197 0.425"> | |
<inertial pos="0 0 0.25" mass="2.275" diaginertia="0.0494433 0.0494433 0.004095" /> | |
<joint name="elbow_joint" class="UR5" pos="0 0 0" axis="0 1 0" limited="true" range="-3.14159 3.14159" /> | |
<geom type="mesh" mesh="forearm" material="ur5_mat"/> | |
<body name="wrist_1_link" pos="0 0 0.39225" quat="0.707107 0 0.707107 0"> | |
<inertial pos="0 0 0" quat="0.5 0.5 -0.5 0.5" mass="1.219" diaginertia="0.21942 0.111173 0.111173" /> | |
<joint name="wrist_1_joint" class="UR5e" pos="0 0 0" axis="0 1 0" limited="true" range="-3.14159 3.14159" /> | |
<geom type="mesh" mesh="wrist1" material="ur5_mat"/> | |
<body name="wrist_2_link" pos="0 0.093 0"> | |
<inertial pos="0 0 0" quat="0.5 0.5 -0.5 0.5" mass="1.219" diaginertia="0.21942 0.111173 0.111173" /> | |
<joint name="wrist_2_joint" class="UR5e" pos="0 0 0" axis="0 0 1" limited="true" range="-3.14159 3.14159" /> | |
<geom type="mesh" mesh="wrist2" material="ur5_mat"/> | |
<body name="wrist_3_link" pos="0 0 0.09465"> | |
<inertial pos="0 0 0" quat="0.5 0.5 -0.5 0.5" mass="0.1879" diaginertia="0.033822 0.0171365 0.0171365" /> | |
<joint name="wrist_3_joint" class="UR5e" pos="0 0 0" axis="0 1 0" limited="true" range="-3.14159 3.14159" /> | |
<geom type="mesh" mesh="wrist3" material="ur5_mat"/> | |
<body name="ee_link" pos="0.0380 0.0823 -0.0377" quat="0.707107 0 -0.707107 0"> | |
<site name="ee_link"/> | |
<inertial pos="0.0380 0.0823 -0.0377" quat="0.707107 0 -0.707107 0" mass="0.2" diaginertia="0 0 0" /> | |
<geom type="mesh" mesh="ee_mount" material="plywood"/> | |
<body name="handle_link" pos="0.05 0.1105 -0.026" euler="1.570796 0 1.570796"> | |
<inertial pos="0.05 0.1105 -0.026" euler="1.570796 0 1.570796" mass="0.2" diaginertia="0 0 0" /> | |
<geom type="mesh" mesh="handle" material="pla"/> | |
<body name="main_tray_link" pos="-0.25 0 -0.22" euler="1.570796 1.570796 0"> | |
<site name="main_tray_link"/> | |
<inertial pos="-0.25 0 -0.22" euler="1.570796 1.570796 0" mass="0.5" diaginertia="0 0 0" /> | |
<joint name="tray_joint" class="UR5e" type="hinge" pos="0 0.233 0.22" axis="0 0 1" limited="true" range="-2 1.770796" /> | |
<geom type="mesh" mesh="main_tray" material="plywood"/> | |
</body> | |
</body> | |
</body> | |
<!--GRIPPER--> | |
</body> | |
</body> | |
</body> | |
</body> | |
</body> | |
</body> | |
<body name="base" pos="0 0 0" quat="1.7949e-09 0 0 -1"> | |
<inertial pos="0 0 0" quat="1.7949e-09 0 0 -1" mass="0" diaginertia="0 0 0" /> | |
</body> | |
</body> | |
</body> | |
</worldbody> | |
<!--ACTUATORS--> | |
<actuator> | |
<!--UR5--> | |
<!--<position name='shoulder_pan' ctrllimited="true" ctrlrange="-3.14159 3.14159" joint='shoulder_pan_joint' kp="500"/>--> | |
<!--<position name='shoulder_lift' ctrllimited="true" ctrlrange="-3.14159 3.14159" joint='shoulder_lift_joint' kp="500"/>--> | |
<!--<position name='forearm' ctrllimited="true" ctrlrange="-3.14159 3.14159" joint='elbow_joint' kp="500"/>--> | |
<!--<position name='wrist_1' ctrllimited="true" ctrlrange="-3.14159 3.14159" joint='wrist_1_joint' kp="100"/>--> | |
<!--<position name='wrist_2' ctrllimited="true" ctrlrange="-3.14159 3.14159" joint='wrist_2_joint' kp="100"/>--> | |
<!--<position name='wrist_3' ctrllimited="true" ctrlrange="-3.14159 3.14159" joint='wrist_3_joint' kp="100"/>--> | |
<motor name='shoulder_pan_T' ctrllimited="true" ctrlrange="-2 2" joint='shoulder_pan_joint' gear="101"/> <!--range -150 150--> | |
<motor name='shoulder_lift_T' ctrllimited="true" ctrlrange="-2 2" joint='shoulder_lift_joint' gear="101"/> | |
<motor name='forearm_T' ctrllimited="true" ctrlrange="-2 2" joint='elbow_joint' gear="101" /> | |
<motor name='wrist_1_T' ctrllimited="true" ctrlrange="-1 1" joint='wrist_1_joint' gear="101"/> <!--range -28 28--> | |
<motor name='wrist_2_T' ctrllimited="true" ctrlrange="-1 1" joint='wrist_2_joint' gear="101"/> | |
<motor name='wrist_3_T' ctrllimited="true" ctrlrange="-1 1" joint='wrist_3_joint' gear="101"/> | |
<motor name='tray_lift_T' ctrllimited="true" ctrlrange="-1 1" joint='tray_joint' gear="101"/> | |
<!--<velocity name='shoulder_pan_v' class="UR5" joint='shoulder_pan_joint' kv="10"/>--> | |
<!--<velocity name='shoulder_lift_v' class="UR5" joint='shoulder_lift_joint' />--> | |
<!--<velocity name='forearm_v' class="UR5" joint='elbow_joint' />--> | |
<!--<velocity name='wrist_1_v' class="UR5" joint='wrist_1_joint' />--> | |
<!--<velocity name='wrist_2_v' class="UR5" joint='wrist_2_joint'/>--> | |
<!--<velocity name='wrist_3_v' class="UR5" joint='wrist_3_joint'/>--> | |
<!--Gripper--> | |
<!--<position name='finger_1' ctrllimited="true" kp="20" joint='gripperfinger_1_joint_1' ctrlrange='0 1.2217'/>--> | |
<!--<position name='finger_2' ctrllimited="true" kp="20" joint='gripperfinger_2_joint_1' ctrlrange='0 1.2217'/>--> | |
<!--<position name='middle_finger' ctrllimited="true" kp="20" joint='gripperfinger_middle_joint_1' ctrlrange='0 1.2217'/>--> | |
<!--<position name='finger_scissor' ctrllimited="true" kp="20" joint='gripperpalm_finger_1_joint' ctrlrange="-0.2967 0.2967"/>--> | |
<!-- | |
<motor name='finger_1_T' ctrllimited="true" joint='gripperfinger_1_joint_1' ctrlrange='-0.1 .8' gear="20"/> | |
<motor name='finger_2_T' ctrllimited="true" joint='gripperfinger_2_joint_1' ctrlrange='-0.1 .8' gear="20"/> | |
<motor name='middle_finger_T' ctrllimited="true" joint='gripperfinger_middle_joint_1' ctrlrange='-0.1 .8' gear="20"/> | |
<motor name='finger_scissor_T' ctrllimited="true" joint='gripperpalm_finger_1_joint' ctrlrange="-0.8 0.8" gear="15"/> --> | |
</actuator> | |
<sensor> | |
<force name="ee_frc" site="ee_link"/> | |
</sensor> | |
</mujoco> |