<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:include filename="$(find tm_description)/xacro/macro.inertial.xacro" />

  <xacro:macro name="tm7s" params="ns prefix color:=none format:=obj
    joint_1_lower_limit:=${radians(-360)}    joint_1_upper_limit:=${radians(360)}
    joint_2_lower_limit:=${radians(-360)}    joint_2_upper_limit:=${radians(360)}
    joint_3_lower_limit:=${radians(-152)}    joint_3_upper_limit:=${radians(152)}
    joint_4_lower_limit:=${radians(-360)}    joint_4_upper_limit:=${radians(360)}
    joint_5_lower_limit:=${radians(-360)}    joint_5_upper_limit:=${radians(360)}
    joint_6_lower_limit:=${radians(-360)}    joint_6_upper_limit:=${radians(360)}
    joint_1_velocity_limit:=${radians(210)}    
    joint_2_velocity_limit:=${radians(210)}
    joint_3_velocity_limit:=${radians(210)}    
    joint_4_velocity_limit:=${radians(225)}
    joint_5_velocity_limit:=${radians(225)}    
    joint_6_velocity_limit:=${radians(450)}
    joint_1_effort_limit:=${170}    
    joint_2_effort_limit:=${170}
    joint_3_effort_limit:=${170}    
    joint_4_effort_limit:=${45}
    joint_5_effort_limit:=${45}    
    joint_6_effort_limit:=${45}   
    safety_limits:=false safety_pos_margin:=0.15
    safety_k_position:=20">

   <xacro:property name="damping_factor" value="0.000"/>
   <xacro:property name="d1" value="${damping_factor*0.1}"/>
   <xacro:property name="d2" value="${damping_factor*0.1}"/>
   <xacro:property name="d3" value="${damping_factor*0.1}"/>
   <xacro:property name="d4" value="${damping_factor*0.1}"/>
   <xacro:property name="d5" value="${damping_factor*0.1}"/>
   <xacro:property name="d6" value="${damping_factor*0.1}"/>

   <xacro:property name="friction_factor" value="0.000"/>
   <xacro:property name="f1" value="${friction_factor*0.1}"/>
   <xacro:property name="f2" value="${friction_factor*0.1}"/>
   <xacro:property name="f3" value="${friction_factor*0.1}"/>
   <xacro:property name="f4" value="${friction_factor*0.1}"/>
   <xacro:property name="f5" value="${friction_factor*0.1}"/>
   <xacro:property name="f6" value="${friction_factor*0.1}"/>
      
    <!-- Inertia parameters -->
    <xacro:property name="mass_0" value="1.0" />
    <xacro:property name="mass_1" value="4.320" />
    <xacro:property name="mass_2" value="9.456" />
    <xacro:property name="mass_3" value="2.618" />
    <xacro:property name="mass_4" value="1.770" />
    <xacro:property name="mass_5" value="1.770" />
    <xacro:property name="mass_6" value="0.720" />

    <xacro:property name="unit_factor" value="0.000001"/>
    <xacro:property name="ixx_1" value="${8439.2993*unit_factor}" />
    <xacro:property name="ixx_2" value="${28756.423*unit_factor}" />
    <xacro:property name="ixx_3" value="${4027.8840*unit_factor}" />
    <xacro:property name="ixx_4" value="${2046.0318*unit_factor}" />
    <xacro:property name="ixx_5" value="${2046.0318*unit_factor}" />
    <xacro:property name="ixx_6" value="${703.44734*unit_factor}" />

    <xacro:property name="iyy_1" value="${13793.964*unit_factor}" />
    <xacro:property name="iyy_2" value="${240700.11*unit_factor}" />
    <xacro:property name="iyy_3" value="${35507.262*unit_factor}" />
    <xacro:property name="iyy_4" value="${3171.8293*unit_factor}" />
    <xacro:property name="iyy_5" value="${3171.8293*unit_factor}" />
    <xacro:property name="iyy_6" value="${1711.0225*unit_factor}" />
    
    <xacro:property name="izz_1" value="${14241.226*unit_factor}" />
    <xacro:property name="izz_2" value="${252296.10*unit_factor}" />
    <xacro:property name="izz_3" value="${36908.834*unit_factor}" />
    <xacro:property name="izz_4" value="${3478.9518*unit_factor}" />
    <xacro:property name="izz_5" value="${3478.9518*unit_factor}" />
    <xacro:property name="izz_6" value="${1854.4783*unit_factor}" />

