Welcome to pilz_robot_programming’s documentation!¶

The pilz_robot_programming package provides the user with an easy to use API to move a MoveIt! enabled robot. It’s target is to execute standard industrial robot commands like Ptp, Lin and Circ using the pilz::CommandPlanner plugin for MoveIt!. It also provides the user with the possibility to execute command sequences (called Sequence). On top of that, the robot movement can be paused, resumed and stopped.

All examples are given for a PRBT robot but the API is general enough to be used with any robot that has a MoveIt! configuration.

The robot API has some similarity to the moveit_commander package but differs in its specialization for classical industrial robot commands to be executed by the pilz_command_planner MoveIt! plugin. The robot API connects to MoveIt! using the standard move_group action interface and the custom sequence_move_group action, that the sequence capability implements.

See the package pilz_trajectory_generation for more details about the parameters for industrial trajectory generation.

A simple demo program¶

To run the demo program it is first necessary to startup the simulated or the real robot. Afterwards, you can execute the demo program by typing:

$ rosrun pilz_robot_programming demo_program.py

The code `demo_program.py`¶

#!/usr/bin/env python
# Copyright (c) 2018 Pilz GmbH & Co. KG
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU Lesser General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU Lesser General Public License for more details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with this program.  If not, see <http://www.gnu.org/licenses/>.

from geometry_msgs.msg import Point
from pilz_robot_programming.robot import *
from pilz_robot_programming.commands import *
import math
import rospy

__REQUIRED_API_VERSION__ = "1"


def start_program():
    print("Executing " + __file__)

    r = Robot(__REQUIRED_API_VERSION__)

    # Simple ptp movement
    r.move(Ptp(goal=[0, 0.5, 0.5, 0, 0, 0], vel_scale=0.4))

    start_joint_values = r.get_current_joint_states()

    # Relative ptp movement
    r.move(Ptp(goal=[0.1, 0, 0, 0, 0, 0], relative=True, vel_scale=0.2))
    r.move(Ptp(goal=Pose(position=Point(0, 0, -0.1)), relative=True))
    r.move(Ptp(goal=[-0.2, 0, 0, 0, 0, 0], relative=True, acc_scale=0.2))

    pose_after_relative = r.get_current_pose()

    # Simple Lin movement
    r.move(Lin(goal=Pose(position=Point(0.2, 0, 0.8)), vel_scale=0.1, acc_scale=0.1))

    # Relative Lin movement
    r.move(Lin(goal=Pose(position=Point(0, -0.2, 0), orientation=from_euler(0, 0, math.radians(15))), relative=True,
           vel_scale=0.1, acc_scale=0.1))
    r.move(Lin(goal=Pose(position=Point(0, 0.2, 0)), relative=True,
           vel_scale=0.1, acc_scale=0.1))

    # Circ movement
    r.move(Circ(goal=Pose(position=Point(0.2, -0.2, 0.8)), center=Point(0.1, -0.1, 0.8), acc_scale=0.4))

    # Move robot with stored pose
    r.move(Ptp(goal=pose_after_relative, vel_scale=0.2))

    # Repeat the previous steps with a sequence command
    sequence = Sequence()
    sequence.append(Lin(goal=Pose(position=Point(0.2, 0, 0.8)), vel_scale=0.1, acc_scale=0.1))
    sequence.append(Circ(goal=Pose(position=Point(0.2, -0.2, 0.8)), center=Point(0.1, -0.1, 0.8), acc_scale=0.4))
    sequence.append(Ptp(goal=pose_after_relative, vel_scale=0.2))

    r.move(sequence)

    # Move to start goal for sequence demonstration
    r.move(Ptp(goal=start_joint_values))

    # Blend sequence
    blend_sequence = Sequence()
    blend_sequence.append(Lin(goal=Pose(position=Point(0.2, 0, 0.7))), blend_radius=0.01)
    blend_sequence.append(Lin(goal=Pose(position=Point(0.2, 0.1, 0.7))))

    r.move(blend_sequence)

    # Move with custom reference frame
    r.move(Ptp(goal=Pose(position=Point(0, 0, 0.1)), reference_frame="prbt_tcp"))
    r.move(Ptp(goal=Pose(position=Point(0, -0.1, 0)), reference_frame="prbt_link_3", relative=True))

    # Create and execute an invalid ptp command with out of bound joint values
    try:
        r.move(Ptp(goal=[0, 10.0, 0, 0, 0, 0]))
    except RobotMoveFailed:
        rospy.loginfo("Ptp command did fail as expected.")


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

    start_program()

