Unable to use Contact plugin to get contact data from robot

[dashanan13]

Hei Community,

I am trying to simulate a snake robot that is navigating obstacles.

In the process, i need to know when a part or link of the robot hits an obstacle and how much force it experiences from the contact.

To accomplish this, i am using "contact plugin" but all i get is

• a log entry where topic name is different from the topic name i defined: [gzserver-1] [INFO] [1714412192.556439992] [link1_bumper_plugin]: Publishing contact states to [/bumper_states]

• and no data upon quering this new topic name: header: stamp: sec: 417 nanosec: 723000000 frame_id: link_2 states: []

The code i use to setup gazebo reference is below.

    <gazebo reference="link_2">
        <material>Gazebo/Blue</material>
        <sensor name="link1_contact_bumper" type="contact">
            <always_on>true</always_on>
            <contact>
                <collision>link_2_collision</collision>
            </contact>
            <update_rate>1</update_rate>
            <plugin name="link1_bumper_plugin" filename="libgazebo_ros_bumper.so">
                <frame_name>link_2</frame_name>
                <bumperTopicName>link1_bumper</bumperTopicName>
            </plugin>
        </sensor>
    </gazebo>

Please can you help get this contact sensor working so i can get contact force and other information from the plugin.

Code Repository: https://github.com/dashanan13/snake-robot-sim/tree/main Branch: 3obstacles-contactsensor

My URDF files, controller file and ROS control file are listed below.

<!--  * * ros2_control.xacro *  *  -->

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

    <gazebo reference="link_1">
        <material>Gazebo/Green</material>
    </gazebo>

    <gazebo reference="link_2">
        <material>Gazebo/Blue</material>
        <sensor name="link1_contact_bumper" type="contact">
            <always_on>true</always_on>
            <contact>
                <collision>link_2_collision</collision>
            </contact>
            <update_rate>1</update_rate>
            <plugin name="link1_bumper_plugin" filename="libgazebo_ros_bumper.so">
                <frame_name>link_2</frame_name>
                <bumperTopicName>link1_bumper</bumperTopicName>
            </plugin>
        </sensor>
    </gazebo>

    <gazebo reference="link_3">
        <material>Gazebo/Orange</material>
    </gazebo>

    <gazebo reference="link_4">
        <material>Gazebo/White</material>
    </gazebo>

    <gazebo reference="link_5">
        <material>Gazebo/Yellow</material>
    </gazebo>

    <gazebo reference="link_6">
        <material>Gazebo/Purple</material>
    </gazebo>

    <gazebo reference="link_7">
        <material>Gazebo/Black</material>
    </gazebo>

    <gazebo reference="link_8">
        <material>Gazebo/Indigo</material>
    </gazebo>

    <gazebo reference="link_9">
        <material>Gazebo/RedBright</material>
    </gazebo>

    <gazebo reference="link_10">
        <material>Gazebo/Turquoise</material>
    </gazebo>

    <ros2_control name="GazeboSystem" type="system">
        <hardware>
            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
        </hardware>

        <joint name="link_1_2_joint">
            <command_interface name="effort">
                <param name="min">0</param>
                <param name="max">200</param>
            </command_interface>
            <state_interface name="effort"/>
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>

        <joint name="link_2_3_joint">
            <command_interface name="effort">
                <param name="min">0</param>
                <param name="max">200</param>
            </command_interface>
            <state_interface name="effort"/>
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>

        <joint name="link_3_4_joint">
            <command_interface name="effort">
                <param name="min">0</param>
                <param name="max">200</param>
            </command_interface>
            <state_interface name="effort"/>
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>

        <joint name="link_4_5_joint">
            <command_interface name="effort">
                <param name="min">0</param>
                <param name="max">200</param>
            </command_interface>
            <state_interface name="effort"/>
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>

        <joint name="link_5_6_joint">
            <command_interface name="effort">
                <param name="min">0</param>
                <param name="max">200</param>
            </command_interface>
            <state_interface name="effort"/>
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>

        <joint name="link_6_7_joint">
            <command_interface name="effort">
                <param name="min">0</param>
                <param name="max">200</param>
            </command_interface>
            <state_interface name="effort"/>
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>

        <joint name="link_7_8_joint">
            <command_interface name="effort">
                <param name="min">0</param>
                <param name="max">200</param>
            </command_interface>
            <state_interface name="effort"/>
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>

        <joint name="link_8_9_joint">
            <command_interface name="effort">
                <param name="min">0</param>
                <param name="max">200</param>
            </command_interface>
            <state_interface name="effort"/>
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>

        <joint name="link_9_10_joint">
            <command_interface name="effort">
                <param name="min">0</param>
                <param name="max">200</param>
            </command_interface>
            <state_interface name="effort"/>
            <state_interface name="position"/>
            <state_interface name="velocity"/>
        </joint>
    </ros2_control>

    <gazebo>

        <plugin name="gazebo_ros_planar_move" filename="libgazebo_ros_planar_move.so">

            <update_rate>100</update_rate>
            <publish_rate>10</publish_rate>

            <!-- output -->
            <publish_odom>true</publish_odom>
            <publish_odom_tf>true</publish_odom_tf>

            <odometry_frame>odom</odometry_frame>
            <robot_base_frame>base_link</robot_base_frame>

        </plugin>

        <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
            <parameters>$(find snake_bot)/config/controllers.yaml</parameters>    
        </plugin>

        <plugin name="ft_sensor_joint12" filename="libgazebo_ros_ft_sensor.so">
            <updateRate>100.0</updateRate>
            <remapping>wrench:=joint_1</remapping>
            <joint_name>link_1_2_joint</joint_name>
            <gaussian_noise>0.01</gaussian_noise>
            <update_rate>1</update_rate>
        </plugin>

        <plugin name="bumper_ft_sensor" filename="path/to/libgazebo_ros_ft_sensor.so">
            <updateRate>100.0</updateRate>
            <remapping>wrench:=bumper_link</remapping>  
            <joint_name>link_1</joint_name>
            <gaussian_noise>0.01</gaussian_noise>
            <update_rate>1</update_rate>
        </plugin>

    </gazebo>

