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)

_images/tfs.png
  • 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.

_images/move_ref.png

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.

  1. goal=Pose(position=Point(0, 0, 0.2))) or goal=Pose(position=Point(0, 0, 0.2)), reference_frame=”prbt_base”)
  2. goal=Pose(position=Point(0, 0, 0.2)), reference_frame=”prbt_tcp”)
  3. 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.

_images/move_rel.png

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

  1. goal=position=Point(0, 0, 0.2)), relative=True)
  2. goal=position=Point(0, 0, 0.2)), reference_frame=”prbt_tcp”, relative=True)
  3. 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.

_images/pick_place.png

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:

  1. close in to grab the object
  2. move straight up to lift it
  3. 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")

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.

API reference

API for easy usage of Pilz robot commands.

class pilz_robot_programming.robot.Robot(version=None, *args, **kwargs)

Bases: object

Main component of the API which allows the user to execute robot motion commands and pause, resume or stop the execution. The following commands are currently supported:

For a more detailed description of the individual commands please see the documentation of the corresponding command. Especially see the documentation of the pilz_trajectory_generation package to get more information on additional parameters that can be configured in the MoveIt! plugin.

The commands are executed with the help of Moveit.

Note:

To any given time only one instance of the Robot class is allowed to exist.

Note:

Before you create an instance of Robot, ensure that MoveIt is up and running, because the constructor blocks until all necessary connections to Moveit are established. Currently connections to the following topics have to be established before the function finishes:

  • move_group
  • sequence_move_group
Note:

Currently the API does not support creating a new instance of Robot after deleting an old one in the same program. However this can be realized by calling _release() before the deletion.

Parameters:

version – To ensure that always the correct API version is used, it is necessary to state which version of the API is expected. If the given version does not match the version of the underlying API then an exception is thrown. Only the major version number is considered.

Raises:
  • RobotVersionError – if the given version string does not match the module version.
  • RobotMultiInstancesError – if an instance of Robot class already exists.
get_current_joint_states(planning_group='manipulator')

Returns the current joint state values of the robot. :param planning_group: Name of the planning group, default value is “manipulator”. :return: Returns the current joint values as array :rtype: array of floats :raises RobotCurrentStateError if given planning group does not exist.

get_current_pose(target_link='prbt_tcp', base='prbt_base')

Returns the current pose of target link in the reference frame. :param target_link: Name of the target_link, default value is “prbt_tcp”. :param base: The target reference system of the pose, default ist “prbt_base”. :return: Returns the pose of the given frame :rtype: geometry_msgs.msg.Pose :raises RobotCurrentStateError if the pose of the given frame is not known

move(cmd)

Allows the user to start/execute robot motion commands.

The function blocks until the specified command is completely executed.

The commands are executed with the help of Moveit.

Note:

While move() is running no further calls to move() are allowed.

Parameters:

cmd

The robot motion command which has to be executed. The following commands are currently supported:

Raises:
  • RobotUnknownCommandType – if an unsupported command is passed to the function.
  • RobotMoveAlreadyRunningError – if a move command is already running.
  • RobotMoveFailed – if the execution of a move command fails. Due to the exception any thread or script calling the move() function ends immediately. An exception is thrown instead of returning an error to ensure that no further robot motion commands are executed.
stop()

The stop function allows the user to cancel the currently running robot motion command and . This is also true for a paused command.

Note:Function calls to move() and stop() have to be performed from different threads because move() blocks the calling thread. The move-thread is terminated. If no motion command is active, the stop-thread is terminated.
pause()

The pause function allows the user to stop the currently running robot motion command. The move() function then waits for resume. The motion can still be canceled using stop().

Note:Function calls to move() and pause() have to be performed from different threads because move() blocks the calling thread.
resume()

The function resumes a paused robot motion. If the motion command is not paused or no motion command is active, it has no effects.

Note:Function calls to move() and resume() have to be performed from different threads because move() blocks the calling thread.
__weakref__

list of weak references to the object (if defined)

API for easy usage of Pilz robot commands.

class pilz_robot_programming.commands.Ptp(vel_scale=1, acc_scale=None, *args, **kwargs)

Bases: pilz_robot_programming.commands._BaseCmd

Represents a single point-to-point (Ptp) command. A Ptp command allows the user to quickly move the robot from its current position to a specified point in space (goal). The trajectory taken to reach the goal is defined by the underlying planning algorithms and cannot not be defined by the user.

Parameters:
  • vel_scale

    The velocity scaling factor allows to limit the highest possible axis velocity. The velocity scaling factor is a scalar. The value is applied to all axes. The value is given in percentage of the maximal velocity of an axis and has to be in range: (0,1]. The allowed axis velocity for each axis is calculated as follows:

    allowed axis velocity = vel_scale * maximal axis velocity
  • acc_scale

    The acceleration scaling factor allows to limit the highest possible axis acceleration. The acceleration scaling factor is a scalar value. The value is applied to all axes. The value is given in percentage of the maximal acceleration of an axis and has to be in range: (0,1]. The allowed axis acceleration for each axis is calculated as follows:

    allowed axis acceleration = vel_scale * maximal axis acceleration

    If no acceleration scaling factor is given, the acceleration scaling factor is set as follows:

    acc_scale = vel_scale * vel_scale
class pilz_robot_programming.commands.Lin(vel_scale=0.1, acc_scale=None, *args, **kwargs)

Bases: pilz_robot_programming.commands._BaseCmd

Represents a linear command. A Lin command allows the user to move the robot from its current position to a specified point in space (goal). The trajectory taken to reach the goal is a straight line (in Cartesian space).