The code explained¶

In this section parts of the demo program are explained to give a better understanding how the robot API is used.

note:	The chosen code snippets are not necessarily in order.

Robot creation¶

from pilz_robot_programming.robot import *

At first we import the robot API.

rospy.init_node('robot_program_node')

This code snippet initializes ROS.

r = Robot(__REQUIRED_API_VERSION__)

Here the robot object is created, which, subsequently, is used to move the robot.

The API version argument ensures, that the correct API version is used. This makes sure, that the robot behaves as expected/intended. In case the versions do not match, an exception is thrown.

note:	For the API version check only the major version number is relevant.

Move¶

r.move(Ptp(goal=[0, 0.5, 0.5, 0, 0, 0]))

r.move(Lin(goal=Pose(position=Point(0.2, 0, 0.8))))

r.move(Circ(goal=Pose(position=Point(0.2, 0.2, 0.8)), center=Point(0.3, 0.1, 0.8)))

The move() function is the most important part of the robot API. With the help of the move() function the user can execute the different robot motion commands, like shown for Ptp, Lin and Circ.

All cartesian goals are interpreted as poses of the tool center point (TCP) link. The transformation between the TCP link and the last robot link can be adjusted through the tcp_offset_xyz and tcp_offset_rpy parameters in prbt.xacro.

Move failure¶

try:
    r.move(Ptp(goal=[0, 10.0, 0, 0, 0, 0]))
except RobotMoveFailed:
    rospy.loginfo("Ptp command did fail as expected.")

In case a robot motion command fails during the execution, the move() function throws an RobotMoveFailed exception which can be caught using standard python mechanisms.

The goal: Joint vs. Cartesian space¶

r.move(Ptp(goal=[0, 0.5, 0.5, 0, 0, 0]))

r.move(Lin(goal=Pose(position=Point(-0.2, -0.2, 0.6), orientation=from_euler(0.1, 0, 0))))

The goal pose for Ptp and Lin commands can be stated either in joint space or in Cartesian space.

r.move(Circ(goal=Pose(position=Point(0.2, 0.2, 0.8)), center=Point(0.3, 0.1, 0.8)))

The goal and the auxiliary pose of Circ commands have to be stated in Cartesian space.

Relative commands¶

r.move(Ptp(goal=[0.1, 0, 0, 0, 0, 0], relative=True))

r.move(Lin(goal=Pose(position=Point(0, -0.2, -0.2)), relative=True))

Ptp and Lin commands can also be stated as relative commands indicated by the argument relative=True. Relative commands state the goal as offset relative to the current robot position. As long as no custom reference frame is set, the offset has to be stated with regard to the base coordinate system. The orientation is added as offset to the euler-angles.

Custom Reference Frame¶

r.move(Ptp(goal=Pose(position=Point(0, 0, 0.1)), reference_frame="prbt_tcp"))

r.move(Ptp(goal=Pose(position=Point(0, -0.1, 0)), reference_frame="prbt_link_3", relative=True))

All three move classes Ptp, Lin and Circ can be executed within a custom reference frame. In this case, the passed goal pose will be seen relative to this coordinate system instead of the default system: prbt_base

