ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Hi, first, in order to move your robot joints you can use Gazebo or ROS. I recommend to use ROS because it is very well integrated, several controllers already provided, and it is a lot easier. However, if you want to learn how to do it with Gazebo itself, have a look at this git.

If you want to control the robot with ROS, then you need to do first a translation of your model to URDF. Sorry SDF does not work with ROS, specially if you want to show the robot in Rviz. So, go for the translation of format. To do a translation from SDF to URDF watch this video.

So now I will assume that you have a robot model in URDF which includes some joints. In order to keep the discussion sound I have created a simple URDF for a two joints robot. That is the URDF.

<?xml version="1.0"?>
<robot name="simple_example">
  <link name="base_link">
    <inertial>
        <mass value="10" />
        <inertia ixx="0.4" ixy="0.0" ixz="0.0" iyy="0.4" iyz="0.0" izz="0.2"/>
    </inertial>
    <collision>
      <geometry>
        <cylinder radius="0.05" length="0.24" />
      </geometry>
        </collision>
    <visual>
      <geometry>
        <cylinder radius="0.05" length="0.24" />
      </geometry>
    </visual>
  </link>

  <link name="second_link">
    <inertial>
        <mass value="0.18" />
        <inertia ixx="0.0002835" ixy="0.0" ixz="0.0" iyy="0.0002835" iyz="0.0" izz="0.000324" />
    </inertial>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0" />
    <collision>
      <geometry>
        <box size="0.05 0.05 0.15" />
      </geometry>
    </collision>
    <visual>
      <geometry>
        <box size="0.05 0.05 0.15" />
      </geometry>
    </visual>
  </link>

  <joint name="base_to_second_joint" type="continuous">
    <parent link="base_link"/>
    <child link="second_link"/>
    <axis xyz="1 0 0"/>
    <origin xyz="0.0 0.0 0.2" rpy="0.0 0.0 0.0"/> 
  </joint>

  <!--                GAZEBO RELATED PART                             -->

  <!-- ROS Control plugin for Gazebo -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/simple_model</robotNamespace>
    </plugin>
  </gazebo>

  <!-- transmission -->
  <transmission name="base_to_second_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor1">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="base_to_second_joint">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
  </transmission>
</robot>`

Now that URDF has the key elements you must use in any kinematic chain to be able to control it using ROS. Basically you do the following:

  1. Add a link for each movable element of the robot
  2. For each link add a joint which describes the union between the different links, which type of union is, and how one element moves in relation to the other.
  3. To be able to control with ROS you need to add the gazebo_ros_control plugin
  4. Finally, you have to include a transmission part, that is the one that actually indicates the controller to be used and for which joint.

Once you have all that, you need to create a config file that must be loaded at launch time, which contains the configuration of the controller itself. Here it is an example for the previous URDF.

simple_model:
joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 20

base_to_second_joint_position_controller:
    type: effort_controllers/JointPositionController
    joint: base_to_second_joint
    pid: {p: 1.0, i: 1.0, d: 0.0}

Finally, you need to create a launch file that launches everything, loads the configuration and publishes the status in ROS. Here an example of such a file:

<launch>
<param name="robot_description" textfile="$(find simple_example_description)/urdf/robot.urdf" />
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" args="-urdf -model simple_model -param robot_description -y -6"/>
<!-- loads the controllers -->
<rosparam file="$(find simple_example_description)/config/config.yaml" command="load" />
<node name="controller_spawner" pkg="controller_manager" type="spawner" ns="/simple_model" args="base_to_second_joint_position_controller joint_state_controller --shutdown-timeout 3"/> 
<!-- converts joint states to TF transforms -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen">
    <remap from="/joint_states" to="/simple_model/joint_states" />
</node>
</launch>

(sorry for the bad formatting, but this editor for questions is really bad, and presents a lot of problems when copy pasting files to keep the indentation and format).

So, you organize all that info in a package and you are done! I have created this video that shows more details about this example. In the video you can see how this example is able to move by publishing in a control topic.

More suggested tutorials that you should study: