<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro">
  <!--
    NOTE the macros defined in this file are NOT part of the public API of this
         package. Users CANNOT rely on this file being available, or stored in
         this location. Nor can they rely on the existence of the two macros.
         The macros store the defined properties in the scope of the caller.
         However, users MUST NOT rely on these properties, their contents or their
         names.

    Author: Chen Shichao
  -->
  <xacro:macro name="build_inertial" params="mass cog inertia">
    <inertial>
      <mass value="${mass}" />
      <origin 
        xyz="${cog['x']} ${cog['y']} ${cog['z']}"
        rpy="0 0 0" />
      <inertia 
        ixx="${inertia['ixx']}"
        ixy="${inertia['ixy']}"
        ixz="${inertia['ixz']}"
        iyy="${inertia['iyy']}"
        iyz="${inertia['iyz']}"
        izz="${inertia['izz']}" />
    </inertial>
  </xacro:macro>

  <xacro:macro name="get_visual_params" params="name:=^ type:=^" >
    <xacro:property name="visual_params" value="${sec_mesh_files[name][type]}" scope="parent"/>
  </xacro:macro>

  <!-- Simplification of getting meshes. Available types can be seen in the visual_parameters.yaml (At the time of writing: visual, collision) -->
  <xacro:macro name="get_mesh_path" params="name:=^ type:=^" >
    <xacro:get_visual_params />
    <xacro:if value="${force_abs_paths}">
      <xacro:property name="mesh" value="file://${mesh_path}/devices/elite_robot/${visual_params['mesh']['path']}" scope="parent"/>
    </xacro:if>
    <xacro:unless value="${force_abs_paths}">
      <xacro:property name="mesh" value="package://${visual_params['mesh']['package']}/${visual_params['mesh']['path']}" scope="parent"/>
    </xacro:unless>
  </xacro:macro>

  <xacro:macro name="get_mesh" params="name type" >
    <xacro:get_mesh_path/>
    <mesh filename="${mesh}"/>
  </xacro:macro>

  <xacro:macro name="read_model_data" params="joint_limits_parameters_file kinematics_parameters_file physical_parameters_file visual_parameters_file force_abs_paths mesh_path">

    <xacro:property name="force_abs_paths" value="${force_abs_paths}" scope="parent"/>
    <!-- Read .yaml files from disk, load content into properties -->
    <xacro:property name="config_joint_limit_parameters" value="${xacro.load_yaml(joint_limits_parameters_file)}"/>
    <xacro:property name="config_kinematics_parameters" value="${xacro.load_yaml(kinematics_parameters_file)}"/>
    <xacro:property name="config_physical_parameters" value="${xacro.load_yaml(physical_parameters_file)}"/>
    <xacro:property name="config_visual_parameters" value="${xacro.load_yaml(visual_parameters_file)}"/>

    <xacro:property name="mesh_path" value="${mesh_path}" scope="parent"/>

    <!-- Extract subsections from yaml dictionaries -->
    <xacro:property name="sec_limits" value="${config_joint_limit_parameters['joint_limits']}"/>
    <xacro:property name="sec_dh_parameters" value="${config_physical_parameters['dh_parameters']}"/>
    <xacro:property name="sec_offsets" value="${config_physical_parameters['offsets']}"/>
    <xacro:property name="sec_inertia_parameters" value="${config_physical_parameters['inertia_parameters']}" />
    <xacro:property name="sec_mesh_files" value="${config_visual_parameters['mesh_files']}" scope="parent"/>
    <xacro:property name="sec_kinematics" value="${config_kinematics_parameters['kinematics']}" />

    <!-- JOINTS LIMIT PARAMETERS -->
    <xacro:property name="shoulder_pan_lower_limit" value="${sec_limits['shoulder_pan_joint']['min_position']}" scope="parent"/>
    <xacro:property name="shoulder_pan_upper_limit" value="${sec_limits['shoulder_pan_joint']['max_position']}" scope="parent"/>
    <xacro:property name="shoulder_pan_velocity_limit" value="${sec_limits['shoulder_pan_joint']['max_velocity']}" scope="parent"/>
    <xacro:property name="shoulder_pan_effort_limit" value="${sec_limits['shoulder_pan_joint']['max_effort']}" scope="parent"/>
    <xacro:property name="shoulder_lift_lower_limit" value="${sec_limits['shoulder_lift_joint']['min_position']}" scope="parent"/>
    <xacro:property name="shoulder_lift_upper_limit" value="${sec_limits['shoulder_lift_joint']['max_position']}" scope="parent"/>
    <xacro:property name="shoulder_lift_velocity_limit" value="${sec_limits['shoulder_lift_joint']['max_velocity']}" scope="parent"/>
    <xacro:property name="shoulder_lift_effort_limit" value="${sec_limits['shoulder_lift_joint']['max_effort']}" scope="parent"/>
    <xacro:property name="elbow_joint_lower_limit" value="${sec_limits['elbow_joint']['min_position']}" scope="parent"/>
    <xacro:property name="elbow_joint_upper_limit" value="${sec_limits['elbow_joint']['max_position']}" scope="parent"/>
    <xacro:property name="elbow_joint_velocity_limit" value="${sec_limits['elbow_joint']['max_velocity']}" scope="parent"/>
    <xacro:property name="elbow_joint_effort_limit" value="${sec_limits['elbow_joint']['max_effort']}" scope="parent"/>
    <xacro:property name="wrist_1_lower_limit" value="${sec_limits['wrist_1_joint']['min_position']}" scope="parent"/>
    <xacro:property name="wrist_1_upper_limit" value="${sec_limits['wrist_1_joint']['max_position']}" scope="parent"/>
    <xacro:property name="wrist_1_velocity_limit" value="${sec_limits['wrist_1_joint']['max_velocity']}" scope="parent"/>
    <xacro:property name="wrist_1_effort_limit" value="${sec_limits['wrist_1_joint']['max_effort']}" scope="parent"/>
    <xacro:property name="wrist_2_lower_limit" value="${sec_limits['wrist_2_joint']['min_position']}" scope="parent"/>
    <xacro:property name="wrist_2_upper_limit" value="${sec_limits['wrist_2_joint']['max_position']}" scope="parent"/>
    <xacro:property name="wrist_2_velocity_limit" value="${sec_limits['wrist_2_joint']['max_velocity']}" scope="parent"/>
    <xacro:property name="wrist_2_effort_limit" value="${sec_limits['wrist_2_joint']['max_effort']}" scope="parent"/>
    <xacro:property name="wrist_3_lower_limit" value="${sec_limits['wrist_3_joint']['min_position']}" scope="parent"/>
    <xacro:property name="wrist_3_upper_limit" value="${sec_limits['wrist_3_joint']['max_position']}" scope="parent"/>
    <xacro:property name="wrist_3_velocity_limit" value="${sec_limits['wrist_3_joint']['max_velocity']}" scope="parent"/>
    <xacro:property name="wrist_3_effort_limit" value="${sec_limits['wrist_3_joint']['max_effort']}" scope="parent"/>

    <!-- DH PARAMETERS -->
    <xacro:property name="d1" value="${sec_dh_parameters['d1']}" scope="parent"/>
    <xacro:property name="a2" value="${sec_dh_parameters['a2']}" scope="parent"/>
    <xacro:property name="a3" value="${sec_dh_parameters['a3']}" scope="parent"/>
    <xacro:property name="d4" value="${sec_dh_parameters['d4']}" scope="parent"/>
    <xacro:property name="d5" value="${sec_dh_parameters['d5']}" scope="parent"/>
    <xacro:property name="d6" value="${sec_dh_parameters['d6']}" scope="parent"/>

    <!-- kinematics -->
    <xacro:property name="shoulder_x" value="${sec_kinematics['shoulder']['x']}" scope="parent"/>
    <xacro:property name="shoulder_y" value="${sec_kinematics['shoulder']['y']}" scope="parent"/>
    <xacro:property name="shoulder_z" value="${sec_kinematics['shoulder']['z']}" scope="parent"/>
    <xacro:property name="shoulder_roll" value="${sec_kinematics['shoulder']['roll']}" scope="parent"/>
    <xacro:property name="shoulder_pitch" value="${sec_kinematics['shoulder']['pitch']}" scope="parent"/>
    <xacro:property name="shoulder_yaw" value="${sec_kinematics['shoulder']['yaw']}" scope="parent"/>

    <xacro:property name="upperarm_x" value="${sec_kinematics['upperarm']['x']}" scope="parent"/>
    <xacro:property name="upperarm_y" value="${sec_kinematics['upperarm']['y']}" scope="parent"/>
    <xacro:property name="upperarm_z" value="${sec_kinematics['upperarm']['z']}" scope="parent"/>
    <xacro:property name="upperarm_roll" value="${sec_kinematics['upperarm']['roll']}" scope="parent"/>
    <xacro:property name="upperarm_pitch" value="${sec_kinematics['upperarm']['pitch']}" scope="parent"/>
    <xacro:property name="upperarm_yaw" value="${sec_kinematics['upperarm']['yaw']}" scope="parent"/>

    <xacro:property name="forearm_x" value="${sec_kinematics['forearm']['x']}" scope="parent"/>
    <xacro:property name="forearm_y" value="${sec_kinematics['forearm']['y']}" scope="parent"/>
    <xacro:property name="forearm_z" value="${sec_kinematics['forearm']['z']}" scope="parent"/>
    <xacro:property name="forearm_roll" value="${sec_kinematics['forearm']['roll']}" scope="parent"/>
    <xacro:property name="forearm_pitch" value="${sec_kinematics['forearm']['pitch']}" scope="parent"/>
    <xacro:property name="forearm_yaw" value="${sec_kinematics['forearm']['yaw']}" scope="parent"/>

    <xacro:property name="wrist_1_x" value="${sec_kinematics['wrist_1']['x']}" scope="parent"/>
    <xacro:property name="wrist_1_y" value="${sec_kinematics['wrist_1']['y']}" scope="parent"/>
    <xacro:property name="wrist_1_z" value="${sec_kinematics['wrist_1']['z']}" scope="parent"/>
    <xacro:property name="wrist_1_roll" value="${sec_kinematics['wrist_1']['roll']}" scope="parent"/>
    <xacro:property name="wrist_1_pitch" value="${sec_kinematics['wrist_1']['pitch']}" scope="parent"/>
    <xacro:property name="wrist_1_yaw" value="${sec_kinematics['wrist_1']['yaw']}" scope="parent"/>

    <xacro:property name="wrist_2_x" value="${sec_kinematics['wrist_2']['x']}" scope="parent"/>
    <xacro:property name="wrist_2_y" value="${sec_kinematics['wrist_2']['y']}" scope="parent"/>
    <xacro:property name="wrist_2_z" value="${sec_kinematics['wrist_2']['z']}" scope="parent"/>
    <xacro:property name="wrist_2_roll" value="${sec_kinematics['wrist_2']['roll']}" scope="parent"/>
    <xacro:property name="wrist_2_pitch" value="${sec_kinematics['wrist_2']['pitch']}" scope="parent"/>
    <xacro:property name="wrist_2_yaw" value="${sec_kinematics['wrist_2']['yaw']}" scope="parent"/>

    <xacro:property name="wrist_3_x" value="${sec_kinematics['wrist_3']['x']}" scope="parent"/>
    <xacro:property name="wrist_3_y" value="${sec_kinematics['wrist_3']['y']}" scope="parent"/>
    <xacro:property name="wrist_3_z" value="${sec_kinematics['wrist_3']['z']}" scope="parent"/>
    <xacro:property name="wrist_3_roll" value="${sec_kinematics['wrist_3']['roll']}" scope="parent"/>
    <xacro:property name="wrist_3_pitch" value="${sec_kinematics['wrist_3']['pitch']}" scope="parent"/>
    <xacro:property name="wrist_3_yaw" value="${sec_kinematics['wrist_3']['yaw']}" scope="parent"/>

    <!-- INERTIA PARAMETERS -->
    <!-- mass -->
    <xacro:property name="base_mass" value="${sec_inertia_parameters['base_mass']}" scope="parent"/>
    <xacro:property name="shoulder_mass" value="${sec_inertia_parameters['shoulder_mass']}" scope="parent"/>
    <xacro:property name="upperarm_mass" value="${sec_inertia_parameters['upperarm_mass']}" scope="parent"/>
    <xacro:property name="forearm_mass" value="${sec_inertia_parameters['forearm_mass']}" scope="parent"/>
    <xacro:property name="wrist_1_mass" value="${sec_inertia_parameters['wrist_1_mass']}" scope="parent"/>
    <xacro:property name="wrist_2_mass" value="${sec_inertia_parameters['wrist_2_mass']}" scope="parent"/>
    <xacro:property name="wrist_3_mass" value="${sec_inertia_parameters['wrist_3_mass']}" scope="parent"/>

    <!-- center of mass -->
    <xacro:property name="prop_base_cog" value="${sec_inertia_parameters['center_of_mass']['base_cog']}" scope="parent"/>
    <xacro:property name="prop_shoulder_cog" value="${sec_inertia_parameters['center_of_mass']['shoulder_cog']}" scope="parent"/>
    <xacro:property name="prop_upperarm_cog" value="${sec_inertia_parameters['center_of_mass']['upperarm_cog']}" scope="parent"/>
    <xacro:property name="prop_forearm_cog" value="${sec_inertia_parameters['center_of_mass']['forearm_cog']}" scope="parent"/>
    <xacro:property name="prop_wrist_1_cog" value="${sec_inertia_parameters['center_of_mass']['wrist_1_cog']}" scope="parent"/>
    <xacro:property name="prop_wrist_2_cog" value="${sec_inertia_parameters['center_of_mass']['wrist_2_cog']}" scope="parent"/>
    <xacro:property name="prop_wrist_3_cog" value="${sec_inertia_parameters['center_of_mass']['wrist_3_cog']}" scope="parent"/>

    <!-- inertia  -->
    <xacro:property name="prop_base_inertia" value="${sec_inertia_parameters['inertia']['base']}" scope="parent"/>
    <xacro:property name="prop_shoulder_inertia" value="${sec_inertia_parameters['inertia']['shoulder']}" scope="parent"/>
    <xacro:property name="prop_upperarm_inertia" value="${sec_inertia_parameters['inertia']['upperarm']}" scope="parent"/>
    <xacro:property name="prop_forearm_inertia" value="${sec_inertia_parameters['inertia']['forearm']}" scope="parent"/>
    <xacro:property name="prop_wrist_1_inertia" value="${sec_inertia_parameters['inertia']['wrist_1']}" scope="parent"/>
    <xacro:property name="prop_wrist_2_inertia" value="${sec_inertia_parameters['inertia']['wrist_2']}" scope="parent"/>
    <xacro:property name="prop_wrist_3_inertia" value="${sec_inertia_parameters['inertia']['wrist_3']}" scope="parent"/>

  </xacro:macro>
</robot>
