Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Move your robot with the pilz_command_planner

Description: Setup your ROS with the pilz_industrial_motion packages and learn how to plan and move your robot along a path with the pilz_command_planner.

Tutorial Level: BEGINNER

Next Tutorial: Program your robot with the Python API

Contents

Introduction
Prerequisites
Setup your simulation environment
Install Pilz industrial motion package
Startup with pilz_industrial_motion capabilities
Move Robot using RViz MotionPlanningPlugin
Python Setup
Run the program in RViz
Runtime tools and debugging
1. Runtime tools
2. Debugging
Program the application
Conclusion

Introduction

In the previous tutorial you learned how to model a virtual environment and to drag the robot around there.

Building on this, this tutorial describes how to control the robot with repetitive commands. We use the Pilz motion planning command_planner, which can process the commands LIN, PTP and CIRC and will be discussed in more detail below.

You will first learn how to control the robot in RViz with the Pilz command_planner. Then we will explain how to create a new Python file, and how to control the robot with a few lines of code. As an example application we'll use a simple pick and place application. We will discuss the basic Python commands as well as the structure and execution of such a script file.

So in the end you have a working application in your virtual environment in which the robot moves in your virtual environment with industrial motion commands in a Python script.

You can also control a real robot manipulator with the same procedure but we limit this tutorial to a virtual environment. In the next tutorial we will show you how to do it with a real robot.

The files of this tutorials are also available for download from GitHub/pilz_tutorials. But we recommend to create the files on your own during this tutorial.

Prerequisites

If you completed tutorial 1 then you don't have to do anything else, you just need:

The prerequisites from tutorial 1 Model your application with PRBT
Completed the first Pilz robots tutorial Model your application with PRBT or download the files from this tutorial GitHub/pilz_tutorials (pilz_tutorial_1)

Setup your simulation environment

The requirement for path-planning is to tell the program where the tool center point is positioned at the robot model. To do so you have to add the tcp-joint. You can find the whole xacro file at my_first_application.xacro:

  <!-- Add the tcp (tutorial 2) -->
  <link name="prbt_tcp"/>

  <!-- connect the added tcp and the flange with a joint (tutorial 2)-->
  <joint name="prbt_fixed_joint_tcp" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.05"/>
    <parent link="prbt_flange"/>
    <child link="prbt_tcp"/>
  </joint>

Furthermore we want to move the table a little bit, to model an environment where the robot can move the PNOZ from one side of the table to the other. Therefore we change the y-value to zero in line 24.

<origin rpy="0 0 0" xyz="0 0 -0.03"/>

But if you have an own real environment for your robot, feel free to change the xacro file to match your specific requirements. We will also do so in one of the next tutorials, when we try to run the real robot.

Install Pilz industrial motion package

The Pilz command_planner is available in the industrial motion package. Update your package list and install the pilz_industrial_motion meta-package in a terminal:

sudo apt-get update
sudo apt install ros-kinetic-pilz-industrial-motion

This installs the pilz_extensions, pilz_msgs and pilz_trajectory_generation package, with includes the Pilz command_planner. To test the successful installation (and your ROS environment) you can run in the terminal:

roslaunch prbt_moveit_config moveit_planning_execution.launch pipeline:=command_planner

Startup with pilz_industrial_motion capabilities

We use industrial motion commands PTP and LIN to move the robot on predefined paths that give fast and reproducible trajectories. Maybe you recognized the argument pipeline:=command_planner in the test command above. We have to add this to our launch file my_application.launch to use the Pilz command_planner and also include the blend capabilities (line 12 and 13):

Toggle line numbers

   1 <?xml version="1.0"?>
   2 <launch>
   3 
   4   <arg name="sim" default="true" />
   5 
   6   <!-- send urdf to param server -->
   7   <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find pilz_tutorial)/urdf/my_first_application.xacro'"/>
   8 
   9   <include file="$(find prbt_moveit_config)/launch/moveit_planning_execution.launch">
  10     <arg name="load_robot_description" value="false"/>
  11     <arg name="sim" value="$(arg sim)"/>
  12     <arg name="pipeline" value="command_planner"/>
  13     <arg name="capabilities" value="pilz_trajectory_generation/MoveGroupBlendAction pilz_trajectory_generation/MoveGroupBlendService"/>
  14   </include>
  15 
  16 </launch>