</robot>

<!--  * * snakebot.urdf *  *  -->

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from snakebot.xacro                 | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="snakebot">
  <!-- This file is not a robot in and of itself, it just contains some useful tags that could be included in any robot -->
  <!-- Specify some standard inertial calculations https://en.wikipedia.org/wiki/List_of_moments_of_inertia -->
  <!-- These make use of xacro's mathematical functionality -->
  <!-- COLORS -->
  <material name="white">
    <color rgba="1 1 1 1"/>
  </material>
  <material name="black">
    <color rgba="0 0 0 1"/>
  </material>
  <material name="blue">
    <color rgba="0.0 0.0 0.8 1.0"/>
  </material>
  <material name="green">
    <color rgba="0.0 0.8 0.0 1.0"/>
  </material>
  <material name="orange">
    <color rgba="1.0 0.4235294117647059 0.0392156862745098 1.0"/>
  </material>
  <material name="brown">
    <color rgba="0.8705882352941177 0.8117647058823529 0.7647058823529411 1.0"/>
  </material>
  <material name="red">
    <color rgba="0.8 0.0 0.0 1.0"/>
  </material>
  <gazebo reference="link_1">
    <material>Gazebo/Green</material>
  </gazebo>
  <gazebo reference="link_2">
    <material>Gazebo/Blue</material>
    <sensor name="link1_contact_bumper" type="contact">
      <always_on>true</always_on>
      <contact>
        <collision>link_2</collision>
      </contact>
      <update_rate>1</update_rate>
      <plugin filename="libgazebo_ros_bumper.so" name="link1_bumper_plugin">
        <frame_name>link_2</frame_name>
        <bumperTopicName>link1_bumper</bumperTopicName>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo reference="link_3">
    <material>Gazebo/Orange</material>
  </gazebo>
  <gazebo reference="link_4">
    <material>Gazebo/White</material>
  </gazebo>
  <gazebo reference="link_5">
    <material>Gazebo/Yellow</material>
  </gazebo>
  <gazebo reference="link_6">
    <material>Gazebo/Purple</material>
  </gazebo>
  <gazebo reference="link_7">
    <material>Gazebo/Black</material>
  </gazebo>
  <gazebo reference="link_8">
    <material>Gazebo/Indigo</material>
  </gazebo>
  <gazebo reference="link_9">
    <material>Gazebo/RedBright</material>
  </gazebo>
  <gazebo reference="link_10">
    <material>Gazebo/Turquoise</material>
  </gazebo>
  <ros2_control name="GazeboSystem" type="system">
    <hardware>
      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
    </hardware>

    <joint name="link_1_2_joint">
      <command_interface name="effort">
        <param name="min">0</param>
        <param name="max">200</param>
      </command_interface>
      <state_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
    <joint name="link_2_3_joint">
      <command_interface name="effort">
        <param name="min">0</param>
        <param name="max">200</param>
      </command_interface>
      <state_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
    <joint name="link_3_4_joint">
      <command_interface name="effort">
        <param name="min">0</param>
        <param name="max">200</param>
      </command_interface>
      <state_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
    <joint name="link_4_5_joint">
      <command_interface name="effort">
        <param name="min">0</param>
        <param name="max">200</param>
      </command_interface>
      <state_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
    <joint name="link_5_6_joint">
      <command_interface name="effort">
        <param name="min">0</param>
        <param name="max">200</param>
      </command_interface>
      <state_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
    <joint name="link_6_7_joint">
      <command_interface name="effort">
        <param name="min">0</param>
        <param name="max">200</param>
      </command_interface>
      <state_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
    <joint name="link_7_8_joint">
      <command_interface name="effort">
        <param name="min">0</param>
        <param name="max">200</param>
      </command_interface>
      <state_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
    <joint name="link_8_9_joint">
      <command_interface name="effort">
        <param name="min">0</param>
        <param name="max">200</param>
      </command_interface>
      <state_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
    <joint name="link_9_10_joint">
      <command_interface name="effort">
        <param name="min">0</param>
        <param name="max">200</param>
      </command_interface>
      <state_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
  </ros2_control>
  <gazebo>
    <plugin filename="libgazebo_ros_planar_move.so" name="gazebo_ros_planar_move">
      <update_rate>100</update_rate>
      <publish_rate>10</publish_rate>
      <!-- output -->
      <publish_odom>true</publish_odom>
      <publish_odom_tf>true</publish_odom_tf>
      <odometry_frame>odom</odometry_frame>
      <robot_base_frame>base_link</robot_base_frame>
    </plugin>
    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
      <parameters>/home/mohit/GITHUBRepos/snake-robot-sim/install/snake_bot/share/snake_bot/config/controllers.yaml</parameters>
    </plugin>
    <plugin filename="libgazebo_ros_ft_sensor.so" name="ft_sensor_joint12">
      <updateRate>100.0</updateRate>
      <remapping>wrench:=joint_1</remapping>
      <joint_name>link_1_2_joint</joint_name>
      <gaussian_noise>0.01</gaussian_noise>
      <update_rate>1</update_rate>
    </plugin>
    <plugin filename="path/to/libgazebo_ros_ft_sensor.so" name="bumper_ft_sensor">
      <updateRate>100.0</updateRate>
      <remapping>wrench:=bumper_link</remapping>
      <joint_name>link_1</joint_name>
      <gaussian_noise>0.01</gaussian_noise>
      <update_rate>1</update_rate>
    </plugin>
  </gazebo>
  <joint name="camera_joint" type="fixed">
    <parent link="link_10"/>
    <child link="camera_link"/>
    <origin rpy="0 -0.17453292519943295 1.5707963267948966" xyz="0 0.14 0.03"/>
  </joint>
  <link name="camera_link">
    <visual>
      <geometry>
        <box size="0.010 0.01 0.01"/>
      </geometry>
      <material name="red"/>
    </visual>
  </link>
  <joint name="camera_optical_joint" type="fixed">
    <parent link="camera_link"/>
    <child link="camera_link_optical"/>
    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
  </joint>
  <link name="camera_link_optical"/>
  <gazebo reference="camera_link">
    <material>Gazebo/Red</material>
    <sensor name="camera" type="camera">
      <pose> 0 0 0 0 0 0 </pose>
      <visualize>true</visualize>
      <update_rate>10</update_rate>
      <camera>
        <horizontal_fov>1.089</horizontal_fov>
        <image>
          <format>R8G8B8</format>
          <width>640</width>
          <height>480</height>
        </image>
        <clip>
          <near>0.05</near>
          <far>8.0</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="camera_controller">
        <frame_name>camera_link_optical</frame_name>
      </plugin>
    </sensor>
  </gazebo>
  <link name="base_link"/>
  <joint name="base_joint" type="fixed">
    <parent link="base_link"/>
    <child link="link_1"/>
    <origin rpy="0 0 0.523599" xyz="0.0 0 0.0"/>
  </joint>
  <!--LINK_1 -->
  <link name="link_1">
    <visual>
      <origin rpy="0.0 0.0 0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0.0 0.0 0" xyz="0.0 0.07 0.03"/>
      <mass value="1"/>
      <inertia ixx="0.0019333333333333333" ixy="0.0" ixz="0.0" iyy="0.0006" iyz="0.0" izz="0.0019333333333333333"/>
    </inertial>
  </link>
  <!-- JOINT BETWEEN THE TAIL AND THE  LINK_2 -->
  <joint name="link_1_2_joint" type="revolute">
    <parent link="link_1"/>
    <child link="link_2"/>
    <limit effort="1000.0" lower="-0.785398163397" upper="0.785398163397" velocity="50.0"/>
    <origin rpy="0 0 -0.174533" xyz="0.0 0.175 0.0"/>
    <axis xyz="0 0 1"/>
    <joint_properties damping="6.0" friction="6.0"/>
  </joint>
  <!-- LINK 2 -->
  <link name="link_2">
    <visual>
      <material name="black"/>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <mass value="1"/>
      <inertia ixx="0.0019333333333333333" ixy="0.0" ixz="0.0" iyy="0.0006" iyz="0.0" izz="0.0019333333333333333"/>
    </inertial>
  </link>
  <!-- JOINT BETWEEM LINK 2 AND 3 -->
  <joint name="link_2_3_joint" type="revolute">
    <parent link="link_2"/>
    <child link="link_3"/>
    <limit effort="1000.0" lower="-0.785398163397" upper="0.785398163397" velocity="50.0"/>
    <origin rpy="0 0 -0.174533" xyz="0.0 0.175 0.0"/>
    <axis xyz="0 0 1"/>
    <joint_properties damping="6.0" friction="6.0"/>
  </joint>
  <!-- LINK 3 -->
  <link name="link_3">
    <visual>
      <material name="orange"/>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <mass value="1"/>
      <inertia ixx="0.0019333333333333333" ixy="0.0" ixz="0.0" iyy="0.0006" iyz="0.0" izz="0.0019333333333333333"/>
    </inertial>
  </link>
  <!--JOINT BETWEEN LINK 3 AND LINK 4 -->
  <joint name="link_3_4_joint" type="revolute">
    <parent link="link_3"/>
    <child link="link_4"/>
    <limit effort="1000.0" lower="-0.785398163397" upper="0.785398163397" velocity="50.0"/>
    <origin rpy="0 0 -0.174533" xyz="0.0 0.175 0.0"/>
    <axis xyz="0 0 1"/>
    <joint_properties damping="6.0" friction="6.0"/>
  </joint>
  <!-- LINK 4 -->
  <link name="link_4">
    <visual>
      <material name="black"/>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <mass value="1"/>
      <inertia ixx="0.0019333333333333333" ixy="0.0" ixz="0.0" iyy="0.0006" iyz="0.0" izz="0.0019333333333333333"/>
    </inertial>
  </link>
  <joint name="link_4_5_joint" type="revolute">
    <parent link="link_4"/>
    <child link="link_5"/>
    <limit effort="1000.0" lower="-0.785398163397" upper="0.785398163397" velocity="50.0"/>
    <origin rpy="0 0 -0.174533" xyz="0.0 0.175 0.0"/>
    <axis xyz="0 0 1"/>
    <joint_properties damping="6.0" friction="6.0"/>
  </joint>
  <!-- LINK 5-->
  <link name="link_5">
    <visual>
      <material name="orange"/>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <mass value="1"/>
      <inertia ixx="0.0019333333333333333" ixy="0.0" ixz="0.0" iyy="0.0006" iyz="0.0" izz="0.0019333333333333333"/>
    </inertial>
  </link>
  <joint name="link_5_6_joint" type="revolute">
    <parent link="link_5"/>
    <child link="link_6"/>
    <limit effort="1000.0" lower="-0.785398163397" upper="0.785398163397" velocity="50.0"/>
    <origin rpy="0 0 -0.174533" xyz="0.0 0.175 0.0"/>
    <axis xyz="0 0 1"/>
    <joint_properties damping="6.0" friction="6.0"/>
  </joint>
  <!--  LINK 6-->
  <link name="link_6">
    <visual>
      <material name="black"/>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <mass value="1"/>
      <inertia ixx="0.0019333333333333333" ixy="0.0" ixz="0.0" iyy="0.0006" iyz="0.0" izz="0.0019333333333333333"/>
    </inertial>
  </link>
  <!-- JOINT BETWEEN LINK 6 AND LINK 7 -->
  <joint name="link_6_7_joint" type="revolute">
    <parent link="link_6"/>
    <child link="link_7"/>
    <limit effort="1000.0" lower="-0.785398163397" upper="0.785398163397" velocity="50.0"/>
    <origin rpy="0 0 -0.174533" xyz="0.0 0.175 0.0"/>
    <axis xyz="0 0 1"/>
    <joint_properties damping="6.0" friction="6.0"/>
  </joint>
  <!--  LINK 7-->
  <link name="link_7">
    <visual>
      <material name="orange"/>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <mass value="1"/>
      <inertia ixx="0.0019333333333333333" ixy="0.0" ixz="0.0" iyy="0.0006" iyz="0.0" izz="0.0019333333333333333"/>
    </inertial>
  </link>
  <!-- JOINT BETWEEN LINK 7 AND LINK 8 -->
  <joint name="link_7_8_joint" type="revolute">
    <parent link="link_7"/>
    <child link="link_8"/>
    <limit effort="1000.0" lower="-0.785398163397" upper="0.785398163397" velocity="50.0"/>
    <origin rpy="0 0 0.174533" xyz="0.0 0.175 0.0"/>
    <axis xyz="0 0 1"/>
    <joint_properties damping="6.0" friction="6.0"/>
  </joint>
  <!--  LINK 8-->
  <link name="link_8">
    <visual>
      <material name="black"/>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <mass value="1"/>
      <inertia ixx="0.0019333333333333333" ixy="0.0" ixz="0.0" iyy="0.0006" iyz="0.0" izz="0.0019333333333333333"/>
    </inertial>
  </link>
  <!-- JOINT BETWEEN LINK 8 AND LINK 9 -->
  <joint name="link_8_9_joint" type="revolute">
    <parent link="link_8"/>
    <child link="link_9"/>
    <limit effort="1000.0" lower="-0.785398163397" upper="0.785398163397" velocity="50.0"/>
    <origin rpy="0 0 0.174533" xyz="0.0 0.175 0.0"/>
    <axis xyz="0 0 1"/>
    <joint_properties damping="6.0" friction="6.0"/>
  </joint>
  <!--  LINK 9-->
  <link name="link_9">
    <visual>
      <material name="orange"/>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <mass value="1"/>
      <inertia ixx="0.0019333333333333333" ixy="0.0" ixz="0.0" iyy="0.0006" iyz="0.0" izz="0.0019333333333333333"/>
    </inertial>
  </link>
  <!-- JOINT BETWEEN LINK 9 AND LINK 10 -->
  <joint name="link_9_10_joint" type="revolute">
    <parent link="link_9"/>
    <child link="link_10"/>
    <limit effort="1000.0" lower="-0.785398163397" upper="0.785398163397" velocity="50.0"/>
    <origin rpy="0 0 0.174533" xyz="0.0 0.175 0.0"/>
    <axis xyz="0 0 1"/>
    <joint_properties damping="6.0" friction="6.0"/>
  </joint>
  <!--  LINK 10-->
  <link name="link_10">
    <visual>
      <material name="black"/>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <geometry>
        <box size="0.06 0.14 0.06"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.07 0.03"/>
      <mass value="1"/>
      <inertia ixx="0.0019333333333333333" ixy="0.0" ixz="0.0" iyy="0.0006" iyz="0.0" izz="0.0019333333333333333"/>
    </inertial>
  </link>
  <!--gazebo>
          <plugin name="gazebo_ros_joint_state_publisher"
                    filename="libgazebo_ros_joint_state_publisher.so">
                    <update_rate>50</update_rate>
                    <joint_name>link_1_2_joint</joint_name>
                    <joint_name>link_2_3_joint</joint_name>
                    <joint_name>link_3_4_joint</joint_name>
                    <joint_name>link_4_5_joint</joint_name>
                    <joint_name>link_5_6_joint</joint_name>
                    <joint_name>link_6_7_joint</joint_name>
                    <joint_name>link_7_8_joint</joint_name>
                    <joint_name>link_8_9_joint</joint_name>
                    <joint_name>link_9_10_joint</joint_name>
        </plugin>
    </gazebo-->
