[ROS Q&A] 129 – Which ROS controllers required for PR2 ?

 

In this video we are going to see why a ROS developer can’t launch a PR2 Gazebo simulation, mainly because the ROS controllers are not found.

We will see which are the ROS packages which provide all the required controllers for the PR2. We will also see how you can apply this check to your own robot simulation, in case it complains about the controllers not being loaded.

This is a video trying to answer the following question posted at the ROS answers forum: https://answers.ros.org/question/294245/roslaunch-pr2_gazebo-pr2_empty_worldlaunch/

 

The following steps take throught the development as shown in the video

  • Open Robot Development Studio and create a new project (use a suitable name for project).
  • Launch the PR2 simulation by entering the following commands in a SHELL

    $ roslaunch pr2_gazebo pr2_empty_world.launch

It will take a while to load the simulation and we need to open a gazebo window (Tools>Gazebo) to see the visualization.

If the simulation doesn’t launch then we need to check for a few things

  • Controller manager : It is important that controller manager be running, we can check that with the following command

$ rosservice list| grep controller_manager

In the output of the above command, if you see enteries starting with /pr2_controller_manager/ that means everything is OK.

The different types of controllers available can be checked with command

$ rosservice call /pr2_controller_manager/list_controller_types

If you see controller types in the output of the above command then everything is OK.

We can check if these controllers are loaded or not using the following command

$ rosservice call /pr2_controller_manager/list_controllers

The above command should output various controllers and thier status.

Now the above case is for the working environment where all the required packages are installed. If some packages are missing then we should systematically look for packages and dependencies. The packages mostly fall into two categories: gazebo-ros controllers and robot specific controllers (pr2-controllers specifically for our case)

  • ros-control : To list the available ros-control packages use the following command

$ dpkg -l | grep ros-control

There are two important packages in this category ros-kinetic-gazebo-ros-control and ros-kinetic-ros-controllers

  • gazebo-ros : To list the available gazebo-ros related packages use the following command

$ dpkg -l | grep gazebo-ros

The output of the command should contain the following two names ros-kinetic-gezebo-ros and ros-kinetic-gezebo-ros-control (this one was seen in previous commands output also)

  • controller interfaces : To list the installed controller interface packages use the command

$ dpkg -l | grep controller-interface

The output of this command should show the following names ros-kinetic-controller-interface, ros-kinetic-controller-manager, ros-kinetic-controller-manager-msgs

  • controllers : In gazebo we use various controllers to actuate different types of joints. We need to check for the following ones
    • joint state controllers : The output of the following command should contain ros-kinetic-joint-state-controller

      $ dpkg -l | grep joint-state

    • joint trajectory controllers : The output of the following command should contain ros-kinetic-joint-trajectory-controller

      $ dpkg -l | grep joint-trajectory

    • effort controllers : The output of the following command should contain ros-kinetic-effort-controllers

      $ dpkg -l | grep effort-controllers

    • velocity controllers : The output of the following command should contain ros-kinetic-velocity-controllers

      $ dpkg -l | grep velocity-controllers

Finally we need to check for PR2 robot related controllers to be able to launch the simulation. Use the following commands

$ dpkg -l | grep pr2-controller $ dpkg -l | grep pr2-mechanism

We expect to see the following names in the output of those commands

  • ros-kinetic-pr2-controller-manager,
  • ros-kinetic-pr2-controller-interface,
  • ros-kinetic-pr2-controller-configuration-gazebo,
  • ros-kinetic-pr2-controllers-msgs,
  • ros-kinetic-pr2-mechanism-controllers,
  • ros-kinetic-pr2-mechanism-diagnostics,
  • ros-kinetic-pr2-mechanism-models,
  • ros-kinetic-pr2-mechanism-msgs

If we have all of these packages installed in our computer, we will be able to launch the pr2 robot. That’s all for this post. Thanks for watching the video, please leave your comments and subscribe the channel if you want to see more such videos.

RELATED LINKS AND RESOURCES

▸ Original question: https://answers.ros.org/question/294245/roslaunch-pr2_gazebo-pr2_empty_worldlaunch/
ROS Development Studio (RDS)
Robot Ignite Academy
ROS Control 101 online course

LIST OF ROS PACKAGES REQUIRED

1st- Gazebo – ROS controllers
* ros-kinetic-ros-control
* ros-kinetic-ros-controllers

* ros-kinetic-controller-interface
* ros-kinetic-controller-manager
* ros-kinetic-controller-manager-msgs

