(!) 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 Python API

Description: Develop a pick and place application and learn how to teach positions. You create a script which runs with the Python API and use the PTP and LIN commands.

Tutorial Level: BEGINNER

Introduction

In the previous tutorial you learned how to move the robot in RViz with the Pilz command_planner. But in RViz you cannot program any sequences of points, let alone branches or loops, so in this tutorial we will show you how to use the Python API.

To do so 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 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 a later tutorial we will show you how to do it with a real robot.

Prerequisites

You need to have the following prerequisites:

The files at the end of this tutorials are also available for download from GitHub/pilz_tutorial_3. But we recommend to start with the end of tutorial 2 and create the files from this tutorial on your own.

Basics and setup

Python Setup

Create the Python-file in a new folder /scripts in your pilz_tutorial package besides the launch and urdf folder. Name the document myFirstApplication.py and make it executable.

$ chmod +x myFirstApplication.py

For Beginners: Making a file executable is necessary to run it later using rosrun. To do so open your file explorer, 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 "Make the program executable".

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.

   1 #!/usr/bin/env python

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

   1 from geometry_msgs.msg import Pose, Point
   2 from pilz_robot_programming import *
   3 from math import radians
   4 import rospy

In the next line we define the pilz_robot_programming 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. Beneath the version definition, we define the standard velocity of the robot. So if we want to slow down or speed up the robot we just change __ROBOT_VELOCITY__ in a single line to change the velocity in the whole program.

   1 __REQUIRED_API_VERSION__ = "1"  # API version
   2 __ROBOT_VELOCITY__ = 0.5        # velocity of the robot

Next define the function start_program(). This function contains the actual program flow and we'll call it later from the python interpreter. We won't write anything in this function yet because for now we just want to show the principial structure of the Python script. So write pass in the function, this means to do nothing and just symbolizes a placeholder.

   1 # main program
   2 def start_program():
   3     pass # do nothing

As last step we have to show the interpreter, which function is the start function, and do some initialization. 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 start function

   1 if __name__ == "__main__":
   2     # init a rosnode
   3     rospy.init_node('robot_program_node')
   4 
   5     # initialisation
   6     r = Robot(__REQUIRED_API_VERSION__)  # instance of the robot
   7 
   8     # start the main program
   9     start_program()

If you pasted all the above lines, you finished your first Python script. It does nothing yet, so we will program the first working application in the next steps. You can find this empty script as template in !emptyPythonTemplate.py. This will help you to create your own script step by step.

RViz: Move Robot to the start position

As RViz is now open, we move the robot with the turquise tracking ball in a valid start position (Move the orange robot to a position similiar to the picture below and press Plan and Execute). We will use this position later to read the start-coordinates from the robot, and safe them in our Python script.

http://wiki.ros.org/pilz_robots/Tutorials/MoveRobotWithPilzCommand_planner?action=AttachFile&do=get&target=planning_request.png

Reading current robot state

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

  • Cartesian - In this one the position of the robot is split in 'position' and 'orientation'. The first one describes the position in the 3D space, with the orientation given in quaternions. The can optional be converted from spherical coordinates with the function from_euler(a, b, c) from the pilz_robot_programming package in intrinsic ZYZ convention (in radians).

  • Joint-Values - This is the position of the manipulator given in radian angles of the robot joints . Therefore we get a unique position of the robot.

To read the joint values, open a new terminal and type:

$ rostopic echo /joint_states

Now all the joint states are presented in the terminal. Press Ctrl + C to stop the updates, but leave it open to copy them in the next step to your program. Of course there are more options for reading the coordinates too, read the note below for more infos.

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. Then you can read out the coordinates in different ways for our Python script. We present you some more 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 disadvantage that you can not copy the coordinates. Also be aware that the orientation will be presented in quaternions. They are a more complicated, mathematically unambiguous indication of an orientation in space and does not need to understand more closely. To do this you have to open in the top left corner the following: MotionPlanning → Scene Robot → Links → prbt_tcp → Position / Orientation Read_RViz_coordinates.png

  • Python
    Run this command in your Python script or in a Python console like iPython:

    $ print(r.get_current_joint_values())
    $ #  or
    $ print(r.get_current_pose())

    As result you will get the joint values or the cartesian pose from the current robot position printed in the console. For more information look in the pilz_robot_programming’s documentation.

  • TF To get the current position from tf, go to a new terminal and run

    $ rosrun tf tf_echo /prbt_base_link /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 values directly, open a new terminal and type:

    $ rostopic echo /joint_states

First Move

You are going to write the command to move the robot in a specific start position now. To create the program clear and readable, first add a position variable for the start state in start_program():. Please use your own start position from the robot that you moved to the start position in the previous step for this. After that we can give the move command r.move(...) to tell the robot that he should move there.

   1 # main program
   2 def start_program():
   3 
   4         # define the positions:
   5         start_pos = [1.49, -0.54, 1.09, 0.05, 0.91,-1.67]   # joint values
   6 
   7         # Move to start point with joint values to avoid random trajectory
   8         r.move(Ptp(goal=start_pos, vel_scale=__ROBOT_VELOCITY__))

It is always recommended 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 start position (Move the robot away manually or restart the launch file).

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 last tutorial Move your robot with the Pilz command_planner again:

$ roslaunch pilz_tutorial my_application.launch

Now we can run the script to execute the Python code at the robot in RViz. Start a new terminal with Ctrl + Shift + T and type:

