Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Unable to use Contact plugin to get contact data from robot #1530

Open
dashanan13 opened this issue Apr 29, 2024 · 0 comments
Open

Unable to use Contact plugin to get contact data from robot #1530

dashanan13 opened this issue Apr 29, 2024 · 0 comments

Comments

@dashanan13
Copy link

dashanan13 commented Apr 29, 2024

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.

snakerobot

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  
  
  
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant