Tutorial 10: Robot Control

Objective: This tutorial session is devoted to understand the ros_control framework used to implement and manage robot controllers for real robots and in simulation within gazebo.

Contents

After this session you will be able to:

  • Understand the structure of the ros_control framework.

  • Which are the available controllers and how are they managed.

  • Understand how to interface with real hardware using the hardware abstraction layer.

  • Understand how ros_control is used within gazebo simulation environment.

  • Use ros_control to define robot trajectories.

Provided packages:

This tutorial will use the following packages:

Package actions_tutorial
Package gazebo_control_tutorial_traj
Package mastering_ros_robot_description_pkg
Package rrbot_control
Package rrbot_description
Package rrbot_gazebo
Package seven_dof_arm_gazebo_simple
Package staubli
Package staubli_experimental
Package fmauch_universal_robot

Prepare the catkin workspace to work with them:

  1. If you have not yet done so, follow the steps in Exercise 0 to:

  • clone in your ~/git-projects folder the meta-repository containing all the required packages,

  • create the catkin workspace folders (use catkin_ws10 for the current tutorial).

  1. Follow the procedure shown in Procedure to prepare the exercises to import in the intro2ros/2023/teamXX/EXERCISE7 subgroup of your GitLab account the package:

  1. In your git-projects folder clone the solution_exercise7 project of your GitLab account inside an exercise7 subfolder (change XX by your team number). You can use https:

$ git clone https://gitlab.com/intro2ros/2023/teamXX/exercise7/solution_exercise7.git exercise7/solution_exercise7

or ssh:

$ git clone git@gitlab.com:intro2ros/2023/teamXX/exercise7/solution_exercise7.git exercise7/solution_exercise7
  1. In your catkin_ws10/src folder create a symbolic link to the packages to be used:

$ ln -s ~/git-projects/all_introduction_to_ros_packages/rrbot_description
$ ln -s ~/git-projects/all_introduction_to_ros_packages/rrbot_gazebo
$ ln -s ~/git-projects/all_introduction_to_ros_packages/rrbot_control
$ ln -s ~/git-projects/all_introduction_to_ros_packages/mastering_ros_robot_description_pkg
$ ln -s ~/git-projects/all_introduction_to_ros_packages/seven_dof_arm_gazebo_simple
$ ln -s ~/git-projects/all_introduction_to_ros_packages/gazebo_control_tutorial_traj
$ ln -s ~/git-projects/all_introduction_to_ros_packages/actions_tutorial
$ ln -s ~/git-projects/all_introduction_to_ros_packages/staubli
$ ln -s ~/git-projects/all_introduction_to_ros_packages/staubli_experimental
$ ln -s ~/git-projects/all_introduction_to_ros_packages/fmauch_universal_robot
  1. Edit the build.sh file of the solution_exercise7 package to add your credencials (token_user and token_code) and push the change.

References:

This tutorial is partially based on the ROS Control tutorial from gazebosim.org, the ROS control documentation from ROS wiki, and on chapter 3 of the book Mastering ROS for Robotics Programming from Lentin Joseph.

Complementary resources:

ros_control overview

The ros_control framework provides the capability to implement and manage robot controllers, that mainly consists of a feedback mechanism, most probably a PID loop, which can receive a setpoint, and control the output, typically effort, using the feedback from the actuators. The primary motivation of ros_control is the lack of realtime-safe communication layer in ROS.

The framework implements abstractions on hardware interfaces with minimal assumptions on hardware or operating system. The output is applied to the real robot or to its simulation in Gazebo by using a simple Gazebo plugin adapter.

The following figure shows the interconnection of the ROS controller, robot hardware interface, and simulator/real hardware:

../_images/overview.png ../_images/controllersAndInterfaces.png

