Revision history [back]

Hi @Khule,

I will post this as an answer since to not have a cap in the max characters.

I took the liberty of using/modifying your files in order to solve the problem and I came up with this solution. In my last comment I said that the problem maybe the transmissions:

<transmission name="trans_1">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="joint_1">
    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
  </joint>
  <actuator name="motor_1">
    <mechanicalReduction>1</mechanicalReduction>
  </actuator>
 </transmission>

And that is technicaally not true since the proper definition seems to be:

<transmission name="tran1">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="joint_1">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
  </joint>
  <actuator name="motor1">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    <mechanicalReduction>1</mechanicalReduction>
  </actuator>
</transmission>

At least with the effort controllers, I used in the past the velocity ones and I did not need the hardwareInterface on the actuator part. In fact the official documentation says that the tag should be placed only in the joint but from my experience with your description if you do that the joint_state_controller is unable to produce a proper tf tree and ROS_control will not work since that tree is broken.

For this reason what I have done is properly instantiated new transmission, tidy up your description and provide full proper configuration and launchers. So I will post here all this files and hope this can help you.

The description should look like this:

<?xml version="1.0"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
     Commit Version: 1.5.1-0-g916b5db  Build Version: 1.5.7152.31018
     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot name="test_1_urdf">
  <link name="world"/>

  <joint name="fixed" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
  </joint>

  <link name="base_link">
    <inertial>
      <origin xyz="0.026901 0.024709 -0.0098697" rpy="0 0 0" />
      <mass value="0.34742" />
      <inertia
        ixx="0.00046368" ixy="-2.9001E-06" ixz="-3.7543E-19"
        iyy="0.0010571"  iyz="-5.1522E-19" izz="0.00076443" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://test_1_urdf/meshes/base_link.STL" />
      </geometry>
      <material name="base_link_material">
        <color rgba="0.79216 0.81961 0.93333 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://test_1_urdf/meshes/base_link.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="joint_1" type="revolute">
    <origin xyz="0 0.01 0" rpy="1.5708 0 0" />
    <parent link="base_link" />
    <child link="link_1" />
    <axis xyz="0 1 0" />
    <limit lower="-1.57" upper="1.57" effort="10" velocity="1" />
  </joint>

  <link name="link_1">
    <inertial>
      <origin xyz="-0.033996 0.053779 0.0013632" rpy="0 0 0" />
      <mass value="0.62403" />
      <inertia
        ixx="0.00055304" ixy="3.8373E-06"  ixz="1.4793E-07"
        iyy="0.00073956" iyz="-2.5812E-09" izz="0.00076923" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://test_1_urdf/meshes/link_1.STL" />
      </geometry>
      <material name="link_1_material">
        <color rgba="0.79216 0.81961 0.93333 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://test_1_urdf/meshes/link_1.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="joint_2" type="revolute">
    <origin xyz="-0.085 0.15 0" rpy="-1.5708 0 1.5365" />
    <parent link="link_1" />
    <child link="link_2" />
    <axis xyz="0 1 0" />
    <limit lower="-1.57" upper="1.57" effort="10" velocity="1" />
  </joint>

  <link name="link_2">
    <inertial>
      <origin xyz="0.093132 -0.0072567 0.0031795" rpy="0 0 0" />
      <mass value="0.66404" />
      <inertia
        ixx="0.0005867" ixy="3.2043E-20" ixz="-0.00010232"
        iyy="0.0033357" iyz="-9.491E-19" izz="0.003555" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://test_1_urdf/meshes/link_2.STL" />
      </geometry>
      <material name="link_2_material">
        <color rgba="0.79216 0.81961 0.93333 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://test_1_urdf/meshes/link_2.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="joint_3" type="revolute">
    <origin xyz="0.35479 -0.04038 0.012185" rpy="1.5708 0.034331 3.1416" />
    <parent link="link_2" />
    <child link="link_3" />
    <axis xyz="0 0 1" />
    <limit lower="-1.57" upper="1.57" effort="10" velocity="1" />
  </joint>

  <link name="link_3">
    <inertial>
      <origin xyz="6.1402E-05 1.9818E-05 -0.076313" rpy="0 0 0" />
      <mass value="0.12484" />
      <inertia 
        ixx="9.6476E-05" ixy="1.9635E-10" ixz="2.755E-07"
        iyy="7.9833E-05" iyz="8.8918E-08" izz="4.1182E-05" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://test_1_urdf/meshes/link_3.STL" />
      </geometry>
      <material name="link_3_material">
        <color rgba="0.79216 0.81961 0.93333 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://test_1_urdf/meshes/link_3.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="joint_4" type="revolute">
    <origin xyz="0 0 -0.1225" rpy="1.5708 0.73625 -1.5708" />
    <parent link="link_3" />
    <child link="link_4" />
    <axis xyz="0 0 1" />
    <limit lower="-1.57" upper="1.57" effort="10" velocity="1" />
  </joint>

  <link name="link_4">
    <inertial>
      <origin xyz="0.0052809 -0.016241 0.12532" rpy="0 0 0" />
      <mass value="0.35413" />
      <inertia
        ixx="0.0016351" ixy="1.9393E-05" ixz="-0.0002487"
        iyy="0.001665"  iyz="0.00010111" izz="0.00029855" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://test_1_urdf/meshes/link_4.STL" />
      </geometry>
      <material name="link_4_material">
        <color rgba="0.79216 0.81961 0.93333 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://test_1_urdf/meshes/link_4.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="joint_5" type="revolute">
    <origin mxyz="0.00014009 0.00012696 0.2605" mrpy="3.1416 0 2.307" />
    <parent link="link_4" />
    <child link="link_5" />
    <axis xyz="1 0 0" />
    <limit lower="-1.57" upper="1.57" effort="10" velocity="1" />
  </joint>

  <link name="link_5">
    <inertial>
      <origin xyz="0.000764 -1.3878E-17 -0.00069468" rpy="0 0 0" />
      <mass value="0.049859" />
      <inertia
        ixx="1.0433E-05" ixy="1.5128E-19" ixz="1.8054E-07"
        iyy="2.2582E-05" iyz="7.9112E-21" izz="2.5114E-05" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://test_1_urdf/meshes/link_5.STL" />
      </geometry>
      <material name="link_5_material">
        <color rgba="0.79216 0.81961 0.93333 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://test_1_urdf/meshes/link_5.STL" />
      </geometry>
    </collision>
  </link>

  <!--adding transmission tag -->
  <transmission name="tran1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint_1">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <transmission name="tran2">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint_2">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor2">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <transmission name="tran3">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint_3">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor3">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <transmission name="tran4">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint_4">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor4">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <transmission name="tran5">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint_5">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor5">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <gazebo>
      <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
        <robotNamespace>/test_1_urdf</robotNamespace>
        <robotParam>robot_description</robotParam>
        <controlPeriod>0.001</controlPeriod>
        <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
        <legacyModeNS>true</legacyModeNS>
      </plugin>
  </gazebo>