The custom reference frame argument (reference_frame="target_frame") has to be a valid tf frame id and can be paired with the relative command. When paired with relative flag, the goal will be applied to the current robot pose in this custom reference frame.

note:	Further information on tf is available on http://wiki.ros.org/tf (e.g. on how to create custom frames: section 6.3).

More Detailed Explanation¶

Let’s assume we have three coordinate systems in our application. (displayed with a green and blue line)

prbt_base is the default coordinate system. It was used in the previous sections.
The prbt_tcp frame is the current position of the gripper.
The third frame pallet is supposed to be an edge of an product tray, that we placed somewhere in the robot environment.

We then have three possible frames, we can choose to execute our goal in.

In the image above we displayed three move commands. All three commands move the robot to position x = y = 0 and z = 0.2, but use the different frames as reference.

goal=Pose(position=Point(0, 0, 0.2))) or goal=Pose(position=Point(0, 0, 0.2)), reference_frame=”prbt_base”)
goal=Pose(position=Point(0, 0, 0.2)), reference_frame=”prbt_tcp”)
goal=Pose(position=Point(0, 0, 0.2)), reference_frame=”pallet”)

When adding the relative flag additionally, the goal will be added to the current tcp pose using the chosen frame. This results in the tcp moving in different directions depending on which frame we used.

In this case we just added the relative flag to the previous goals.

goal=position=Point(0, 0, 0.2)), relative=True)
goal=position=Point(0, 0, 0.2)), reference_frame=”prbt_tcp”, relative=True)
goal=position=Point(0, 0, 0.2)), reference_frame=”pallet”, relative=True)

As can be seen above, the relative movement used the z axis of the choosen reference frame, which resulted in different movements of the tcp, except for the tcp frame itself. In the case of the tcp frame, relative and absolut movement is the same.

Example of Usage¶

To display how and when to use this options we take a look on a small example.

This image is supposed to display a series of pick operations on a rigid object. The products are placed on a product tray, thus having a fixed position relative to the pallet reference frame.

To get the robot in a position similar to the robot in this image we could use a move command with a custom reference_frame.

r.move(Ptp(goal=[0.1, -0.05, 0.2, 2.3561, 0, 0], reference_frame="pallet"))

This would result in a scene, that looks somewhat like the image above. (The Rotation around the x axis is necessary to reach the current tcp rotation)

The next commands in the sequence will be:

close in to grab the object
move straight up to lift it
move to the next object

For the first task we can easily use the tcp ref, since its rotation already fits our goal.

r.move(Ptp(goal=position=Point(0, 0, 0.1)), reference_frame="prbt_tcp"))

The second command - lifting the object - is best achieved by using an relative movement to the pallet frame. (We could as well use the global system in this case, but when the tray is tilted, like in the images above it could be problematic to do so.)

r.move(Ptp(goal=position=Point(0, 0, 0.1)), relative= True, reference_frame="pallet"))

For the third move we again should use the relative move in the “pallet” reference frame.

r.move(Ptp(goal=position=Point(0, -0.1, 0)), relative= True, reference_frame="pallet"))

In case we want to place the product somewhere else, previous to moving to the next object, we would instead use the absolute command in the pallet reference frame or add new frames for each object on the tray and do all operations for each object relative its frame.

Sequence¶

# Repeat the previous steps with a sequence command
sequence = Sequence()
sequence.append(Lin(goal=Pose(position=Point(0.2, 0, 0.8)), vel_scale=0.1, acc_scale=0.1))
sequence.append(Circ(goal=Pose(position=Point(0.2, -0.2, 0.8)), center=Point(0.1, -0.1, 0.8), acc_scale=0.4))
sequence.append(Ptp(goal=pose_after_relative, vel_scale=0.2))

r.move(sequence)

To concatenate multiple trajectories and plan the trajectory at once, you can use the Sequence command.

note:	In case the planning of a command in a `Sequence` fails, non of the commands in the `Sequence` are executed.