More details on all the components of ros_control are shown in the next figure. They can be grouped in two main blocks:

  • The controllers and the control manager (the blue and yellow blocks; they will be detailed in Section Controllers):

    Several controllers may be used (some are already available) and are managed by the Control Manager that is responsible of loading, unloading and switching between them.

  • The hardware abstraction layer (the gray and orange blocks; they will be detailed in Section Hardware Abstraction Layer).

    The ROS controllers do not directly communicate with the hardware, but do it through the Hardware Abstraction Layer, enabling controllers to be hardware-agnostic. It is composed of:

    • the ros_control interfaces found in the figure within the Hardware Resource Interface Layer

    • the hardware interfaces that encapsulate the hardware drivers. They are modeled with classes that inherit from the hardware_interface::RobotHW class (this class also acts as resource manager by performing resource conflict checking for a given set of controllers, maintaining a map of hardware interfaces). In the case of simulation, the Gazebo plugin version hardware_interface::RobotHWSim is used.

../_images/ros_control_overview.png

Controllers

Available controllers

The main ROS controllers are grouped according to the commands get passed to your hardware/simulator:

  • position_controllers: Used for position-controlled joints (position commands are used to control joint positions).

    joint_position_controller: just transfers the position input as a position command to the hardware interface (using a simple forward_command_controller).
    joint_group_position_controller: Set multiple joint positions at once.
  • velocity_controllers: Used for velocity-controlled joints (velocity commands are used to control joint positions or velocities).

    joint_velocity_controller: just transfers the velocity input as a velocity command to the hardware interface (using a simple forward_command_controller).
    joint_group_velocity_controller: Set multiple joint velocities at once.
    joint_position_controller: the position error is mapped to a velocity command using a PID controller.
  • effort_controllers: Used for effort-controlled joints (efforts commands are used to control joint positions, velocities or efforts).

    joint_effort_controller: just transfers the effort input as an effort command to the hardware interface (using a simple forward_command_controller).
    joint_group_effort_controller: Set multiple joint efforts at once.
    joint_position_controller: the position error is mapped to an effort command using a PID controller.
    joint_velocity_controller: the velocity error is mapped to an effort command using a PID controller.
  • joint_trajectory_controllers: Used to control the execution of joint-space trajectories on a group of joints, specified as a set of waypoints to be reached at specific time instants. They use a linear, cubic or quintic spline interpolator depending on how the waypoints are specified (i.e. as joint positions, and optionally velocities and accelerations), and are implemented to work with multiple hardware interface types.

    position_controller: just transfers the position inputs as a position commands to the hardware interfaces.
    velocity_controller: the position+velocity trajectory following error is mapped to velocity commands using a PID controller.
    effort_controller: the position+velocity trajectory following error is mapped to effort commands using a PID controller.

Also, a “controller” is defined to publish joint states:

Other available controllers can be found here.

The controller manager

The controller_manager provides the infrastructure to interact with controllers, i.e. load, unload, start and stop controllers.

../_images/controller_manager.png

This interaction can be done:

  • Using the command line tools:

$ rosrun controller_manager controller_manager <command> <controller_name>

where the available commands are load, unload, start, stop, spawn (load+start), kill (stop+unload).

Also, several controllers can be loaded and started at once with the spawner tool:

$ rosrun controller_manager spawner [--stopped] name1 name2 name3
  • Using launch files (which is the most practical and common way):

    ../_images/controller_manager_launch.png
  • Using rqt:

$ rosrun rqt_controller_manager rqt_controller_manager
  • Using service calls to interact with controllers from another ROS node, e.g.:

    • controller_manager/load_controller has as request the name of the controller to be loaded and returns success or failure.

    • controller_manager/switch_controller has as request a list (which can be empty) of controller names to start, a list (which can be empty) of controller names to stop, and the switching policy.

More information can be found in the controller manager wiki page.

Configuring and launching controllers

Controllers are usually defined with yaml files (stored in a config folder), like those shown here.

Another example is shown in the following figure, that corresponds to the rrbot_control.yaml configuration file of the rrbot_control package where the controllers for the rrbot robot are defined. In this case there is:

  • the joint state controller to publish the joint states of the arm (with a publishing rate set to 50 Hz),

  • two joint position controllers to move each of the two joints upon receiving a goal position.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