</robot>

I have only changed the position of several tag and params as well as the transmissions. Finally please note the properrobot_namespace for your robot name in the gazebo_ros_control plugin.

Then you should need proper launchers:

For the robot description

<launch>
  <arg name="vehicle" default="test_1_urdf"/>
  <arg name="sim" default="false"/>
  <arg name="output" default="log"/>

  <!-- Load the URDF into the ROS Parameter Server -->
  <param name="robot_description" textfile="$(find gazebo_test1)/urdf/test_1_urdf.urdf"/>

  <!-- State joints publisher for debug -->
  <node unless="$(arg sim)" name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" output="$(arg output)" />

  <!-- Combine joint values -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="$(arg output)" />
</launch>

For the robot control

<launch>
  <!-- Coordinates to spawn model -->
  <arg name="x" default="0.0"/>
  <arg name="y" default="0.0"/>
  <arg name="z" default="0.01"/>
  <arg name="roll" default="0.0"/>
  <arg name="pitch" default="0.0"/>
  <arg name="yaw" default="0.0"/>

  <!-- Prefix for links: Multiple vehicles -->
  <arg name="vehicle" default="test_1_urdf"/>
  <arg name="output" default="log"/>

  <!-- Load controllers config file into the ROS Parameter Server -->
  <rosparam file="$(find gazebo_test1)/config/test1_control.yaml" command="load"/>

  <!-- Start controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="$(arg output)" args="
    joint_1_position_controller
    joint_2_position_controller
    joint_3_position_controller
    joint_4_position_controller
    joint_5_position_controller
    joint_state_controller"
  />

  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="$(arg output)"
    args="-urdf -model $(arg vehicle) -param robot_description -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg roll) -P $(arg pitch) -Y $(arg yaw)"/>
</launch>

An finally to bring up all in Gazebo:

<launch>
  <arg name="sim" value="true"/>
  <arg name="vehicle" default="test_1_urdf"/>

  <!-- Coordinates to spawn model -->
  <arg name="x" default="0.0"/>
  <arg name="y" default="0.0"/>
  <arg name="z" default="0.05"/>
  <arg name="roll" default="0.0"/>
  <arg name="pitch" default="0.0"/>
  <arg name="yaw" default="0.0"/>

  <!-- Robot description for TF -->
  <group ns="$(arg vehicle)">
    <include file="$(find gazebo_test1)/launch/robot_description.launch">
      <arg name="sim" value="$(arg sim)"/>
      <arg name="vehicle" value="$(arg vehicle)"/>
    </include>
  </group>

  <include file="$(find gazebo_ros)/launch/empty_world.launch"/>

  <!-- Ares Spawner and controllers -->
  <group ns="$(arg vehicle)">
    <include file="$(find gazebo_test1)/launch/robot_control.launch">
      <arg name="x" value="$(arg x)"/>
      <arg name="y" value="$(arg y)"/>
      <arg name="z" value="$(arg z)"/>
      <arg name="roll" value="$(arg roll)"/>
      <arg name="pitch" value="$(arg pitch)"/>
      <arg name="yaw" value="$(arg yaw)"/>
      <arg name="vehicle" value="$(arg vehicle)"/>
    </include>
  </group>

  <!-- Show in Rviz   -->
  <node name="rviz" pkg="rviz" type="rviz" />

</launch>

Please note that if you already use the joint_state_controller you do not nee to launch the joint_state_publisher node and that you will need to generate the config and launch folders for your project to store properly these files.

Hope this helps you with the problem.

Regards.