• Diff for "pilz_robots/Tutorials/ProgramRobotWithPythonAPI"
Differences between revisions 118 and 119
Revision 118 as of 2018-12-04 07:10:50
Size: 20473
Editor: JoGruner
Comment:
Revision 119 as of 2018-12-04 07:18:19
Size: 20476
Editor: JoGruner
Comment:
Deletions are marked like this. Additions are marked like this.
Line 101: Line 101:
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 [[pilz_tutorials/pilz_tutorial_3/pilz_tutorial/scripts/emptyPythonTemplate.py|emptyPythonTemplate.py]]. 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 last tutorial [[pilz_robots/Tutorials/MoveRobotWithPilzCommand_planner|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 chapter '''Runtime tools and debugging''' 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.
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 [[pilz_tutorials/pilz_tutorial_3/pilz_tutorial/scripts/emptyPythonTemplate.py|!emptyPythonTemplate.py]]. This will help you to create your own script step by step.
Line 165: Line 152:


=== 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 [[pilz_robots/Tutorials/MoveRobotWithPilzCommand_planner|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 chapter '''Runtime tools and debugging''' 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.

(!) 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.

Program 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

Next Tutorial: Pick and Place application with the Pilz Manipulator Module PRBT and the Python API

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.

Toggle line numbers
   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.

Toggle line numbers
   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.

Toggle line numbers
   1 __REQUIRED_API_VERSION__ = "0"  #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.

Toggle line numbers
   1 #main program
   2 def start_program():
   3     pass

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

Toggle line numbers
   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. 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 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 values 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 values directly, open a new terminal and type: rostopic echo /joint_states

First Move

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

Toggle line numbers
   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 chapter Runtime tools and debugging 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.

Setup RViz for better visualisation

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.

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

Toggle line numbers
   1 def pick_and_place():
   2     """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:

Toggle line numbers
   1 def pick_and_place():
   2     """pick and place function"""
   3 
   4     #A static velocity from 0.2 is used
   5     #The position is given relative to the TCP.
   6     r.move(Ptp(goal=Pose(position=Point(0, 0, 0.1)), reference_frame="prbt_tcp", vel_scale=0.2))
   7     rospy.sleep(0.2)    #Pick or Place the PNOZ (close or open the gripper)
   8     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 all positions of the robot allows a down and up movement without a collision.

Toggle line numbers
   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 whole program now. So first write the positions in start_program() beneath the other position variables. You can use your own positions too if you want: 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 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: 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: GitHub/myFirstApplication.py, or simply copy the following lines to your script.

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

Runtime tools and debugging

At last we present you some helpful tips and tricks. Work this chapter through if you have had any problems to run or debug your program.

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.

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 NOT WORKING YET!pilz_robots_programming API documentation.

Wiki: pilz_robots/Tutorials/ProgramRobotWithPythonAPI (last edited 2021-05-10 13:24:31 by ImmanuelMartini)