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

  <xacro:macro name="inertial_cuboid" params="mass x_length y_length z_length">
    <inertial>
      <mass value="${mass}" />
      <origin xyz="0 0 0" />
      <inertia ixx="${(1/12) * mass* (y_length*y_length+ z_length*z_length)}" ixy="0.0" ixz="0.0"
               iyy="${(1/12) * mass* (x_length*x_length+ z_length*z_length)}" iyz="0.0"
               izz="${(1/12) * mass* (x_length*x_length+ y_length*y_length)}" />
    </inertial>
  </xacro:macro>
  
  <xacro:macro name="inertial_cuboid_with_pose" params="mass x_length y_length z_length *origin">
    <inertial>
      <insert_block name="origin"/>
      <mass value="${mass}" />
      <origin xyz="0 0 0" />
      <inertia ixx="${(1/12) * mass* (y_length*y_length+ z_length*z_length)}" ixy="0.0" ixz="0.0"
               iyy="${(1/12) * mass* (x_length*x_length+ z_length*z_length)}" iyz="0.0"
               izz="${(1/12) * mass* (x_length*x_length+ y_length*y_length)}" />
    </inertial>
  </xacro:macro>

  <xacro:macro name="inertial_sphere" params="mass diameter">
    <inertial>
      <mass value="${mass}" />
      <origin xyz="0 0 0" />
      <inertia ixx="${(2/5) * mass* ( (diameter*0.5)) * (diameter*0.5))}" ixy="0.0" ixz="0.0"
               iyy="${(2/5) * mass* ( (diameter*0.5)) * (diameter*0.5))}" iyz="0.0"
               izz="${(2/5) * mass* ( (diameter*0.5)) * (diameter*0.5))}" />
    </inertial>
  </xacro:macro>
  
  <xacro:macro name="inertial_sphere_with_pose" params="mass diameter *origin">
    <inertial>
      <insert_block name="origin"/>
      <mass value="${mass}" />
      <inertia ixx="${(2/5) * mass* ( (diameter*0.5)) * (diameter*0.5))}" ixy="0.0" ixz="0.0"
               iyy="${(2/5) * mass* ( (diameter*0.5)) * (diameter*0.5))}" iyz="0.0"
               izz="${(2/5) * mass* ( (diameter*0.5)) * (diameter*0.5))}" />
    </inertial>
  </xacro:macro>  

</robot>
