Uni-Lab-OS/unilabos/device_mesh/devices/toyo_xyz/macro_device.xacro

<?xml version="1.0" ?>

<robot xmlns:xacro="http://ros.org/wiki/xacro">

  <xacro:macro name="toyo_xyz" 
  params="length1:=0.1 min_d1:=0.01 max_d1:=0.01 slider_d1:=0.135
          length2:=0.1 min_d2:=0.01 max_d2:=0.01 slider_d2:=0.116
          length3:=0.1 min_d3:=0.01 max_d3:=0.01 slider_d3:=0.090
          parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 rx:=0 ry:=0 r:=0
          mesh_path:=''">


    <joint name="${station_name}${device_name}base_link_joint" type="fixed">
      <origin xyz="${x} ${y} ${z}" rpy="${rx} ${ry} ${r}" />
      <parent link="${parent_link}"/>
      <child link="${station_name}${device_name}device_link"/>
      <axis xyz="0 0 0"/>
    </joint>

    <link name="${station_name}${device_name}device_link"/>
    <joint name="${station_name}${device_name}device_link_joint" type="fixed">
      <origin xyz="${-min_d1}  0 0" rpy="0 0 0" />
      <parent link="${station_name}${device_name}device_link"/>
      <child link="${station_name}${device_name}base_link"/>
      <axis xyz="0 0 0"/>
    </joint>

    <link name="${station_name}${device_name}base_link">
      <inertial>
        <origin rpy="0 0 0" xyz="-0.0120236017581094 -0.0517106842624209 0.0409192044637189"/>
        <mass value="0.69481680376074"/>
        <inertia ixx="0.000936712501344659" ixy="7.23677144617562E-06" ixz="-1.24166862138852E-06" iyy="0.000379264833590696" iyz="9.52650081379508E-07" izz="0.000898185165939333"/>
      </inertial>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/base_link.STL"/>
        </geometry>
         <material name="">
          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/base_link.STL"/>
        </geometry>
      </collision>
    </link>
    <link name="${station_name}${device_name}length1_link">
      <inertial>
        <origin rpy="0 0 0" xyz="0.494579219297241 -0.0104662192149046 0.020043711867505"/>
        <mass value="5.94417515905243"/>
        <inertia ixx="0.00674507129097306" ixy="9.38238612394185E-09" ixz="4.35684774317116E-06" iyy="0.485770401104387" iyz="2.57353070463078E-06" izz="0.489757831272925"/>
      </inertial>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/length1_link.STL" scale="${(length1 + min_d1 + max_d1 + slider_d1)} 1 1"/>
        </geometry>
         <material name="">
          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/length1_link.STL" scale="${(length1 + min_d1 + max_d1 + slider_d1)} 1 1"/>
        </geometry>
      </collision>
    </link>
    <joint name="${station_name}${device_name}length1_joint" type="fixed">
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <parent link="${station_name}${device_name}base_link"/>
      <child link="${station_name}${device_name}length1_link"/>
      <axis xyz="0 0 1"/>
    </joint>




    <link name="${station_name}${device_name}slider1_link">
      <inertial>
        <origin rpy="0 0 0" xyz="0 0 -0.00730412052530301"/>
        <mass value="0.118155140657232"/>
        <inertia ixx="6.43342434854928E-05" ixy="-2.31210361323828E-11" ixz="3.06922820322201E-10" iyy="0.000154199389116579" iyz="1.20524516927812E-08" izz="0.000215806482689218"/>
      </inertial>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/slider1_link.STL"/>
        </geometry>
         <material name="">
          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/slider1_link.STL"/>
        </geometry>
      </collision>
    </link>
    <joint name="${station_name}${device_name}slider1_joint" type="prismatic">
      <origin rpy="0 0 0" xyz="${min_d1 + slider_d1/2} 0 0.078"/>
      <parent link="${station_name}${device_name}base_link"/>
      <child link="${station_name}${device_name}slider1_link"/>
      <axis xyz="1 0 0"/>
      <limit effort="0" lower="0" upper="${length1}" velocity="0"/>
    </joint>



    <link name="${station_name}${device_name}chain_link">
      <inertial>
        <origin rpy="0 0 0" xyz="0.49922930196331 -0.00327071484975672 0.0207539916460562"/>
        <mass value="2.00787563607981"/>
        <inertia ixx="0.00240786362662634" ixy="1.44801915606072E-05" ixz="-3.1726019259437E-05" iyy="0.167850452350525" iyz="-0.000138217054257536" izz="0.168373005725347"/>
      </inertial>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/chain_link.STL" scale="${(length2 + min_d2 + max_d2 + slider_d2)} 1 1"/>
        </geometry>
         <material name="">
          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/chain_link.STL" scale="${(length2 + min_d2 + max_d2 + slider_d2)} 1 1"/>
        </geometry>
      </collision>
    </link>
    <joint name="${station_name}${device_name}chain_joint" type="fixed">
      <origin rpy="0 0 1.5708" xyz="-0.02 -0.041 -0.0005"/>
      <parent link="${station_name}${device_name}slider1_link"/>
      <child link="${station_name}${device_name}chain_link"/>
      <axis xyz="0 0 0"/>
    </joint>



    <link name="${station_name}${device_name}base2_link">
      <inertial>
        <origin rpy="0 0 0" xyz="-0.00645898766177746 0.0354023221709439 0.0277640039757229"/>
        <mass value="0.341816215620884"/>
        <inertia ixx="0.000246380769664854" ixy="-3.5162220656232E-06" ixz="3.72156984182819E-08" iyy="0.00014938476423929" iyz="-2.01567401863811E-06" izz="0.000271539741067036"/>
      </inertial>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/base2_link.STL"/>
        </geometry>
         <material name="">
          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/base2_link.STL"/>
        </geometry>
      </collision>
    </link>
    <joint name="${station_name}${device_name}base2_joint" type="fixed">
      <origin rpy="1.5708 0 1.5708" xyz="-0.0145 0.0295 0.04915"/>
      <parent link="${station_name}${device_name}slider1_link"/>
      <child link="${station_name}${device_name}base2_link"/>
      <axis xyz="0 0 0"/>
    </joint>



    <link name="${station_name}${device_name}slider2_link">
      <inertial>
        <origin rpy="0 0 0" xyz="-1.94863440558524E-06 -4.2412482447185E-05 -0.00600639156586869"/>
        <mass value="0.0568102419437891"/>
        <inertia ixx="1.29719509999494E-05" ixy="-2.84670156002291E-09" ixz="-1.99529353027075E-10" iyy="5.43277686573641E-05" iyz="-5.94709690026503E-09" izz="6.6299564390705E-05"/>
      </inertial>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/slider2_link.STL"/>
        </geometry>
         <material name="">
          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/slider2_link.STL"/>
        </geometry>
      </collision>
    </link>
    <joint name="${station_name}${device_name}slider2_joint" type="prismatic">
      <origin rpy="1.5708 0 1.5708" xyz="0.0455 ${min_d2 + slider_d2/2 + 0.0295} 0.049"/>
      <parent link="${station_name}${device_name}slider1_link"/>
      <child link="${station_name}${device_name}slider2_link"/>
      <axis xyz="1 0 0"/>
      <limit effort="0" lower="0" upper="${length2}" velocity="0"/>
    </joint>


    <link  name="${station_name}${device_name}length2_link">
      <inertial>
        <origin
          xyz="0.495 -0.00012803097983341 0.0216248093421714"
          rpy="0 0 0" />
        <mass  value="2.0281266583716" />
        <inertia
          ixx="0.000735867784216875"
          ixy="4.46823528502648E-18"
          ixz="1.62564733709183E-17"
          iyy="0.166018547616233"
          iyz="8.35811182464874E-07"
          izz="0.166011809812984" />
      </inertial>
      <visual>
        <origin  xyz="0 0 0"  rpy="0 0 0" />
        <geometry>
          <mesh  filename="file://${mesh_path}/devices/toyo_xyz/meshes/length2_link.STL" scale="${(length2 + min_d2 + max_d2 + slider_d2)} 1 1"/>
        </geometry>
        <material  name="">
          <color  rgba="0.756862745098039 0.768627450980392 0.752941176470588 1" />
        </material>
      </visual>
      <collision>
        <origin  xyz="0 0 0"  rpy="0 0 0" />
        <geometry>
          <mesh  filename="file://${mesh_path}/devices/toyo_xyz/meshes/length2_link.STL" scale="${(length2 + min_d2 + max_d2 + slider_d2)} 1 1"/>
        </geometry>
      </collision>
    </link>
    <joint name="${station_name}${device_name}length2_joint" type="fixed">
      <origin  xyz="-0.0145 0.0295 0.04915"  rpy="1.5708 0 1.5708" />
      <parent  link="${station_name}${device_name}slider1_link" />
      <child   link="${station_name}${device_name}length2_link" />
      <axis    xyz="0 0 0" />
    </joint>



    <link name="${station_name}${device_name}fixed_link">
      <inertial>
        <origin rpy="0 0 0" xyz="0.0841027924880233 -1.38777878078145E-17 -0.00753342821771308"/>
        <mass value="0.114714657110459"/>
        <inertia ixx="4.09226560926746E-05" ixy="-1.19246814602201E-20" ixz="1.10812310425511E-07" iyy="0.000219464025912457" iyz="-3.47441993328604E-22" izz="0.000257639354963189"/>
      </inertial>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/fixed_link.STL"/>
        </geometry>
         <material name="">
          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/fixed_link.STL"/>
        </geometry>
      </collision>
    </link>
    <joint name="${station_name}${device_name}fixed_joint" type="fixed">
      <origin rpy="0 0 -1.5708" xyz="0 0.113 0.013"/>
      <parent link="${station_name}${device_name}slider2_link"/>
      <child link="${station_name}${device_name}fixed_link"/>
      <axis xyz="0 0 0"/>
    </joint>



    <link name="${station_name}${device_name}base3_link">
      <inertial>
        <origin rpy="0 0 0" xyz="-0.0898266891415951 -0.000686554465181816 0.0214841690633115"/>
        <mass value="0.253768676399038"/>
        <inertia ixx="7.3143781299483E-05" ixy="-9.27468544438197E-07" ixz="-6.25920202213005E-07" iyy="0.000222729105415939" iyz="1.73297633775367E-06" izz="0.000226940773440045"/>
      </inertial>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/base3_link.STL"/>
        </geometry>
         <material name="">
          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/base3_link.STL"/>
        </geometry>
      </collision>
    </link>
    <joint name="${station_name}${device_name}base3_joint" type="fixed">
      <origin rpy="0 0 -1.5708" xyz="0 0.113 0.013"/>
      <parent link="${station_name}${device_name}slider2_link"/>
      <child link="${station_name}${device_name}base3_link"/>
      <axis xyz="0 0 0"/>
    </joint>



    <link name="${station_name}${device_name}length3_link">
      <inertial>
        <origin rpy="0 0 0" xyz="0.495 2.22485346140083E-05 0.017182121490278"/>
        <mass value="1.46660245378617"/>
        <inertia ixx="0.000378412905544255" ixy="6.68641853450019E-20" ixz="1.30946080093724E-18" iyy="0.119979432170624" iyz="5.03952092605041E-07" izz="0.11996849156089"/>
      </inertial>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/length3_link.STL" scale="${(length3 + min_d3 + max_d3 + slider_d3)} 1 1"/>
        </geometry>
         <material name="">
          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/length3_link.STL" scale="${(length3 + min_d3 + max_d3 + slider_d3)} 1 1"/>
        </geometry>
      </collision>
    </link>
    <joint name="${station_name}${device_name}length3_joint" type="fixed">
      <origin rpy="0 0 -1.5708" xyz="0 0.113 0.013"/>
      <parent link="${station_name}${device_name}slider2_link"/>
      <child link="${station_name}${device_name}length3_link"/>
      <axis xyz="0 0 0"/>
    </joint> 



    <link name="${station_name}${device_name}slider3_link">
      <inertial>
        <origin rpy="0 0 0" xyz="6.92336766672241E-06 2.33762226242717E-09 -0.0056316911606703"/>
        <mass value="0.0319537658681183"/>
        <inertia ixx="4.74895305241407E-06" ixy="-2.68838717157416E-13" ixz="4.74105113238926E-10" iyy="1.78967089054364E-05" iyz="-4.41481885417567E-13" izz="2.21974556051535E-05"/>
      </inertial>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/slider3_link.STL"/>
        </geometry>
         <material name="">
          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/slider3_link.STL"/>
        </geometry>
      </collision>
    </link>
    <joint name="${station_name}${device_name}slider3_joint" type="prismatic">
      <origin rpy="0 0 -1.5708" xyz="0 ${0.113 - min_d3 - slider_d3/2} 0.0635"/>
      <parent link="${station_name}${device_name}slider2_link"/>
      <child link="${station_name}${device_name}slider3_link"/>
      <axis xyz="1 0 0"/>
      <limit effort="0" lower="0" upper="${length3}" velocity="0"/>
    </joint>



    <link name="${station_name}${device_name}end3_link">
      <inertial>
        <origin rpy="0 0 0" xyz="0.00497294496885192 -2.44043265976157E-06 0.0183349877681029"/>
        <mass value="0.0140489551253671"/>
        <inertia ixx="3.43932725883609E-06" ixy="4.00452842192135E-11" ixz="-1.53817578472123E-08" iyy="1.94727500633638E-06" iyz="-3.82376052540752E-10" izz="1.72263562631362E-06"/>
      </inertial>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/end3_link.STL"/>
        </geometry>
         <material name="">
          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/end3_link.STL"/>
        </geometry>
      </collision>
    </link>
    <joint name="${station_name}${device_name}end3_joint" type="fixed">
      <origin rpy="0 0 -1.5708" xyz="0 -${min_d3 + slider_d3 + length3 + max_d3 -0.113} 0.013"/>
      <parent link="${station_name}${device_name}slider2_link"/>
      <child link="${station_name}${device_name}end3_link"/>
      <axis xyz="0 0 0"/>
    </joint>



    <link name="${station_name}${device_name}end2_link">
      <inertial>
        <origin rpy="0 0 0" xyz="0.00495670363919842 -8.74633492434218E-06 0.0214414396703796"/>
        <mass value="0.0200788183514264"/>
        <inertia ixx="7.16397592830092E-06" ixy="4.75643325623778E-11" ixz="-2.20469268570818E-08" iyy="3.90488015971723E-06" iyz="-3.17180365916489E-09" izz="3.58779761734039E-06"/>
      </inertial>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/end2_link.STL"/>
        </geometry>
         <material name="">
          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/end2_link.STL"/>
        </geometry>
      </collision>
    </link>
    <joint name="${station_name}${device_name}end2_joint" type="fixed">
      <origin rpy="1.5708 0 1.5708" xyz="-0.0135 ${min_d2 + slider_d2 + length2 + max_d2 + 0.0295} 0.0492"/>
      <parent link="${station_name}${device_name}slider1_link"/>
      <child link="${station_name}${device_name}end2_link"/>
      <axis xyz="0 0 0"/>
    </joint>



    <link name="${station_name}${device_name}end_link">
      <inertial>
        <origin rpy="0 0 0" xyz="0.00744127217617607 -2.30544170541074E-06 0.0287548952899474"/>
        <mass value="0.0648209796507272"/>
        <inertia ixx="4.85939921252094E-05" ixy="1.05486327324319E-09" ixz="-4.17427158603031E-08" iyy="2.1549381051207E-05" iyz="2.1129085201095E-09" izz="2.9433994127647E-05"/>
      </inertial>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/end_link.STL"/>
        </geometry>
         <material name="">
          <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/end_link.STL"/>
        </geometry>
      </collision>
    </link>
    <joint name="${station_name}${device_name}end_joint" type="fixed">
      <origin rpy="0 0 0" xyz="${min_d1 + length1 + max_d1 + slider_d1} 0 0.001"/>
      <parent link="${station_name}${device_name}base_link"/>
      <child link="${station_name}${device_name}end_link"/>
      <axis xyz="0 0 0"/>
    </joint>



  </xacro:macro>
</robot>