rrbot:
 # 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: joint1
   pid: {p: 100.0, i: 0.01, d: 10.0}
 joint2_position_controller:
   type: effort_controllers/JointPositionController
   joint: joint2
   pid: {p: 100.0, i: 0.01, d: 10.0}

We can see that all the controllers are inside the namespace rrbot.

This configuration file can be loaded in a launch file, prior to the spawning of the controller:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
<launch>
 <!-- Load joint controller configurations from YAML file to parameter server -->
 <rosparam file="$(find rrbot_control)/config/rrbot_control.yaml" command="load"/>

 <!-- load the controllers -->
 <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
       output="screen" ns="/rrbot" args="joint_state_controller
                                         joint1_position_controller
                                         joint2_position_controller"/>

 <!-- convert joint states to TF transforms for rviz, etc -->
 <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
       respawn="false" output="screen">
   <remap from="/joint_states" to="/rrbot/joint_states" />
 </node>
</launch>

If order to simulate the robot in Gazebo with those controllers, the corresponding plugin has to be added to the URDF model, as detailed in Section Preparing Gazebo for ros_control.

Important

Note that the controllers have to be spawned within the same namespace where the controls have been defined, rrbot in this case, using the ns= argument in the controller_spawner node tag. Also, the gazebo plugin will need to be within this namespace.

Hardware Abstraction Layer

ros_control interfaces

The connection of the controllers with the hardware interface (that encapsulate the hardware) is done through the ros_control interfaces. Some of the commonly used are:

  • Joint Command Interfaces:

    • EffortJointInterface: to send the effort command.

    • VelocityJointInterface: to send the velocity command.

    • PositionJointInterface: to send the position command.

  • Joint State Interfaces: to retrieve the joint states from the actuators encoder.

Hardware interface

To control a given robot using ros_control, a class derived from the hardware_interface::RobotHW class should be implemented. This class should support one or more of the standard interfaces like e.g. the EfforJointInterface or VelocityJointInterface:

../_images/hw_interface_2.png

For instance, the following code shows the class implemented for a robot with two position-controlled joints. It uses the JointPositionInterface and the JointStateInterface:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>

class MyRobot : public hardware_interface::RobotHW
{
public:
  MyRobot()
 {
   // connect and register the joint state interface
   hardware_interface::JointStateHandle state_handle_a("A", &pos[0], &vel[0], &eff[0]);
   jnt_state_interface.registerHandle(state_handle_a);

   hardware_interface::JointStateHandle state_handle_b("B", &pos[1], &vel[1], &eff[1]);
   jnt_state_interface.registerHandle(state_handle_b);

   registerInterface(&jnt_state_interface);

   // connect and register the joint position interface
   hardware_interface::JointHandle pos_handle_a(jnt_state_interface.getHandle("A"), &cmd[0]);
   jnt_pos_interface.registerHandle(pos_handle_a);

   hardware_interface::JointHandle pos_handle_b(jnt_state_interface.getHandle("B"), &cmd[1]);
   jnt_pos_interface.registerHandle(pos_handle_b);

   registerInterface(&jnt_pos_interface);
  }

private:
  hardware_interface::JointStateInterface jnt_state_interface;
  hardware_interface::PositionJointInterface jnt_pos_interface;
  double cmd[2];
  double pos[2];
  double vel[2];
  double eff[2];
};

In this example, the controller manager (and the controllers inside the controller manager) will get read access to the joint state of the robot (pos, vel and eff variables) through the hardware_interface::JointStateInterface and write access to the command to be sent to the robot (the cmd variable) through the hardware_interface::PositionJointInterface. Two functions should be added to this class, namely read() and write(), to fill the pos, vel and eff with the latest available values from the robot and to run the command available at cmd on the robot. These functions will then be called in the control loop before and after the computation of the control command as shown next:

../_images/control_loop_code.png ../_images/control_loop.png

More information can be found in the ros_controls git repository.

Optionally, the hardware interface can be enhanced by considering joint limits and by modeling the joint transmission dynamics, as explained below.

Joint Limits