As an optional argument, a blending radius can be given to the Sequence command. The blending radius states how much the robot trajectory can deviate from the original trajectory (trajectory without blending) to blend the robot motion from one trajectory to the next. Setting the blending radius to zero corresponds to a Sequence without blending like above. If a blending radius greaten than zero is given, the robot will move from one trajectory to the next without stopping.

# Blend sequence
blend_sequence = Sequence()
blend_sequence.append(Lin(goal=Pose(position=Point(0.2, 0, 0.6))), blend_radius=0.01)
blend_sequence.append(Lin(goal=Pose(position=Point(0.2, 0, 0.7))))

r.move(blend_sequence)

note:	The last command of the sequence has to have zero blending radius which can be achieved by omitting the blend radius argument.
note:	The robot always stops between gripper and non-gripper commands.
note:	Gripper commands cannot be blended together.

Gripper¶

If the launch file is started with the real robot and the argument gripper:=pg70, the gripper can be opened or closed via:

r.move(Gripper(gripper_pos=0.02))

Set the gripper_pos argument to a distance in meters. Both gripper fingers of the PG+70 gripper move by the same distance so the gripper is twice as open as specified.

You can also append a Gripper to a Sequence.

Current TCP pose and current joint values¶

start_joint_values = r.get_current_joint_states()

pose_after_relative = r.get_current_pose()

The API provides functions which allow the user to determine the current joint values of the robot and the current TCP pose. The return value of both functions can directly be used to create new motion commands:

r.move(Ptp(goal=pose_after_relative, vel_scale=0.2))

r.move(Ptp(goal=start_joint_values))

The function get_current_pose can also return the current pose in respect to another frame. To do this, set the base argument, to the corresponding reference frame.

tcp_pose_in_tf = r.get_current_pose(base="target_frame")

Brake Test¶

The method is_brake_test_required() will check whether the robot needs to perform a brake test. So place it in your program somewhere such that it is checked repeatedly. The method execute_brake_test() executes the brake test and throws an exception, should it fail.

if r.is_brake_test_required():
    try:
        r.execute_brake_test()
    except RobotBrakeTestException as e:
        rospy.logerr(e)
    except rospy.ROSException as e:
        rospy.logerr("failed to call the service")

Move control orders¶

The user can make service calls in order to control the movement of the robot. A running program can be paused by typing

rosservice call pause_movement

If the robot is currently moving, it is stopped. A paused execution can be resumed via

rosservice call resume_movement

This also resumes the last robot movement from where it stopped. A resume order without preceding pause has no effects. There also exists the possibility to abort the program using

rosservice call stop_movement

Multithreading¶

When move() is running in a separate thread, the move control orders can be issued directly via the following methods of the robot object:

r.pause()

r.resume()

r.stop()

In this case stop() only ends the move-thread.

Table Of Contents

This Page

Welcome to pilz_robot_programming’s documentation!¶

A simple demo program¶

The code `demo_program.py`¶

The code explained¶

Robot creation¶

Move¶

Move failure¶

The goal: Joint vs. Cartesian space¶

Relative commands¶

Custom Reference Frame¶

More Detailed Explanation¶

Example of Usage¶

Sequence¶

Gripper¶

Current TCP pose and current joint values¶

Brake Test¶

Move control orders¶

Multithreading¶

API reference¶

Indices and tables¶

Welcome to pilz_robot_programming’s documentation!¶

A simple demo program¶

The code demo_program.py¶

The code explained¶

Robot creation¶

Move¶

Move failure¶

The goal: Joint vs. Cartesian space¶

Relative commands¶

Custom Reference Frame¶

More Detailed Explanation¶

Example of Usage¶

Sequence¶

Gripper¶

Current TCP pose and current joint values¶

Brake Test¶

Move control orders¶

Multithreading¶

API reference¶

Indices and tables¶

The code `demo_program.py`¶