<!--LinkDescription-->
  <link name="${prefix}link_0">
    <visual>
      <geometry>
        <mesh filename="package://tm_description/meshes/tm7s/visual/tm7s-base.${format}" />
      </geometry>
      <material name="${color}" />
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://tm_description/meshes/tm7s/collision/tm7s-base_c.stl" />
      </geometry>
    </collision>
    <xacro:cylinder_inertial_z radius="0.06" length="0.05" mass="${mass_0}">
      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000" />
    </xacro:cylinder_inertial_z>
  </link>
  <joint name="${prefix}joint_1" type="revolute">
    <parent link="${prefix}link_0" />
    <child link="${prefix}link_1" />
    <origin rpy="0.000000 -0.000000 0.000001" xyz="0.000000 0.000000 0.148400" />
    <axis xyz="0 0 1" />
    <limit lower="${joint_1_lower_limit}" upper="${joint_1_upper_limit}" velocity="${joint_1_velocity_limit}" effort="${joint_1_effort_limit}" />
    <xacro:if value="${safety_limits}">
      <safety_controller soft_lower_limit="${joint_1_lower_limit + safety_pos_margin}" soft_upper_limit="${joint_1_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0" />
    </xacro:if>
    <dynamics damping="${d1}" friction="${f1}" />
  </joint>
  <link name="${prefix}link_1">
    <visual>
      <geometry>
        <mesh filename="package://tm_description/meshes/tm7s/visual/tm7s-j1.${format}" />
      </geometry>
      <material name="${color}" />
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://tm_description/meshes/tm7s/collision/tm7s-j1_c.stl" />
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000" />
      <mass value="${mass_1}" />
      <inertia ixx="${ixx_1}" ixy="0.000000" ixz="0.000000" iyy="${iyy_1}" iyz="0.000000" izz="${izz_1}" />
    </inertial>
  </link>
  <joint name="${prefix}joint_2" type="revolute">
    <parent link="${prefix}link_1" />
    <child link="${prefix}link_2" />
    <origin rpy="-1.330839 -1.569479 -0.239957" xyz="0.000017 -0.000022 -0.000000" />
    <axis xyz="0 0 1" />
    <limit lower="${joint_2_lower_limit}" upper="${joint_2_upper_limit}" velocity="${joint_2_velocity_limit}" effort="${joint_2_effort_limit}" />
    <xacro:if value="${safety_limits}">
      <safety_controller soft_lower_limit="${joint_2_lower_limit + safety_pos_margin}" soft_upper_limit="${joint_2_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0" />
    </xacro:if>
    <dynamics damping="${d2}" friction="${f2}" />
  </joint>
  <link name="${prefix}link_2">
    <visual>
      <geometry>
        <mesh filename="package://tm_description/meshes/tm7s/visual/tm7s-a1.${format}" />
      </geometry>
      <material name="${color}" />
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://tm_description/meshes/tm7s/collision/tm7s-a1_c.stl" />
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000" />
      <mass value="${mass_2}" />
      <inertia ixx="${ixx_2}" ixy="0.000000" ixz="0.000000" iyy="${iyy_2}" iyz="0.000000" izz="${izz_2}" />
    </inertial>
  </link>
  <joint name="${prefix}joint_3" type="revolute">
    <parent link="${prefix}link_2" />
    <child link="${prefix}link_3" />
    <origin rpy="0.000451 -0.000434 0.004107" xyz="0.329004 0.000000 -0.000022" />
    <axis xyz="0 0 1" />
    <limit lower="${joint_3_lower_limit}" upper="${joint_3_upper_limit}" velocity="${joint_3_velocity_limit}" effort="${joint_3_effort_limit}" />
    <xacro:if value="${safety_limits}">
      <safety_controller soft_lower_limit="${joint_3_lower_limit + safety_pos_margin}" soft_upper_limit="${joint_3_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0" />
    </xacro:if>
    <dynamics damping="${d3}" friction="${f3}" />
  </joint>
  <link name="${prefix}link_3">
    <visual>
      <geometry>
        <mesh filename="package://tm_description/meshes/tm7s/visual/tm7s-a2.${format}" />
      </geometry>
      <material name="${color}" />
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://tm_description/meshes/tm7s/collision/tm7s-a2_c.stl" />
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000" />
      <mass value="${mass_3}" />
      <inertia ixx="${ixx_3}" ixy="0.000000" ixz="0.000000" iyy="${iyy_3}" iyz="0.000000" izz="${izz_3}" />
    </inertial>
  </link>
  <joint name="${prefix}joint_4" type="revolute">
    <parent link="${prefix}link_3" />
    <child link="${prefix}link_4" />
    <origin rpy="-0.000402 -0.002823 1.569122" xyz="0.298064 0.000417 -0.147822" />
    <axis xyz="0 0 1" />
    <limit lower="${joint_4_lower_limit}" upper="${joint_4_upper_limit}" velocity="${joint_4_velocity_limit}" effort="${joint_4_effort_limit}" />
    <xacro:if value="${safety_limits}">
      <safety_controller soft_lower_limit="${joint_4_lower_limit + safety_pos_margin}" soft_upper_limit="${joint_4_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0" />
    </xacro:if>
    <dynamics damping="${d4}" friction="${f4}" />
  </joint>
  <link name="${prefix}link_4">
    <visual>
      <geometry>
        <mesh filename="package://tm_description/meshes/tm7s/visual/tm7s-j5.${format}" />
      </geometry>
      <material name="${color}" />
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://tm_description/meshes/tm7s/collision/tm7s-j5_c.stl" />
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000" />
      <mass value="${mass_4}" />
      <inertia ixx="${ixx_4}" ixy="0.000000" ixz="0.000000" iyy="${iyy_4}" iyz="0.000000" izz="${izz_4}" />
    </inertial>
  </link>
  <joint name="${prefix}joint_5" type="revolute">
    <parent link="${prefix}link_4" />
    <child link="${prefix}link_5" />
    <origin rpy="1.571564 -0.000593 -0.001681" xyz="-0.000329 -0.131470 -0.000101" />
    <axis xyz="0 0 1" />
    <limit lower="${joint_5_lower_limit}" upper="${joint_5_upper_limit}" velocity="${joint_5_velocity_limit}" effort="${joint_5_effort_limit}" />
    <xacro:if value="${safety_limits}">
      <safety_controller soft_lower_limit="${joint_5_lower_limit + safety_pos_margin}" soft_upper_limit="${joint_5_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0" />
    </xacro:if>
    <dynamics damping="${d5}" friction="${f5}" />
  </joint>
  <link name="${prefix}link_5">
    <visual>
      <geometry>
        <mesh filename="package://tm_description/meshes/tm7s/visual/tm7s-j6.${format}" />
      </geometry>
      <material name="${color}" />
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://tm_description/meshes/tm7s/collision/tm7s-j6_c.stl" />
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000" />
      <mass value="${mass_5}" />
      <inertia ixx="${ixx_5}" ixy="0.000000" ixz="0.000000" iyy="${iyy_5}" iyz="0.000000" izz="${izz_5}" />
    </inertial>
  </link>
  <joint name="${prefix}joint_6" type="revolute">
    <parent link="${prefix}link_5" />
    <child link="${prefix}link_6" />
    <origin rpy="1.569861 -0.006099 0.000597" xyz="0.000125 -0.134925 0.000126" />
    <axis xyz="0 0 1" />
    <limit lower="${joint_6_lower_limit}" upper="${joint_6_upper_limit}" velocity="${joint_6_velocity_limit}" effort="${joint_6_effort_limit}" />
    <xacro:if value="${safety_limits}">
      <safety_controller soft_lower_limit="${joint_6_lower_limit + safety_pos_margin}" soft_upper_limit="${joint_6_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0" />
    </xacro:if>
    <dynamics damping="${d6}" friction="${f6}" />
  </joint>
  <link name="${prefix}link_6">
    <visual>
      <geometry>
        <mesh filename="package://tm_description/meshes/tm7s/visual/tm7s-io.${format}" />
      </geometry>
      <material name="${color}" />
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://tm_description/meshes/tm7s/collision/tm7s-io_c.stl" />
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000" />
      <mass value="${mass_6}" />
      <inertia ixx="${ixx_6}" ixy="0.000000" ixz="0.000000" iyy="${iyy_6}" iyz="0.000000" izz="${izz_6}" />
    </inertial>
  </link>
  <link name="${prefix}base" />
  <joint name="${prefix}base_fixed_joint" type="fixed">
    <parent link="${prefix}base" />
    <child link="${prefix}link_0" />
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0" />
  </joint>
  <link name="${prefix}flange" />
  <joint name="${prefix}flange_fixed_joint" type="fixed">
    <parent link="${prefix}link_6" />
    <child link="${prefix}flange" />
    <origin rpy="0.003745 0.003914 0.000015" xyz="-0.000068 0.000000 0.000000" />
  </joint>
<!--LinkDescription-->

  </xacro:macro>

</robot>