The joint_limits_interface package contains data structures for representing joint limits, methods to populate them through URDF or yaml files, and methods to enforce these limits. An example of joint limits specification in URDF and YAML formats is:

../_images/joint_limits_urdf.png ../_images/joint_limits_yaml.png

An example of how to read these values from the URDF and populate the joint_limits_interface data structures can be found in the GitHub wiki.

Transmissions

Transmission interfaces implement mechanical transmissions, like a mechanical reducer with a given ratio n, and therefore maps effort/flow variables to output effort/flow variables while preserving power.

Transmissions are defined in the URDF robot file as detailed in the ROS wiki urdf transmission page, e.g.:

../_images/urdf_transmissions.png

The complete control loop when considering joint limits and the existence of transmissions is shown in the next figure:

../_images/complete_control_loop.png

Preparing Gazebo for ros_control

To use ros_control with in Gazebo, the URDF model of the robot has to include two additional elements: transmissions and the plugin.

Adding transmission tags

The <transmission> element has to be defined to link actuators to joints. The only important information in these transmission tags are:

  • <joint name=””>: the name must correspond to a joint

  • <type>: the type of transmission. Currently only transmission_interface/SimpleTransmission is implemented.

  • <hardwareInterface>: within both the <joint> and <actuator> tags, this tells the gazebo_ros_control plugin what hardware interface to load (position, velocity or effort interfaces). Currently only effort interfaces are implemented.

As an example, for joint1 in the rrbot the following is defined:

../_images/rrbot_transmission.png

Adding the plugin

A Gazebo plugin needs to be added to the URDF to actually parse the transmission tags and load the appropriate hardware interfaces and controller manager.

../_images/control_gazebo_plugin.png

The gazebo_ros_control tag has the following optional child elements:

  • <robotNamespace>: The ROS namespace to be used for this instance of the plugin (must be the same as the namespace used for defining and spawing the controllers, as detailed in Section Configuring and launching controllers).

  • <controlPeriod>: The period of the controller update (in seconds), defaults to Gazebo’s period.

  • <robotParam>: The location of the robot_description (URDF) on the parameter server, defaults to ‘/robot_description’.

  • <robotSimType>: The pluginlib name of a custom robot sim interface to be used, defaults to ‘DefaultRobotHWSim’ that provides the following ros_control interfaces:

    • hardware_interface::JointStateInterface

    • hardware_interface::EffortJointInterface

    • hardware_interface::VelocityJointInterface - not fully implemented

Adding pid_gains

When initializing RobotHWSim, the robot_ros_control tries to load the pid_gains; if not found the joints will be controlled with gazebo methods. When using the PositionJointInterface, however, an error may appear (even though it may still work correctly), e.g.:

[ERROR] [1581587647.510556781, 0.391000000]: No p gain specified for pid. Namespace: /seven_dof_arm/gazebo_ros_control/pid_gains/shoulder_pan_joint

To fix this issue, PID controller gains can be specified in a yaml file, like e.g.:

seven_dof_arm:

 #http://answers.gazebosim.org/question/5468/gazebo_ros_pkgs-no-p-gain-specified-for-pid-in-positionjointinterface/
 gazebo_ros_control:
   pid_gains:
     shoulder_pan_joint: {p: 100.0, i: 0.01, d: 10.0}
     shoulder_pitch_joint: {p: 100.0, i: 0.01, d: 10.0}
     elbow_pitch_joint: {p: 100.0, i: 0.01, d: 10.0}
     wrist_roll_joint: {p: 100.0, i: 0.01, d: 10.0}
     wrist_pitch_joint: {p: 100.0, i: 0.01, d: 10.0}
     gripper_roll_joint: {p: 100.0, i: 0.01, d: 10.0}
     finger_joint1: {p: 100.0, i: 0.01, d: 10.0}
     finger_joint2: {p: 100.0, i: 0.01, d: 10.0}

and loaded with the launch file:

<rosparam file="$(find gazebo_control_tutorial)/config/gazebo_ros_control_params.yaml" command="load"/>

However, the use of the above file may degrade the behavior if the parameters are not correctly tunned. Do not load it if not sure.