</robot>

  <!-- controller.yaml -->

controller_manager:
  ros__parameters:
    update_rate: 100

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    joints1_controllers:
      type: forward_command_controller/ForwardCommandController

    joints2_controllers:
      type: forward_command_controller/ForwardCommandController

    joints3_controllers:
      type: forward_command_controller/ForwardCommandController

    joints4_controllers:
      type: forward_command_controller/ForwardCommandController

    joints5_controllers:
      type: forward_command_controller/ForwardCommandController

    joints6_controllers:
      type: forward_command_controller/ForwardCommandController

    joints7_controllers:
      type: forward_command_controller/ForwardCommandController

    joints8_controllers:
      type: forward_command_controller/ForwardCommandController

    joints9_controllers:
      type: forward_command_controller/ForwardCommandController
# forward_command_controller/ForwardCommandController
# position_controllers/JointGroupPositionController
  # Position Controllers ---------------------------------------
joint_state_broadcaster:
   ros__parameters:
    joints:
      - link_1_2_joint
      - link_2_3_joint
      - link_3_4_joint
      - link_4_5_joint
      - link_5_6_joint
      - link_6_7_joint
      - link_7_8_joint
      - link_8_9_joint  
      - link_9_10_joint  

joints1_controllers:
   ros__parameters:
    joints:
      - link_1_2_joint
    interface_name: effort
    command_interfaces:
      - effort   

joints2_controllers:
   ros__parameters:
    joints:
      - link_2_3_joint
    interface_name: effort
    command_interfaces:
      - effort  

joints3_controllers:
   ros__parameters:
    joints:
      - link_3_4_joint
    interface_name: effort
    command_interfaces:
      - effort 

joints4_controllers:
   ros__parameters:
    joints:
      - link_4_5_joint
    interface_name: effort
    command_interfaces:
      - effort  

joints5_controllers:
   ros__parameters:
    joints:
      - link_5_6_joint
    interface_name: effort
    command_interfaces:
      - effort  

joints6_controllers:
   ros__parameters:
    joints:
      - link_6_7_joint
    interface_name: effort
    command_interfaces:
      - effort  

joints7_controllers:
   ros__parameters:
    joints:
      - link_7_8_joint
    interface_name: effort
    command_interfaces:
      - effort  

joints8_controllers:
   ros__parameters:
    joints:
      - link_8_9_joint
    interface_name: effort
    command_interfaces:
      - effort 

joints9_controllers:
   ros__parameters:
    joints:
      - link_9_10_joint
    interface_name: effort
    command_interfaces:
      - effort