* ros-kinetic-gazebo-ros
* ros-kinetic-gazebo-ros-control

* ros-kinetic-joint-state-controller
* ros-kinetic-joint-trajectory-controller
* ros-kinetic-effort-controllers
* ros-kinetic-velocity-controllers

2nd- PR2 robot specific controllers

* ros-kinetic-pr2-controller-manager
* ros-kinetic-pr2-controllers
* ros-kinetic-pr2-controller-interface
* ros-kinetic-pr2-controller-configuration-gazebo

* ros-kinetic-pr2-mechanism-controllers
* ros-kinetic-pr2-mechanism
* ros-kinetic-pr2-mechanism-diagnostics
* ros-kinetic-pr2-mechanism-model


Feedback

Did you like this video? Do you have questions about what is explained? Whatever the case, please leave a comment on the comments section below, so we can interact and learn from each other.

If you want to learn about other ROS topics, please let us know on the comments area and we will do a video about it.

ros-control-course-banner

[ROS Projects] – My Robotic Manipulator – #Part 5 – ROS Controllers and XACRO

[ROS Projects] – My Robotic Manipulator – #Part 5 – ROS Controllers and XACRO

 

This is a series of posts. If you didn’t follow up, you can find the previous post here. In this 5th video of the robotic manipulator series, we will expand the ROS controllers to all joints of our robot using XACRO. At the end of the video we’ll have a full controlled robot through ROS topics. We are also going to use RQT Publisher and RQT Reconfigure to do some experiments with the robot.

Step 0. Create a project in ROS Development Studio(ROSDS)

ROSDS helps you follow our tutorial in a fast pace without dealing without setting up an environment locally. If you haven’t had an account yet, you can create a free account here. We’ll start the project for the previous video – manipulator_video_no4.

Step 1. Configure controller

In order to use controllers for our robot.  The first step is we need to add transmission definitions for each joint in the links_joints.xarco and add limitation for each joint in the mrm.xarco file. The most important thing is to add the gazebo plugin in the mrm.xarco file.

...  
<xacro:macro name="m_joint" params="name type axis_xyz origin_rpy origin_xyz parent child limit_e limit_l limit_u limit_v">
    <joint name="${name}" type="${type}">
      <axis xyz="${axis_xyz}" />
      <limit effort="${limit_e}" lower="${limit_l}" upper="${limit_u}" velocity="${limit_v}" />
      <origin rpy="${origin_rpy}" xyz="${origin_xyz}" />
      <parent link="${parent}" />
      <child link="${child}" />
    </joint>
    <transmission name="trans_${name}">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="${name}">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      </joint>
      <actuator name="motor_${name}">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
    </transmission>
  </xacro:macro>
...
...  
<m_joint name="${link_01_name}__${link_02_name}" type="revolute"
           axis_xyz="0 1 0"
           origin_rpy="0 0 0" origin_xyz="0 0 0.4"
           parent="link_01" child="link_02"
           limit_e="1000" limit_l="0" limit_u="0.5" limit_v="0.5" />
           
  <m_link_cylinder name="${link_02_name}"
              origin_rpy="0 0 0" origin_xyz="0 0 0.4"
              mass="57.906"
              ixx="12.679" ixy="0" ixz="0"
              iyy="12.679" iyz="0"
              izz="0.651"
              radius="0.15" length="0.8" />
              
  <m_joint name="${link_02_name}__${link_03_name}" type="revolute"
           axis_xyz="0 1 0"
           origin_rpy="0 0 0" origin_xyz="0 0 0.8"
           parent="link_02" child="link_03"
           limit_e="1000" limit_l="0" limit_u="0.75" limit_v="0.5" />
           
  <m_link_cylinder name="${link_03_name}"
              origin_rpy="0 0 0" origin_xyz="0 0 0.4"
              mass="57.906"
              ixx="12.679" ixy="0" ixz="0"
              iyy="12.679" iyz="0"
              izz="0.651"
              radius="0.15" length="0.8" />
              
  <m_joint name="${link_03_name}__${link_04_name}" type="revolute"
           axis_xyz="0 1 0"
           origin_rpy="0 0 0" origin_xyz="0 0 0.8"
           parent="link_03" child="link_04"
           limit_e="1000" limit_l="0" limit_u="0.75" limit_v="0.5" />
           
  <m_link_cylinder name="${link_04_name}"
              origin_rpy="0 0 0" origin_xyz="0 0 0.4"
              mass="57.906"
              ixx="12.679" ixy="0" ixz="0"
              iyy="12.679" iyz="0"
              izz="0.651"
              radius="0.15" length="0.8" />
              
  <m_joint name="${link_04_name}__${link_05_name}" type="revolute"
           axis_xyz="0 0 1"
           origin_rpy="0 0 0" origin_xyz="0 0 0.8"
           parent="link_04" child="link_05"
           limit_e="1000" limit_l="-3.14" limit_u="3.14" limit_v="0.5" />

  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    </plugin>
  </gazebo>