Working with ros_control

Using topics

Advertising joint values: The joint position controllers of the rrbot robot of Section Configuring and launching controllers can be tested by advertising the desired joint values:

$ catkin build rrbot_description
$ catkin build rrbot_gazebo
$ catkin build rrbot_control
$ source devel/setup.bash
$ roslaunch rrbot_gazebo rrbot_world.launch
$ roslaunch rrbot_control rrbot_control.launch
$ rostopic pub -1 /rrbot/joint1_position_controller/command std_msgs/Float64 "data: 1.5"
$ rostopic pub -1 /rrbot/joint2_position_controller/command std_msgs/Float64 "data: 1.0"
../_images/rrbot_control_gazebo_example.png

The following example of the gazebo_control_tutorial package moves the seven_dof_arm robot along a sinusoidal trajectory by advertising the desired joint values. The task can be monitorized using rviz. To run the example, first one controller of type position_controllers/JointPositionController per joint is spawned in the launch file gazebo_control_tutorial aruco_seven_dof_arm_with_rgbd_world_position_controllers.launch:

$ catkin build gazebo_control_tutorial
$ source devel/setup.bash
$ roslaunch gazebo_control_tutorial aruco_seven_dof_arm_with_rgbd_world_position_controllers.launch
$ rosrun gazebo_control_tutorial pubcontrols
$ roslaunch gazebo_control_tutorial kinect_arm_aruco_cube_rviz_c.launch

Exercise

Take a look at the file pubcontrols.cpp of the gazebo_control_tutorial package.

Advertising a trajectory: A better alternative is to use the trajectory_msgs/JointTrajectory Message, as done in the pubcontrols_traj.cpp file.

For this, the arm_joint_trajectory_controller, which is a controller of type position_controllers/JointTrajectoryController, needs to be first spawned. This is done in the launch file gazebo_control_tutorial aruco_seven_dof_arm_world_trajectory_controller.launch.

Kill the nodes and try the pubcontrols_traj:

$ roslaunch gazebo_control_tutorial aruco_seven_dof_arm_world_trajectory_controller.launch
$ rosrun gazebo_control_tutorial pubcontrols_traj
$ roslaunch gazebo_control_tutorial kinect_arm_aruco_cube_rviz_c.launch

Exercise

Take a look at the file pubcontrols_traj.cpp of the gazebo_control_tutorial package.

Using actions

Despite the possibility of sending control by advertising their values as done before, the correct way to control the robot joints is using actions, as we introduced in Tutorial 9, where the FollowJointTrajectory action was used in the actions_tutorial package to define a trajectory (as well as the tolerance parameters) for the pan-tilt structure.

That is, a FollowJointTrajectory action client (called robotClient) was created in the pantilt_follow_traj.cpp file using the SimpleActionClient template class instantiated for the control_msgs::FollowJointTrajectory as follows:

actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> robotClient("/pan_tilt/arm_joint_trajectory_controller/follow_joint_trajectory");

This action client sent the trajectory through a control_msgs::FollowJointTrajectoryGoal to the FollowJointTrajectory action server, that was available because a controller has been defined in a yaml configuration file and spawned in a launch file:

  • The yaml configuration file is config/pantilt_joint_position_full.yaml:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
pan_tilt:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50
  # Position Controllers ---------------------------------------
  pan_position_controller:
    type: position_controllers/JointPositionController
    joint: pan_joint
  tilt_position_controller:
    type: position_controllers/JointPositionController
    joint: tilt_joint

# Trajectory Controller ---------------------------------------
  arm_joint_trajectory_controller:
    type: position_controllers/JointTrajectoryController
    joints:
      - pan_joint
      - tilt_joint

