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

Introduction

In the previous tutorial you learned how to create a virtual environment for the robot to simulate it.

Building on this, this tutorial describes how to control the robot with commands. For this the Pilz motion planning tool command_planner is used, this can process the commands LIN, PTP and CIRC and will be discussed in more detail below.

After the explanation of the Pilz command_planner you will be shown how you can control the robot in RViz with it. 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 will develop a simple pick and place application, in which we place the implemented PNOZ from tutorial 1 plus another PNOZ from one side to the other. The basic Python commands as well as the structure and execution of such a file will also be discussed.

So in the end you have a working application in your virtual environment in which the robot can move PNOZes with the commands mentioned above.""pick and place function"""

Prerequisites

In order to complete this tutorial, you need the following. If you did tutorial 1 then you don't have to do anything else:

• The prerequisites from tutorial 1 pilz_robots/Tutorials/ModelYourApplicationWithPRBT

• Completed the first Pilz robots tutorial pilz_robots/Tutorials/ModelYourApplicationWithPRBT 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 to NOT WORKING YET!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 to the other. Therefore we change in line 24 the y-value to zero.

<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 with 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 command_planner. To test the successful installation (and your ROS environment) you can run:

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 paths. Maybe you recognized the argument pipeline:=command_planner in the test command above. We have to add this in our launch file my_application.launch to use the Pilz command_planner pipeline including the blend capabilities (line 12 and 13):

   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 (you can use the command: roslaunch pilz_tutorial my_application.launch in the terminal) RViz will show you 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 of 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 the 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 an 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) you can use drag and drop the green 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 green sphere. Furthermore reduce the velocity and acceleration in the planning tab to 0.2 to avoid collisions cause through unexpected movements at your real robot.

2

Now you can start to plan the trajectory path and after that move your robot. Once you press the button "Plan" in the Planning tab at the bottom left from your window, the pilz_command_planner will plan the path for the robot and show it in RViz.

Is the planned path ok with you, you can press "Execute" to move your robot among the planned path. As result the robot moves to that goal position with avoiding all possible collisions with scene objects.

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

Python Setup

Because with RViz you cannot program any sequences of points, let alone branches or loops, 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 directory /scripts in your pilz_tutorial package (beneath the launch and urdf file). Open the new created folder and create a new document with right-click →New Document. Name the document for example myFirstApplication.py. Do you recognized 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 the only way to start it later.

Copy the following lines in sequence into your file to make a working application, you can find the finished application at the end of chapter 9.

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, Pilz-specific and general libraries are imported. These are needed to provide the desired functionality later. For example the module math we will need to convert degree to radian later. For detailed information see the docomentation from the respective library.

from geometry_msgs.msg import Pose, Point
from pilz_robot_programming.robot import *
from pilz_robot_programming.commands 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 modulename. as prefix in front of your variable or function. For more information see Effbot.org/difference between import and from import

In the next line we have to define the API version we want to use. This is reasoned by the fact 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

Now we can proceed to the development of the function start_program(). This function contains the main program flow and will be called later from the pythonm interpreter.

#main program
def start_program():

We won't write anything in this function yet.

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

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 in here:

• Init a rosnode: Start a rosnode for the Python program. This node will communicate the python script 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. This one does nothing yet, but after the explenation 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 progrgram in RViz

Make sure you saved the Python script, and that your robot is running in RViz. Otherwise you have to run the launchfile 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 visualisation 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 analyse 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 in the launchfile. In this case everytime you run the launchfile your script will executed. But we recommend to start the launchfile at the first steps separate to the Python script. The result is that you save a lot of time because of the shorter start process and the program is easier to debug.

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:

• Resume the program:

Debugging

If any errors are occuring look in the Terminals, and read the error messages. It also shows in which line of your program the error occurs, to simplify the debugging process.

Program the application

Description

As an example we will program a simple pick and place movement. In this, the robot moves to one side, makes a pick movement and then moves to a defined point. There it will insert the imaginary workpiece into an imaginary machine, wait, and take it out again. It then moves to the place point and makes the place movement. These functions are called sequentially in an endless loop. You can try out your program after every step, to see if your code works or not.

First Move

To move your robot there are two coordinate systems you can use:

• Cartesian
• Joint-Values

To teach your robot you have to move it with an panel, with commands, with RViz or with some other possibilitys to the right position. In this case we will use RViz to move the robot in the position about the PNOZ.

Therefore we have to

• Check Planning Request --> Query Goal State

• Move the orange robot with the blue ball to the desired position
• Plan and execute the path with the pilz_command_planner plugin PTP
• Wait until the robot is in the right position

Then 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. To do this you have to open in the top left corner the following: MotionPlanning → Scene Robot → Links → prbt_tcp → Position / Orientation (picture Read_RViz_coordinates.png) You have to

• TF rosrun tf tf_echo /table /prbt_tcp, Ctrl + C

• Python
  iPython or real Python script; print(r.get_current_joint_values()) print(r.get_current_pose())

• Rostopic rostopic echo /joint_states

To create the program clear and readable, we will first add a position variable for each system start_program():. Then we will move first to the joint-value-goal, and then to the cartesian goal.

#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 mostly avoid random trajectorys from i.e. a singularity at your program.

Try out the program, and look how the robot is moving 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 can add the move commands. In this case 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 /pilz_robot_programming/documentation. Of course you also can move absolute, but in this case it doesen't make sense.

To try your program you just have to 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.

Erklärungsblabla mit code zitaten

my_first_application.py

   1 #!/usr/bin/env python
   2 from geometry_msgs.msg import Pose, Point
   3 from pilz_robot_programming.robot import *
   4 from pilz_robot_programming.commands import *
   5 import math
   6 import rospy
   7 
   8 __REQUIRED_API_VERSION__ = "0"  #API version
   9 __ROBOT_VELOCITY__ = 1          #velocity of the robot
  10 
  11 #main program
  12 def start_program():
  13 
  14     #important positions
  15     start_pos = [1.49, -0.54, 1.09, 0.05, 0.91,-1.67]   #joint values
  16     
  17     pos_pick = Point (0, -0.5, 0.25)            #cartesian coordinates
  18     pos_work_station = Point(-0.5, 0.1, 0.2)    #cartesian coordinates
  19     pos_place = Point(-0.1,0.4,0.25)            #cartesian coordinates
  20     
  21     #Spherical coordinates
  22     orientation_pick = from_euler(0, math.radians(180), math.radians(0))
  23     orientation_work_station = from_euler(0, math.radians(-135), math.radians(90))
  24     orientation_place = from_euler(0, math.radians(180),  math.radians(90))
  25 
  26     #move to start point with joint values to avoid random trajectory
  27     r.move(Ptp(goal=start_pos, vel_scale=__ROBOT_VELOCITY__))
  28     
  29     while(True):
  30         #Do infinite loop
  31         #Pick the PNOZ
  32         r.move(Ptp(goal=Pose(position=pos_pick, orientation=orientation_pick),
  33             vel_scale = __ROBOT_VELOCITY__, relative=False))
  34         pick_and_place()
  35 
  36         #Put the PNOZ in a "machine"
  37         r.move(Ptp(goal=Pose(position=pos_work_station,
  38             orientation=orientation_work_station),vel_scale = __ROBOT_VELOCITY__, relative=False))
  39         pick_and_place()
  40         rospy.sleep(1)      # Sleeps for 1 sec (wait until work is finished)
  41         pick_and_place()
  42 
  43         #Place the PNOZ
  44         r.move(Ptp(goal=Pose(position=pos_place, orientation=orientation_place),
  45             vel_scale = __ROBOT_VELOCITY__, relative=False))
  46         pick_and_place()
  47 
  48 def pick_and_place():
  49     """pick and place function"""
  50 
  51     #A static velocity from 0.2 is used
  52     #The position is given relative to the TCP. 
  53     r.move(Ptp(goal=Pose(position=Point(0, 0, 0.1)), reference_frame="prbt_tcp", vel_scale=0.2))
  54     rospy.sleep(0.2)    #Pick or Place the PNOZ (close or open the gripper)
  55     r.move(Ptp(goal=Pose(position=Point(0, 0, -0.1)), reference_frame="prbt_tcp", vel_scale=0.2))
  56 
  57 
  58 if __name__ == "__main__":
  59     # Init a rosnode
  60     rospy.init_node('robot_program_node')
  61 
  62     #initialisation
  63     r = Robot(__REQUIRED_API_VERSION__)  #instance of the robot
  64    
  65     #start the main program
  66     start_program()     

Then the robot will be moved analogous to pick_and_place(r), first to pick (line 67 - 69), and then to place (line 71 - 73) the imaginary workpiece into the machine. Between these movements, the robot waits one second (line 70) until the imaginary machine has finished working with the piece. Therefore the function is called work(r).

Error: No code_block found

Now we can proceed to the development of the main function (line 10 - 46). This is at the top of the program, and contains the while(true) loop as well as the variable initializations. At first the robot is instantiated (line 13), the '0' means the version number and should simply be adopted.

Then the positions (lines 19 - 21) as well as the orientations (lines 24 - 26) of the three main positions of the robot are defined. The start position (line 17) should be defined as an array of joint-values (in radian angles) to avoid random movements out of a singularity. In line 29 the robot will be moved to this position. Error: No code_block found

Now the endless loop while(true) is called. It moves to the previously defined positions one after the other and calls the respective function to pick, place or work. The movements are executed absolutely in the Cartesian world coordinate system. You can see this at the parameter relative=False. The velocity of the robot is set by the variable vel at line 14. So of you are in a real environment, you just have to change this line, and your whole program will run slower or faster. But don't forget to pass this parameter to your sub-functions, if you want to. We didn't do that at this example application, to keep it simple. Error: No code_block found

Conclusion

In this tutorial you have learned, how to setup and control the robot with the Python API. Therefore you can now write your first own program with moving the PRBT.