...

Then we create a file called joints.yaml file in the config folder which defines the parameters for controllers.

# Publish all joint states -----------------------------------
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50

# Position Controllers ---------------------------------------
joint1_position_controller:
  type: effort_controllers/JointPositionController
  joint: base_link__link_01
  pid: {p: 2000.0, i: 100, d: 500.0}
joint2_position_controller:
  type: effort_controllers/JointPositionController
  joint: link_01__link_02
  pid: {p: 50000.0, i: 100, d: 2000.0}
joint3_position_controller:
  type: effort_controllers/JointPositionController
  joint: link_02__link_03
  pid: {p: 20000.0, i: 50, d: 1000.0}
joint4_position_controller:
  type: effort_controllers/JointPositionController
  joint: link_03__link_04
  pid: {p: 2000.0, i: 50, d: 200.0}
joint5_position_controller:
  type: effort_controllers/JointPositionController
  joint: link_04__link_05
  pid: {p: 700.0, i: 50, d: 70.0}

This file defines the type of the controller and the pid parameters for each controller.

The last step is to modify the launch file in order to launch the gazebo plugin for the controller in the spawn.launch file.

<?xml version="1.0" encoding="UTF-8"?>
<launch>
    
    <group ns="/mrm">
        
        <!-- Robot model -->
        <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mrm_description)/urdf/mrm.xacro'" />
        <arg name="x" default="0"/>
        <arg name="y" default="0"/>
        <arg name="z" default="0.5"/>
        
        <!-- Spawn the robot model -->
        <node name="mybot_spawn" pkg="gazebo_ros" type="spawn_model" output="screen"
              args="-urdf -param robot_description -model mrm -x $(arg x) -y $(arg y) -z $(arg z)" />
      
        <!-- Load controllers -->
        <rosparam command="load" file="$(find mrm_description)/config/joints.yaml" />
        
        <!-- Controllers -->
        <node name="controller_spawner" pkg="controller_manager" type="spawner"
            respawn="false" output="screen" ns="/mrm"
            args="--namespace=/mrm
            joint_state_controller
            joint1_position_controller
            joint2_position_controller
            joint3_position_controller
            joint4_position_controller
            joint5_position_controller
            --timeout 60">
        </node>
        
        <!-- rqt -->
        <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" />
        <node name="rqt_publisher" pkg="rqt_publisher" type="rqt_publisher" />
    
    </group>
          
</launch>

Then you can open an empty simulation from Simulations->Empty and launch it with the following command

roslaunch mrm_description spawn.launch

After you see the robot appear in the simulation, you can open the graphical tool from Tools->graphical tool

By using the rqt tool, you can send parameter to topics control the joint movement(e.g mrm/joint4_position_controller/command/dara) to move the robot.

The PID parameters are not well tuned yet, we’ll do it in the next post. But the robot is moving(with some oscillation) now.

Want to learn more?

If you want to learn more about how to create manipulator simulation in ROS, please check our Robot Creation with URDF ROS and ROS Control 101 course.

 

Edit by: Tony Huang

 

RELATED LINKS & RESOURCES

Did you like the video? If you did please give us a thumbs up and remember to subscribe to our channel and press the bell for a new video every day. Either you like it or not, please share your thoughts and questions in the comments area. See you!