Parameters:
  • vel_scale

    The velocity scaling factor allows to limit the highest possible cartesian velocity of the TCP frame. The velocity scaling factor is a scalar value. The value is given in percentage of the maximal allowed cartesian velocity and has to be in range: (0,1]. The allowed cartesian velocity of the TCP frame is calculated as follows:

    allowed cartesian velocity = vel_scale * maximal cartesian velocity
  • acc_scale

    The acceleration scaling factor allows to limit the highest possible cartesian acceleration of the TCP frame. The acceleration scaling factor is a scalar value. The value is given in percentage of the maximal allowed cartesian acceleration and has to be in range: (0,1]. The allowed cartesian acceleration of the TCP frame is calculated as follows:

    allowed cartesian acceleration = acc_scale * maximal cartesian acceleration

    If no acceleration scaling factor is given, the acceleration scaling factor is set as follows:

    acc_scale = vel_scale
class pilz_robot_programming.commands.Circ(interim=None, center=None, vel_scale=0.1, acc_scale=None, *args, **kwargs)

Bases: pilz_robot_programming.commands._BaseCmd

Represents a circular command. A Circ command allows the user to move the robot from its current position to a specified point in space (goal). The trajectory taken to reach the goal represents a circle (in Cartesian space). The circle is defined by the current position of the robot, the specified interim/center point and the goal position.

Note:

The circle can be completely defined by stating a interim or an center position. However, only one of both should be stated.

Parameters:
  • interim – Position in cartesian space (geometry_msgs/Point), which lies on the circle on which the robot is supposed to move. The position has to lie between the current position of the robot and the goal position. The interim position indicates in which direction of the circle the robot is supposed to move.
  • center – The center point (stated in Cartesian space) of the circle on which the robot is supposed to move. If the center point is given, the robot moves in the direction of the smallest angle to the goal.
  • vel_scale

    The velocity scaling factor allows to limit the highest possible cartesian velocity of the TCP frame. The velocity scaling factor is a scalar value. The value is given in percentage of the maximal allowed cartesian velocity and has to be in range: (0,1]. The allowed cartesian velocity of the TCP frame is calculated as follows:

    allowed cartesian velocity = vel_scale * maximal cartesian velocity
  • acc_scale

    The acceleration scaling factor allows to limit the highest possible cartesian acceleration of the TCP frame. The acceleration scaling factor is a scalar value. The value is given in percentage of the maximal allowed cartesian acceleration and has to be in range: (0,1]. The allowed cartesian acceleration of the TCP frame is calculated as follows:

    allowed cartesian acceleration = acc_scale * maximal cartesian acceleration

    If no acceleration scaling factor is given, the acceleration scaling factor is set as follows:

    acc_scale = vel_scale
class pilz_robot_programming.commands.Sequence(*args, **kwargs)

Bases: pilz_robot_programming.commands._AbstractCmd

Represents an overall Sequence command. A Sequence consists of one or more robot motion commands. All commands in a sequence are planned first. After all commands in a sequence are planned, they are executed.

If the blending radius between two or more commands is greater than zero, the commands are blended together, in other words, the robot will not stop at the end of each command. To allow a smooth transition from one trajectory to the next (in case of blending), the original trajectories are altered slightly within the sphere defined by the blending radius.

Note:In case the blend radius is zero, the robot executes the robot motion commands as if they are sent separately.
Note:The robot always stops between gripper and non-gripper commands.
Note:Gripper commands cannot be blended together.
Note:In case the planning of a command in a sequence fails, non of the commands in the sequence are executed.
append(cmd, blend_radius=0)

Adds the given robot motion command to the sequence.

Parameters:
  • cmd (pilz_robot_programming.commands._BaseCmd) – The robot motion command which has to be added to the sequence. The blending happens between the specified command and the command following the specified command if a non-zero blend_radius is defined. Otherwise, if the blend radius is zero, the commands will execute consecutively. The blend radius preceding a gripper command is always ignored. The blend radius stated with a gripper command is also ignored.
  • blend_radius (float) – 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. The blending happens inside a sphere with a radius specified by the blending radius. When the trajectory leaves the sphere the trajectory is back on the original trajectory.
Note:

The last command of the sequence has to have zero blending radius which can be achieved by omitting the last blend radius argument.

class pilz_robot_programming.commands.Gripper(goal, vel_scale=0.1, *args, **kwargs)

Bases: pilz_robot_programming.commands._BaseCmd

Represents a gripper command to open and close the gripper. A gripper command allows the user to move the gripper finger to desired opening width.

Parameters:
  • goal – half of the opening width in meter (0 to 0.03m).
  • vel_scale

    The velocity scaling factor allows to limit the highest possible axis velocity. The velocity scaling factor is a scalar. The value is given in percentage of the maximal velocity of an axis and has to be in range: (0,1]. The allowed axis velocity for each axis is calculated as follows:

    allowed axis velocity = vel_scale * maximal axis velocity
pilz_robot_programming.commands.from_euler(a, b, c)

Convert euler angles into a geometry.msg.Quaternion.

Pass euler angles a, b, c in intrinsic ZYZ convention (in radians).

Use this function to fill pose values for Ptp / Lin commands:

r.move(Ptp(goal=Pose(position=Point(0.6, -0.3, 0.2), orientation=from_euler(0, pi, 0))))
Parameters:
  • a – rotates around the z-axis.
  • b – rotates around the new y-axis.
  • c – rotates around the new z-axis.
Note:

e.g. (0, pi, 0) orients the tool downwards, (pi/2., pi/2., 0) horizontal west

Indices and tables