The name of the trajectory controller is arm_joint_trajectory_controller and it is defined within the namespace pan_tilt (that is why the FollowJointTrajectory action client is initialized with the name "/pan_tilt/arm_joint_trajectory_controller/follow_joint_trajectory").

  • The launch file is pantilt_joint_trajectory.launch. Here the arm_joint_trajectory_controller is spawned together with the joint_state_controller (note that the spawning is done within the namespace used ns="/pan_tilt"):

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
<?xml version="1.0" ?>
<launch>
 <!-- these are the arguments you can pass this launch file, for example paused:=true -->
 <!-- HERE GUI IS SET TO FALSE -->
 <arg name="paused" default="false"/>
 <arg name="use_sim_time" default="true"/>
 <arg name="gui" default="false"/>
 <arg name="headless" default="false"/>
 <arg name="debug" default="false"/>

 <!-- We resume the logic in empty_world.launch -->
 <include file="$(find gazebo_ros)/launch/empty_world.launch">
   <arg name="debug" value="$(arg debug)" />
   <arg name="gui" value="$(arg gui)" />
   <arg name="paused" value="$(arg paused)"/>
   <arg name="use_sim_time" value="$(arg use_sim_time)"/>
   <arg name="headless" value="$(arg headless)"/>
 </include>

 <!-- Load the URDF into the ROS Parameter Server -->
 <param name="robot_description" command="$(find xacro)/xacro $(find mastering_ros_robot_description_pkg)/urdf/pan_tilt_complete.xacro" />

 <!-- 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="screen"
       args="-urdf -model pan_tilt -param robot_description -z 0.1 "/> <!-- -J tilt_joint -4.5"/-->

 <!-- Load joint controller configurations from YAML file to parameter server -->
 <rosparam file="$(find actions_tutorial)/config/pantilt_joint_position_full.yaml" command="load"/>

 <!-- load the controllers -->
 <node name="trajectory_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
     output="screen" ns="/pan_tilt" args="arm_joint_trajectory_controller"/>

 <node name="joint_state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
         output="screen" ns="/pan_tilt" args="joint_state_controller" />

 <!-- convert joint states to TF transforms for rviz, etc -->
 <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen">
       <remap from="/joint_states" to="/pan_tilt/joint_states" />
 </node>


 <rosparam file="$(find actions_tutorial)/config/gazebo_ros_control_params.yaml" command="load"/>

 <!-- Starting action client to send trajectories to the robot -->
 <node
   name="pantilt_follow_traj"
   pkg="actions_tutorial"
   type="pantilt_follow_traj"
   output="screen"
 />

 <!-- Launch visualization in rviz -->
 <node unless="$(arg gui)"
   name="rviz"
   pkg="rviz"
   type="rviz"
   args="-d $(find actions_tutorial)/config/pan_tilt.rviz"
   required="true"
 />
</launch>

Run again that example but visualizing it with gazebo instead of rviz (setting the argument gui to true):

$ roslaunch actions_tutorial pantilt_trajectory_controller.launch gui:=true

Exercise

Revise the FollowJointTrajectory action explained in Tutorial 9 and take again a close look at the file pantilt_follow_traj.cpp of the actions_tutorial package.

Using rqt

The rqt tool has two interesting plugins for control purposes:

  • The Robot Tools/Controller Manager: To load, unload, start and stop controllers.

  • The Robot Tools/Joint Trajectory Controller: To move the robot joints using a Joint Trajectory Controller.

The following launch file opens Gazebo with the pan-tilt structure equipped with the controllers of the controller file shown above, and also opens rqt with a perspective (config/tutorial10.persepctive) that already includes both tools, as shown in the figure.

$ roslaunch gazebo_control_tutorial pan_tilt_rqt.launch
../_images/pan_tilt_rqt.png

Industrial robots

Some industrial robots already provide a ROS interface, which make them really useful for their integration in robotic applications, like Universal Robots or Stäubli (experimental).

Universal Robots

In the universal_robot package you can find for the UR3 robot:

  • The joint trajectory controller defined in the ur_gazebo/controller/arm_controller_ur3.yaml file:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