$ rosrun pilz_tutorial myFirstApplication.py

This should start your program. Because we have an empty Python script the robot won't move yet. If there are any errors, it won't move too or will stop moving at a specific step. In this case look at the Debugging section below to analyze the mistake. In that 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.

In the next steps we won't move the robot with the tracking ball any more but move it with the Python script. Therefore we change some display settings to see a better visualization at the end. This won't effect any functions or technical settings of the program.

Adjust the following settings (see picture):

  1. Press Add to insert a new module

  2. Select RobotModel

  3. Uncheck MotionPlanning plugin to hide the orange robot goal

RVIZ_Deselect_MotionPlanning.png

Program the application

Description

As an example we now 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.

PAP_Tutorial_2_small.png

First we will program the pick_and place function based on the previous Python file. Then we can write our main positions in the start sequence. After that we simply write the program flow into an endless loop. You can try out your program after every step, to see if your code works and fine-tune the positions.

Pick_and_place function

Now you have to modify and implement the move-commands in a pick_and_place() function. Therefore at the end you 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 you only have to call this function to do the pick and place movement.

First of all implement the function pick_and_place() in your program:

  58 def pick_and_place():
  59     """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:

  58 def pick_and_place():
  59     """pick and place function"""
  60 
  61     # a static velocity from 0.2 is used
  62     # the position is given relative to the TCP. 
  63     r.move(Ptp(goal=Pose(position=Point(0, 0, 0.1)), reference_frame="prbt_tcp", vel_scale=0.2))
  64     rospy.loginfo("Open/Close the gripper") # log
  65     rospy.sleep(0.2)    # pick or Place the PNOZ (close or open the gripper)
  66     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’s 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 positions of the robot allows a down and up movement without a collision.

   1 #main program
   2 def start_program():
   3 
   4     #[...] other code, not presented here
   5     pick_and_place()

Start sequence

We did all preparations to develop the remaining program now. So first search and write three valid position and orientation variables in start_program() beneath the start position variable. You can orientate at the little video at the end of this chapter. You will call your variables at a later move command to design our later program more readable and understandable.

If you need help, visit pilz_robot_programming’s documentation.

   1 # main program
   2 def start_program():
   3 
   4     rospy.loginfo("Program started") # log
   5 
   6     # important positions
   7     start_pos = [1.49, -0.54, 1.09, 0.05, 0.91,-1.67]   # joint values
   8     
   9     pos_pick = # fill it in
  10     pos_work_station = # fill it in
  11     pos_place = # fill it in
  12     
  13     # spherical coordinates
  14     orientation_pick = # fill it in
  15     orientation_work_station = # fill it in
  16     orientation_place = # fill it in

At next step we have to move the robot to a predefined start position, so we leave the existing move command in the main program where it is (beneath the variable declarations).

After that we can start the main program sequence. It runs in a endless loop and moves the robot in a loop.

Endless loop

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:

  31     while(True):
  32         # do infinite loop  
  33         
  34         # pick the PNOZ
  35         rospy.loginfo("Move to pick position") # log
  36         r.move(Ptp(goal=Pose(position=pos_pick, orientation=orientation_pick),
  37             vel_scale = __ROBOT_VELOCITY__, relative=False))
  38         rospy.loginfo("Pick movement") # log
  39         pick_and_place()
  40 
  41         # put the PNOZ in a "machine"
  42         rospy.loginfo("Move to virtual machine") # log
  43         r.move(Ptp(goal=Pose(position=pos_work_station,
  44             orientation=orientation_work_station),vel_scale = __ROBOT_VELOCITY__, relative=False))
  45         rospy.loginfo("Place PNOZ in machine") # log
  46         pick_and_place()
  47         rospy.sleep(1)      # Sleeps for 1 sec (wait until work is finished)
  48         rospy.loginfo("Pick PNOZ from machine") # log
  49         pick_and_place()
  50 
  51         # place the PNOZ
  52         rospy.loginfo("Move to place position") # log
  53         r.move(Ptp(goal=Pose(position=pos_place, orientation=orientation_place),
  54             vel_scale = __ROBOT_VELOCITY__, relative=False))
  55         rospy.loginfo("Place movement") # log
  56         pick_and_place()

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.

Whole program

You can find the whole Python script here: GitHub/myFirstApplication.py, or simply copy the following lines to your script.

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

Runtime tools and debugging

At last we present you some helpful tips and tricks. They are not necessary for this tutorial, but if you have any issues you can read it through. This might help you to solve common bugs or general mistakes. This chapter is sectioned in runtime tools, debugging and common mistake description.

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.You alternatively could close all the windows, this will stop your application too.

  • 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.

You can write a console log too if you want to know until which point your program is executed. Therefore use for different prioritys:

   1 rospy.loginfo("infomessage")
   2 rospy.logwarning("warningmessage")
   3 rospy.logerror("errormessage")

Common mistakes and bugs

  • If the program doesn't work as espected: Did you saved all the files?
  • If the program couldn't be found: Did you made the file executable? Is the path correct?
  • If the robot doesn't want to move: Are your coordinates unplausible?
  • If the program only drops errors: Is your synthax correct?

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. If you want to learn more commands or want to see what you can general do with the Python API, you should check out the pilz_robot_programming’s documentation.

Wiki: pilz_robots/Tutorials/ProgramRobotWithPythonAPI (last edited 2018-12-05 14:24:14 by JoGruner)