openarm_description/urdf/ros2_control/openarm.bimanual.ros2_control.xacro

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

<xacro:macro name="openarm_bimanual_ros2_control"
         params="arm_type
             control_gains
             left_can_interface:=^|can1
             right_can_interface:=^|can0
             use_fake_hardware:=^|false
             fake_sensor_commands:=^|false
             hand:=^|false
             left_arm_prefix:=^|left_
             right_arm_prefix:=^|right_
             can_fd:=^|false
             gravity_compensation_enabled:=^|true
             gravity_compensation_use_kdl:=^|true
             gravity_torque_mode:=^|true
             gravity_scale:=^|2.5
             gravity_tau_limit1:=^|10.0
             gravity_tau_limit2:=^|10.0
             gravity_tau_limit3:=^|5.0
             gravity_tau_limit4:=^|5.0
             gravity_tau_limit5:=^|5.0
             gravity_tau_limit6:=^|3.0
             gravity_tau_limit7:=^|3.0
             kdl_urdf_path:=^|/tmp/openarm_bimanual_runtime.urdf
             kdl_base_link:=^|openarm_body_link0
             kdl_tip_link:=^|">

    <!-- Left Arm Hardware Interface -->
    <ros2_control name="openarm_left_hardware_interface" type="system">
        <hardware>
          <param name="arm_type">${arm_type}</param>
          <xacro:if value="${use_fake_hardware}">
            <plugin>mock_components/GenericSystem</plugin>
            <param name="fake_sensor_commands">${fake_sensor_commands}</param>
            <param name="state_following_offset">0.0</param>
          </xacro:if>
          <xacro:unless value="${use_fake_hardware}">
            <plugin>openarm_hardware/OpenArm_v10HW</plugin>
            <param name="can_interface">${left_can_interface}</param>
            <param name="arm_prefix">${left_arm_prefix}</param>
            <param name="hand">${hand}</param>
            <param name="can_fd">${can_fd}</param>
            <param name="gravity_compensation_enabled">${gravity_compensation_enabled}</param>
            <param name="gravity_compensation_use_kdl">${gravity_compensation_use_kdl}</param>
            <param name="gravity_torque_mode">${gravity_torque_mode}</param>
            <param name="gravity_scale">${gravity_scale}</param>
            <param name="gravity_tau_limit1">${gravity_tau_limit1}</param>
            <param name="gravity_tau_limit2">${gravity_tau_limit2}</param>
            <param name="gravity_tau_limit3">${gravity_tau_limit3}</param>
            <param name="gravity_tau_limit4">${gravity_tau_limit4}</param>
            <param name="gravity_tau_limit5">${gravity_tau_limit5}</param>
            <param name="gravity_tau_limit6">${gravity_tau_limit6}</param>
            <param name="gravity_tau_limit7">${gravity_tau_limit7}</param>
            <param name="kdl_urdf_path">${kdl_urdf_path}</param>
            <param name="kdl_base_link">${kdl_base_link}</param>
            <param name="kdl_tip_link">${kdl_tip_link if kdl_tip_link != '' else 'openarm_' + left_arm_prefix + 'link7'}</param>
            <param name="kp1">${control_gains['joint1']['kp']}</param>
            <param name="kp2">${control_gains['joint2']['kp']}</param>
            <param name="kp3">${control_gains['joint3']['kp']}</param>
            <param name="kp4">${control_gains['joint4']['kp']}</param>
            <param name="kp5">${control_gains['joint5']['kp']}</param>
            <param name="kp6">${control_gains['joint6']['kp']}</param>
            <param name="kp7">${control_gains['joint7']['kp']}</param>
            <param name="kd1">${control_gains['joint1']['kd']}</param>
            <param name="kd2">${control_gains['joint2']['kd']}</param>
            <param name="kd3">${control_gains['joint3']['kd']}</param>
            <param name="kd4">${control_gains['joint4']['kd']}</param>
            <param name="kd5">${control_gains['joint5']['kd']}</param>
            <param name="kd6">${control_gains['joint6']['kd']}</param>
            <param name="kd7">${control_gains['joint7']['kd']}</param>
          </xacro:unless>
        </hardware>

      <xacro:macro name="configure_joint" params="joint_name initial_position">
        <joint name="${joint_name}">
          <command_interface name="position"/>
          <command_interface name="velocity"/>
          <command_interface name="effort"/>

          <state_interface name="position">
            <param name="initial_value">${initial_position}</param>
          </state_interface>

          <state_interface name="velocity">
            <param name="initial_value">0.0</param>
          </state_interface>

          <state_interface name="effort">
            <param name="initial_value">0.0</param>
          </state_interface>
        </joint>
      </xacro:macro>

      <xacro:configure_joint joint_name="openarm_${left_arm_prefix}joint1" initial_position="0.0"/>
      <xacro:configure_joint joint_name="openarm_${left_arm_prefix}joint2" initial_position="0.0"/>
      <xacro:configure_joint joint_name="openarm_${left_arm_prefix}joint3" initial_position="0.0"/>
      <xacro:configure_joint joint_name="openarm_${left_arm_prefix}joint4" initial_position="0.0"/>
      <xacro:configure_joint joint_name="openarm_${left_arm_prefix}joint5" initial_position="0.0"/>
      <xacro:configure_joint joint_name="openarm_${left_arm_prefix}joint6" initial_position="0.0"/>
      <xacro:configure_joint joint_name="openarm_${left_arm_prefix}joint7" initial_position="0.0"/>

      <xacro:if value="${hand}">
        <xacro:configure_joint joint_name="openarm_${left_arm_prefix}finger_joint1" initial_position="0.0" />
      </xacro:if>
    </ros2_control>

    <!-- Right Arm Hardware Interface -->
    <ros2_control name="openarm_right_hardware_interface" type="system">
        <hardware>
          <param name="arm_type">${arm_type}</param>
          <xacro:if value="${use_fake_hardware}">
            <plugin>mock_components/GenericSystem</plugin>
            <param name="fake_sensor_commands">${fake_sensor_commands}</param>
            <param name="state_following_offset">0.0</param>
          </xacro:if>
          <xacro:unless value="${use_fake_hardware}">
            <plugin>openarm_hardware/OpenArm_v10HW</plugin>
            <param name="can_interface">${right_can_interface}</param>
            <param name="arm_prefix">${right_arm_prefix}</param>
            <param name="hand">${hand}</param>
            <param name="can_fd">${can_fd}</param>
            <param name="gravity_compensation_enabled">${gravity_compensation_enabled}</param>
            <param name="gravity_compensation_use_kdl">${gravity_compensation_use_kdl}</param>
            <param name="gravity_torque_mode">${gravity_torque_mode}</param>
            <param name="gravity_scale">${gravity_scale}</param>
            <param name="gravity_tau_limit1">${gravity_tau_limit1}</param>
            <param name="gravity_tau_limit2">${gravity_tau_limit2}</param>
            <param name="gravity_tau_limit3">${gravity_tau_limit3}</param>
            <param name="gravity_tau_limit4">${gravity_tau_limit4}</param>
            <param name="gravity_tau_limit5">${gravity_tau_limit5}</param>
            <param name="gravity_tau_limit6">${gravity_tau_limit6}</param>
            <param name="gravity_tau_limit7">${gravity_tau_limit7}</param>
            <param name="kdl_urdf_path">${kdl_urdf_path}</param>
            <param name="kdl_base_link">${kdl_base_link}</param>
            <param name="kdl_tip_link">${kdl_tip_link if kdl_tip_link != '' else 'openarm_' + right_arm_prefix + 'link7'}</param>
            <param name="kp1">${control_gains['joint1']['kp']}</param>
            <param name="kp2">${control_gains['joint2']['kp']}</param>
            <param name="kp3">${control_gains['joint3']['kp']}</param>
            <param name="kp4">${control_gains['joint4']['kp']}</param>
            <param name="kp5">${control_gains['joint5']['kp']}</param>
            <param name="kp6">${control_gains['joint6']['kp']}</param>
            <param name="kp7">${control_gains['joint7']['kp']}</param>
            <param name="kd1">${control_gains['joint1']['kd']}</param>
            <param name="kd2">${control_gains['joint2']['kd']}</param>
            <param name="kd3">${control_gains['joint3']['kd']}</param>
            <param name="kd4">${control_gains['joint4']['kd']}</param>
            <param name="kd5">${control_gains['joint5']['kd']}</param>
            <param name="kd6">${control_gains['joint6']['kd']}</param>
            <param name="kd7">${control_gains['joint7']['kd']}</param>
          </xacro:unless>
        </hardware>

      <xacro:configure_joint joint_name="openarm_${right_arm_prefix}joint1" initial_position="0.0"/>
      <xacro:configure_joint joint_name="openarm_${right_arm_prefix}joint2" initial_position="0.0"/>
      <xacro:configure_joint joint_name="openarm_${right_arm_prefix}joint3" initial_position="0.0"/>
      <xacro:configure_joint joint_name="openarm_${right_arm_prefix}joint4" initial_position="0.0"/>
      <xacro:configure_joint joint_name="openarm_${right_arm_prefix}joint5" initial_position="0.0"/>
      <xacro:configure_joint joint_name="openarm_${right_arm_prefix}joint6" initial_position="0.0"/>
      <xacro:configure_joint joint_name="openarm_${right_arm_prefix}joint7" initial_position="0.0"/>

      <xacro:if value="${hand}">
        <xacro:configure_joint joint_name="openarm_${right_arm_prefix}finger_joint1" initial_position="0.0" />
      </xacro:if>
    </ros2_control>

  </xacro:macro>

</robot>