arm_controller:
 type: position_controllers/JointTrajectoryController
 joints:
    - shoulder_pan_joint
    - shoulder_lift_joint
    - elbow_joint
    - wrist_1_joint
    - wrist_2_joint
    - wrist_3_joint
 constraints:
     goal_time: 0.6
     stopped_velocity_tolerance: 0.05
     shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
     shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
     elbow_joint: {trajectory: 0.1, goal: 0.1}
     wrist_1_joint: {trajectory: 0.1, goal: 0.1}
     wrist_2_joint: {trajectory: 0.1, goal: 0.1}
     wrist_3_joint: {trajectory: 0.1, goal: 0.1}
 stop_trajectory_duration: 0.5
 state_publish_rate:  25
 action_monitor_rate: 10

Note that the default values of the tolerance parameters of the FollowJointTrajectory action are defined in this file as constraints.

  • The joint state controller defined in the ur_gazebo/controller/joint_state_controller.yaml file:

1
2
3
joint_state_controller:
   type: joint_state_controller/JointStateController
   publish_rate: 50

To bring up the UR3 simulated robot in Gazebo, you can run:

$ roslaunch ur_gazebo ur3.launch

Exercise

Use the rqt tool to control the UR3 robot (you can use the tutorial10.persepctive).

../_images/ur_rqt.png

Stäubli

In the staubli_experimental package you can find for the TX90 robot:

  • The joint trajectory controller defined in the staubli_tx90_gazebo/config/tx90_arm_controller.yaml file:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
arm_controller:
 type: position_controllers/JointTrajectoryController
 joints:
   - joint_1
   - joint_2
   - joint_3
   - joint_4
   - joint_5
   - joint_6
 constraints:
   goal_time: 0.6
   stopped_velocity_tolerance: 0.05
   joint_1: {trajectory: 0.1, goal: 0.1}
   joint_2: {trajectory: 0.1, goal: 0.1}
   joint_3: {trajectory: 0.1, goal: 0.1}
   joint_4: {trajectory: 0.1, goal: 0.1}
   joint_5: {trajectory: 0.1, goal: 0.1}
   joint_6: {trajectory: 0.1, goal: 0.1}
 stop_trajectory_duration: 0.5
 state_publish_rate:  25
 action_monitor_rate: 10
  • The joint state controller defined in the staubli_tx90_gazebo/config/joint_state_controller.yaml file:

1
2
3
joint_state_controller:
 type: joint_state_controller/JointStateController
 publish_rate: 50

To bring up the simulated TX90 robot in Gazebo, you can run:

$ roslaunch staubli_tx90_gazebo tx90_gazebo.launch

Exercise

Use the rqt tool to control the TX90 robot (you can use the tutorial10.perspective).

../_images/staubli_rqt.png

EXERCISE 7

To solve Exercise 7 you must create a new package:

  • Create a new project in the EXERCISE7 subgroup of your GitLab account: Use Create blank project and set the Project name as action_manager.

  • In your git-projects folder clone the new project:

    $ git clone https://gitlab.com/intro2ros/2023/teamXX/exercise7/action_manager.git exercise7/action_manager
    

    or

    $ git clone clone git@gitlab.com:intro2ros/2023/teamXX/exercise7/action_manager.git exercise7/action_manager
    
  • In your catkin_ws10/src folder create a symbolic link to it:

    $ ln -s ~/git-projects/exercise7/action_manager
    

Recall to keep commiting the changes when coding the solution (do small commits with good commit messages!) and write a README.md file in the solution_exercise7 package explaining your solution (Markdown Cheat Sheet).

Exercise 7

Modify the follow_traj_wrap_server of exercise 6 to move the UR3 robot instead of the pan-tilt. You can use this client to test it (when working in the constraint mode, the client allows to preempt the motion to avoid collisions with the floor). You can reuse many things from the actions_tutorial package implemented for exercise 6.

Note 1: In the follow_traj_wrap_server recall to change any actions_tutorial by action_manager. Also, be careful with the name of the action in the creator of the actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>.

Note 2: To run the system, create a launch file called ur3_trajectory_controller_server.launch that:
  • includes the ur3.launch file from the ur_gazebo package to start the controller and spawn the robot in gazebo;

  • starts the follow_traj_wrap_server and ur3_follow_traj_client nodes.

SOLUTION EXERCISE 7

Download the zipped solution.