If you start the launch file (use the command: roslaunch pilz_tutorial my_application.launch in the terminal) RViz shows the Planner Plugin "Simple Command Planner" and you can select the planner Plugin "PTP":

Of course you can use the other Plugins LIN and CIRC too, but pay attention to the following restrictions:

With LIN you have to watch your velocity and acceleration (if it is too high, the robot won't move. In this case you can observe an error-messages in the terminal to see if it is because of too high velocity, or if it is because something else).
The plugin CIRC you can not use in RViz because you need to define at least two points for a circle. But you could use it for example in your soon created Python script.

Move Robot using RViz MotionPlanningPlugin

If you have selected the desired plugin (LIN or PTP) drag and drop the turquoise sphere to move the robot to an approximate goal location. You can change the tool orientation by moving the red, green or blue circles around the turquoise sphere. Furthermore reduce the velocity and acceleration in the planning tab to 0.2 to avoid collisions caused by unexpected movements at your real robot.

Now you can start to plan the trajectory path and after that move your robot. Press the button "Plan" in the Planning tab at the bottom left of your window and the Pilz command_planner will plan the path for the robot and show it in RViz.

Is the planned path ok with you, press "Execute" to move your robot among the planned path. As result the robot moves to that goal position still checking possible collisions with scene objects and rejecting trajectories that would result in collisions.

If you don't want to see the planned path first, you can directly press "Plan and Execute" to do both steps at once.

As an exercise move the robot just above the PNOZ now, so that we can read the coordinates of its position in the next steps.

Python Setup

In RViz you cannot program any sequences of points, let alone branches or loops, so in this step we will show you how to principially use the Python API.

First of all we have to create the Python-file. So at the first step you have to create a new folder /scripts in your pilz_tutorial package (beneath the launch and urdf folder). Open the new created folder and create a new document with right-click →"New Document". Name the document for example myFirstApplication.py. Did you recognize the ending *.py? This one is reserved for Python-scripts.

Now right-click the file and open the settings window. In this window you have to switch to the second tab "Permissions". Check the checkbox at "Execute" to make the program executable. This is necessary to start it with rosrun later on.

Copy the following lines in sequence into your file to build a working application:

In the first line we have to tell the program loader in which language we wrote the script and which interpreter it should use.

 #!/usr/bin/env python

Then we import Pilz-specific and general libraries. These are needed to provide the desired functionality. We use the module math to convert angles from degree to radian. For detailed information see the documentation from the respective library.

from geometry_msgs.msg import Pose, Point
from pilz_robot_programming import *
import math
import rospy

The difference between from ... import and import is that if you use the first expression you can directly access all the variables and functions of this module without prefix. So it overwrites all same named variables you declared before. With import instead you have to write the modulename as prefix in front of your variable or function, i.e. math.radians(). For more information see Effbot.org/difference between import and from import

In the next line we define the API version we want to use. The reason is, that in a newer API version the robot could move a little bit different to the older version. In that way if you don't change the API version in this line the robot will probably move the same, even after an update.

__REQUIRED_API_VERSION__ = "0"  #API version

Next we define the function start_program(). This function contains the main program flow and we'll call it later from the python interpreter.

#main program
def start_program():

We won't write anything in this function yet.

As last step we have to show the interpreter, which function is the main function, and do some initialization.

if __name__ == "__main__":
    # Init a rosnode
    rospy.init_node('robot_program_node')

    #initialisation
    r = Robot(__REQUIRED_API_VERSION__)  #instance of the robot

    #start the main program
    start_program()

We do three things here:

Init a rosnode: Start a rosnode to setup communication with the ROS-system.
Instantiate the robot: We create a new instance of the class robot and pass the API version in there.
Call the main function

Now we finished our first Python script. It does nothing yet, but after the explanation of how to run and debug the script, we will program the first working application. This will help you to create your own script step by step.

Run the program in RViz

Make sure you saved the Python script, and that your robot is running in RViz. Otherwise you have to run the launch file from step 4 again:

roslaunch prbt_moveit_config moveit_planning_execution.launch pipeline:=command_planner

After RViz is open, we don't want to move it with the tracking ball but with the Python script. Therefore we don't need to plan and execute the robot program in RViz, so first of all we will change some display settings to see a better visualization at the end. This won't effect any functions or technical settings of the program. You can find the settings in the top left corner:

Check Scene Robot --> Show Robot Visual
Set value to 1 in Scene Robot --> Robot Alpha
Uncheck Planning Request --> Query Goal State
Uncheck Planned Path --> Show Robot Visual

Now we can run the script. Start a new terminal with Ctrl + Alt + T and type:

rosrun pilz_tutorial myFirstApplication.py

This should start your program. If you did everything correct, your robot should move as desired. If not, it won't move or will stop moving at a specific step. In this case look at the chapter Runtime tools and debugging to analyze the mistake. In this chapter we will explain how to end, pause or debug the program.

If your program works as desired, you can add the program into the launch file. In this case everytime you run the launch file your script will executed. But we recommend to start the launch file separate from the Python script for now. The result is, that you can restart the python script faster and debug it easier.

Runtime tools and debugging

Runtime tools

End the program: Simply press Ctrl + C in the terminal in which you started the program, and wait until the program ends. This also stops the robot movement.
Pause the program: Open up a new terminal and call rosservice call /pause_movement. Interrupts your program immediately at its current position. To continue the program at this point again, follow the next step: Resume the program
Resume the program: Open up a new terminal and call rosservice call /resume_movement. Continues your paused program immediately at its current position.

Debugging

If any errors are occurring look at first in the terminals, and read the error messages. If you started the robot (launch file) and your python script (*.py) in separate terminals, you can debug your program easier. The reason is that the terminal where you started your script shows all the Python errors. Furthermore it also shows in which line of your program the error occurs, to simplify the debugging process.

On the other hand the terminal in which you started the robot shows all the errors from the robot. For example if you collide with the wall or have a cable rupture.

But errors if for example the kinematic solution can not be found will be shown in both terminals. So if you want to find the reason why your robot stopped, or did unexpected things, you better check both terminals for errors.

Program the application

Description

As an example we finally continue to program our simple pick and place trajectory. The robot shall move to one side, pick up an imaginary part and then moves to a defined point. There it inserts the imaginary workpiece into an imaginary machine, waits, and takes it out again. It then moves to the place point and makes the place movement. These functions are called sequentially in an endless loop. So the program flow will look like the following.

You can try out your program after every step, to see if your code works and fine-tune the positions.

Teaching and reading current robot state

To move your robot, you can use two coordinate systems:

Cartesian
Joint-Values

To teach your robot you have to move it with a panel, with commands, with RViz or with some other possibilities to the right position. For now we will use RViz to move the robot to a pick position above the PNOZ (see step 6. Move Robot using RViz MotionPlanningPlugin; don't forget to undo the settings in step 8. Run the program in RViz).

You can read out the coordinates in different ways for our Python script. We present you some options and you can choose on your own which one is the best for you.

RViz - You can read the cartesian coordinates directly in RViz, with the minus that you can not copy the coordinates. Also be aware that the orientation will be presented in quaternions. To do this you have to open in the top left corner the following: MotionPlanning → Scene Robot → Links → prbt_tcp → Position / Orientation
Python
Run this command in iPython or in your Python script:
```
print(r.get_current_joint_values())
#  or
print(r.get_current_pose())
```
As result you will get the joint states or the cartesian pose from the current robot position printed in the console. For more information look in the pilz_robot_programming API documentation.
TF To get the current position from tf, go to a new terminal and run
```
rosrun tf tf_echo /table /prbt_tcp
```
Once the desired values are presented in the terminal you can press Ctrl + C to stop the position updates.
Rostopic To read the joint_states directly, open a new terminal and type:
```
rostopic echo /joint_states
```
Now all the joint states are presented in the terminal.

First Move

We will write now the start sequence of the program to move the robot in a specific start position. To create the program clear and readable, we will first add a position variable for each state in start_program():. Then we move first to the joint-value-goal, and then to the cartesian goal. Please use your own start position from the robot that you moved to the PNOZ in the previous step.

#main program
def start_program():

        #define the positions:
        start_pos = [1.49, -0.54, 1.09, 0.05, 0.91,-1.67]   #joint values

        pos_pick = Point (0, -0.5, 0.25) #cartesian coordinates
        orientation_pick = from_euler(0, math.radians(180), math.radians(0)) #cartesian coordinates

        #Move to start point with joint values to avoid random trajectory
        r.move(Ptp(goal=start_pos, vel_scale=__ROBOT_VELOCITY__))

        #Move cartesian
        r.move(Ptp(goal=Pose(position=pos_pick, orientation=orientation_pick),vel_scale = 0.2))

It is recommend that your first move command is in joint values. This will avoid random trajectorys from multiple kinematics solutions for the same pose.

Try out the program, and look how the robot moves to the different positions.

Pick_and_place function

Now we will modify and implement the move-commands in a pick_and_place function. Therefore at the end we have a function in which the robot will move relative to the tcp down, open/close the gripper, and move up again. So in the main program we only have to call this function to do the pick and place movement.

First of all we have to implement the function:

def pick_and_place():
    """pick and place function"""

Then we add the move commands. Currently we don't have a gripper mounted at the arm, so instead of opening and closing the gripper we will sleep for 0.2 seconds:

def pick_and_place():
    """pick and place function"""

    #A static velocity from 0.2 is used
    #The position is given relative to the TCP.
    r.move(Ptp(goal=Pose(position=Point(0, 0, 0.1)), reference_frame="prbt_tcp", vel_scale=0.2))
    rospy.sleep(0.2)    #Pick or Place the PNOZ (close or open the gripper)
    r.move(Ptp(goal=Pose(position=Point(0, 0, -0.1)), reference_frame="prbt_tcp", vel_scale=0.2))

The movement is first 0.1 (10 cm) downwards, and then 0.1 upwards. The reference_frame for this movement is set to prbt_tcp, which means that the movement is relative to the tcp of the robot. So in whatever position the robot is standing, the pick_and_place() function will always move orthogonal to the tcp. If you want to create your own reference frame, you can look the commands for it up in the NOT WORKING YET /pilz_robot_programming/documentation. Of course you can also move absolute, but for pick-and-place prbt_tcp is the correct coordinate system.

To try your program just call the function in the main function start_program(). Make sure that the start-position of the robot allows a down and up movement.

#main program
def start_program():

    #[...] other code, not presented here
    pick_and_place()

Main function

We did all preparations to develop the whole program now. It will consist of the definition of all the points and a endless loop executing the main program.

So first search with RViz for two more points (except the pick and start point), the robot will move from start to the work point, than to the place point and in the end back to the pick point. Beware that the tcp need a bit space to move a little bit up and down for picking and placing.

Now we write the positions in start_program() beneath the other position variables: Error: No code_block found

That will design our later program more readable and understandable. At next step we have to move the robot to a predefined start position, so we add a move command: Error: No code_block found

After that we can start the main program sequence. It runs in a endless loop and consists of: (Bild Programmablauf)

Therefore we simply alternate between moving to a point, and then call the pick_and_place() function we developed. The result should be similar to the following code: Error: No code_block found

If we now run the program, the robot should move as specified from one point to another. At each point he should do an up-down-movement to pick the PNOZ. This all will be done in an endless loop so the program won't stop if you don't stop it.

You can find the whole Python script here: https://github.com/PilzDE/pilz_tutorials/blob/master/pilz_tutorial_2/pilz_tutorial/scripts/myFirstApplication.py|GitHub/myFirstApplication.py]], or simply copy the following lines to your script.