[irp posts=”9549″ name=”My Robotic Manipulator – #Part 4 – ROS + URDF/Transmission + Gazebo Controllers”]

[ROS Q&A] 126 – How to configure the differential drive ROS controller

[ROS Q&A] 126 – How to configure the differential drive ROS controller

 

In this video we are going to see how to configure the differential drive ROS controller for a wheeled robot using a Gazebo simulation.

This is a video trying to answer the question of Jaime posted at the ROS answers forum about how he cannot make the controller work, and receiving the error:

Controller Spawner couldn’t find the expected controller_manager ROS interface

 

Step1. Create Project

Let’s start with creating a new project in ROS development studio.

Notice: If you haven’t had an account yet. You can register one here for free.

Step2. Spawn a robot

As an example, we’ll use a self-build two-wheel differential drive robot.

You can test the code with your own robot with differential drive configuration.

Step3. Add the controller configuration file for your robot

Put the configuration file(e.g. the my_diff_drive.yaml file shows here) under the config folder, your source tree may look like this.

Let’s start by pasting the whole code from the question into the my_diff_drive.yaml file.

mobile_base_controller:
  type        : "diff_drive_controller/DiffDriveController"
  left_wheel  : 'wheel_left_joint'
  right_wheel : 'wheel_right_joint'
  publish_rate: 50.0               # default: 50
  pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
  twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]

  # Wheel separation and diameter. These are both optional.
  # diff_drive_controller will attempt to read either one or both from the
  # URDF if not specified as a parameter
  wheel_separation : 1.0
  wheel_radius : 0.3

  # Wheel separation and radius multipliers
  wheel_separation_multiplier: 1.0 # default: 1.0
  wheel_radius_multiplier    : 1.0 # default: 1.0

  # Velocity commands timeout [s], default 0.5
  cmd_vel_timeout: 0.25

  # Base frame_id
  base_frame_id: base_footprint #default: base_link

  # Velocity and acceleration limits
  # Whenever a min_* is unspecified, default to -max_*
  linear:
    x:
      has_velocity_limits    : true
      max_velocity           : 1.0  # m/s
      min_velocity           : -0.5 # m/s
      has_acceleration_limits: true
      max_acceleration       : 0.8  # m/s^2
      min_acceleration       : -0.4 # m/s^2
      has_jerk_limits        : true
      max_jerk               : 5.0  # m/s^3
  angular:
    z:
      has_velocity_limits    : true
      max_velocity           : 1.7  # rad/s
      has_acceleration_limits: true
      max_acceleration       : 1.5  # rad/s^2
      has_jerk_limits        : true
      max_jerk               : 2.5  # rad/s^3

Step4. Create Launch file

For our case, the launch file should look something similar like this.

<?xml version="1.0" encoding="UTF-8"?>
<launch>
    <param name="robot_description" command="cat '$(find two_wheels_description)/urdf/two_wheels.urdf'" />
    
    <arg name="x" default="-2"/>
    <arg name="y" default="0"/>
    <arg name="z" default="0.1"/>
    
    <node name="mybot_spawn" pkg="gazebo_ros" type="spawn_model" output="screen"
          args="-urdf -param robot_description -model mybot -x $(arg x) -y $(arg y) -z $(arg z)" />
          
    <rosparam file="$(find two_wheels_description)/config/my_diff_drive.yaml" command="load" />
    
    <node name="SARA_controller manager" pkg="controller_manager" type="spawner"
          respawn="false" output="screen" args="mobile_base_controller" />
          
          
</launch>

NOTICE:

Two errors are the spot when we are doing this. 

  1. The args for the controller should have the same name in the .yaml file which is “mobile_base_controller”
  2. According to the .yaml file, there is no namespace /robot here, so we don’t need to add this to the controller node.

Things to make sure:

  1. The left wheel and right wheel in the .yaml file should be the same as your robot’s URDF definition.
  2. The gazebo controller should be added to the URDF definition as well as the transmission tag which will be used for the gazebo controller. In our case, we add the following code in the .urdf to add gazebo control in it.
...

<transmission name="left_wheel_transmission">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="wheel_left_joint">
    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>  
  </joint>
  <actuator name="left_wheel_actuator">
    <mechanicalReduction>7</mechanicalReduction>
    <hardwareInterface>VelocityJointInterface</hardwareInterface>
  </actuator>
</transmission>

<transmission name="right_wheel_transmission">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="wheel_right_joint">
    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>  
  </joint>
  <actuator name="right_wheel_actuator">
    <mechanicalReduction>7</mechanicalReduction>
    <hardwareInterface>VelocityJointInterface</hardwareInterface>
  </actuator>
</transmission>

...

<gazebo>
  <plugin name="gazebo_ros_control" filename="libgazebo_ros_contol.so">
  </plugin>
</gazebo>

 

Step4. Lanch again

It’s better to compile again and run:

cd ~/simulation_ws

catkin_make

source devel/setup.bash

roslaunch two_wheel_drscription question.launch

Then you can use

rostopic list

If you see the following topics, then your controller is up and run correctly.

Takeaway today:

  1. The arg name of the controller node should be the same as in the controller configuration file.
  2. Don’t specify robot namespace if you are not using it.
  3. The joint name in the controller configuration file should be the same as the name in urdf
  4. The gazebo_ros_control plugin should also be added to the urdf file.
  5. Remember to compile again before you run.

If you want to learn more about ROS control and how to build a two-wheel robot in ROS from scratch, please visit Robot Ignite Academy for more information.

 

// RELATED LINKS

▸ Original question: https://answers.ros.org/question/289561/help-to-run-diff_drive_controller/
▸ Robot Ignite Academy: https://goo.gl/pF81sN
▸ ROS Basics in 5 days (Python): https://goo.gl/HGPP1M
▸ ROS Basics in 5 days (C++): https://goo.gl/evXQCA
▸ ROS Development Studio: https://goo.gl/FzHTQU

ROS Control Tutorials (using Gazebo robot simulation)

ROS Control Tutorials (using Gazebo robot simulation)

ROS Control Tutorials #Unit 1: Introduction to ROS Control

In this short video you’ll be introduced to ROS Control, an aspect of ROS that enables robots to move and get things done. We’ll be using Gazebo robot simulation. Follow along!

[irp posts=”8194″ name=”All about Gazebo ROS (Gazebo 9)”]

ROS Control Tutorials #Unit 2: Basic Concepts

In this video we look at some basic theoretical concepts regarding ROS control that we will use throughout the rest of the course.

 

ROS Control Tutorials #Unit 3 : How To Configure and Launch the Controllers

This video explains how to configure and launch the controllers after configuring transmissions and the Gazebo plugin in the robot URDF files. It shows: 1. how to create a package to launch the controllers, and 2. how to write commands to test the controllers.

Slide used in the video: https://goo.gl/6aNeSC

 


ros_control_tutorials_course_banner

 

[ROS Q&A] 112 – How to tune a PID with ROS Control

 

We show you a manual method to tune a PID for a robot that uses ROS Control to control its joints with a position controller. This method is very artisan but it is enough for most of the cases.

We do not teach how ROS Control works in this video. We assume you already know how to configure a joint to work with the ROS controllers.

Step 1. Create a project in ROS Development Studio(ROSDS)

ROSDS helps you follow our tutorial in a fast pace without dealing without setting up an environment locally. If you haven’t had an account yet, you can create a free account here. Let’s call the testing.

Step 2. Tune PID

At first, let’s launch the rrbot simulation from Simulations->RRBot. To control the robot, we also need to launch the control with

roslaunch rrbot_control rrbot_control.launch

You will have to tune the pid value in the rrbot_contorl.yaml file in the rrbot_control package under the config.

We can also use rqt_gui to help us tune the parameter.

rosrun rqt_gui rqt_gui

Then open the Tools->graphical tool.

In the gui, let’s send command to the rrbot/joint1_position_controller/command topic to control the robot. The message type is automatically selected to std_msgs/Float64. We’ll change the frequency to 50 Hz, then click +.

Then click the + on the left to unfold the topic and change the expression to sin(i/50)*0.5. Check the box on the left side to start publishing into the topic.

You should see the robot starts to swing it’s arm now.

It’s also possible to plot the message from Plugins->Visualization->Plot

Select the correct topic /rrbot/joint1_position_controller/state/process_value/data then click +.  You should see the data sent.

How about the actual value? It’s in the topic /rrbot/joint1_position_controller/state/process_value.

You’ll see that the actual state of the robot didn’t follow the command sent to the robot. That’s the reason why we need to tune the pid value.

Let’s open another tool to help us tune the value at the same time. Go to Plugins->Configuration->Dynamic Reconfigure.

You can change all the pid value in the plugin.

To tune the pid:

  1.  We start by increasing the p value to make the controller output more to catch the command.
  2. Increase the d value a bit to make the movement smoother.

You should see the state can synchronize with the command now. You can change the command to see if the controller can follow the command nicely. Then you can save the value in the rrbot_controller.yaml file.

Remember, you have to launch the controller again to apply the new value.

 

If you need to understand better ROS Control, please take the following course for fast understanding:

* ROS Control 101 (online course with tons of practice): https://goo.gl/6hEjyw

* Learn ROS fast in this online academy: https://goo.gl/KiCfXx

* Practice with the second joint, by using the ROS Development Studio: https://goo.gl/pBk44x

 

 

Edit by: Tony Huang

Pin It on Pinterest