【neotic moveit1】Pilz Industrial Motion Planner官方demo

安装package:

sudo apt install ros-$ROS_DISTRO-pilz-robotssudo apt install ros-$ROS_DISTRO-pilz-industrial-motion

 打开两个终端，分别运行：

roslaunch moveit_resources_prbt_moveit_config moveit_planning_execution.launch sim:=true pipeline:=pilz_industrial_motion_plannerrosrun pilz_robot_programming demo_program.py

demo_program.py源码：

from geometry_msgs.msg import Point
#首先我们导入机器人API。
from pilz_robot_programming import *
import math
import rospy
#API 版本参数确保使用正确的 API 版本。这可确保机器人按预期/预期运行。如果版本不匹配，则会引发异常。
__REQUIRED_API_VERSION__ = "1"
​
​
def start_program():
    print("Executing " + __file__)
    #在此创建机器人对象，随后用于移动机器人。
    r = Robot(__REQUIRED_API_VERSION__)
    #move() 函数是机器人 API 中最重要的部分。在 move() 函数的帮助下，用户可以执行不同的机器人运动命令，如 Ptp、Lin 和 Circ 所示。
    # 默认情况下，笛卡尔目标被解释为工具中心点 (TCP) 连杆的位姿。TCP连杆和最后一个机器人链接之间的转换可以通过prbt.xacro中的tcp_offset_xyz和tcp_offset_rpy参数进行调整。
    # Simple ptp movement 简单PTP运动到目标，速度因子0.4
    r.move(Ptp(goal=[0, 0.5, 0.5, 0, 0, 0], vel_scale=0.4))
    #获取当前关节位置
    start_joint_values = r.get_current_joint_states()
    #Ptp 和 Lin 命令的目标姿势可以在关节空间或笛卡尔空间中说明。
    #Circ 命令的目标和辅助位姿必须在笛卡尔空间中说明。
    # Relative ptp movement 相对PTP运动
    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))
    #Ptp 和 Lin 命令也可以表示为由参数 relative=True 指示的相对命令。
    # 相对命令将目标声明为相对于当前机器人位置的偏移量。只要没有设置
    #自定义参考系，就必须相对于基础坐标系说明偏移量。方向添加为欧拉角的偏移量。
    # 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.1))
    # 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.1))
    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)), acc_scale=0.05), blend_radius=0.01)
    blend_sequence.append(Lin(goal=Pose(position=Point(0.2, 0.1, 0.7)), acc_scale=0.05))
​
    r.move(blend_sequence)
    
    #对于所有三个移动类 Ptp、Lin 和 Circ，您可以定义自定义参考系。
    #除了额外的参数 reference_frame 之外，还支持传递 PoseStamped 
    #并将 Header 设置为任何有效的 tf2 frame_id。传递的目标是相对于
    # 给定坐标系而不是默认系统 prbt_base 进行解释的。
 
    #自定义参考坐标系参数 (reference_frame="target_frame") 必须是有效
    # 的 tf 框架 ID，并且可以与相关命令配对。与相对标志配对时，目标将应用于此自定义参考系中的当前机器人姿势。
    # Move with custom reference frame 移动到自定义坐标系框架
    r.move(Ptp(goal=PoseStamped(header=Header(frame_id="prbt_tcp"),
                                pose=Pose(position=Point(0, 0, 0.1)))))
    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
  #创建并执行无效的（关节值越界） ptp 命令，
  #如果机器人运动命令在执行过程中失败，move() 函数会抛出一个 RobotMoveFailed 异常，可以使用标准的 Python 机制捕获该异常。
    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__":
    # 此代码片段初始化 ROS。
    rospy.init_node('robot_program_node')
​
    start_program()

 pilz_robot_programming 文档！

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

pilz_robot_programming 包为用户提供了一个易于使用的 API 来移动 MoveIt！启用的机器人。它的目标是使用 MoveIt! 的 pilz_industrial_motion_planner::CommandPlanner 插件执行标准工业机器人命令，如 Ptp、Lin 和 Circ。它还为用户提供了执行命令序列（称为序列）的可能性。最重要的是，机器人运动可以暂停、恢复和停止。

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, it merely requires the availability of the service /get_speed_override for obtaining the speed override of the robot system.

所有示例都是针对 PRBT 机器人给出的，但 API 的通用性足以与任何具有 MoveIt 的机器人一起使用！配置，它只需要服务 /get_speed_override 的可用性来获取机器人系统的速度覆盖。

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

机器人 API 与 moveit_commander 包有一些相似之处，但不同之处在于其专门用于由 pilz_industrial_motion_planner MoveIt 执行的经典工业机器人命令！插入。机器人 API 连接到 MoveIt！使用序列功能实现的标准 move_group 操作接口和自定义 sequence_move_group 操作。

有关工业轨迹生成参数的更多详细信息，请参阅包 pilz_industrial_motion_planner。

更详细的解释：

假设我们的应用程序中有三个坐标系。（以绿线和蓝线显示）

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

然后我们有三个可能的框架，我们可以选择执行我们的目标。

在上图中，我们显示了三个移动命令。所有三个命令都将机器人移动到位置 x = y = 0 和 z = 0.2，但使用不同的框架作为参考。

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”)

额外添加相关标志时，目标将使用所选坐标系添加到当前 tcp 位姿。。这导致 tcp 根据我们使用的坐标系向不同方向移动。

在这种情况下，我们只是在之前的目标中添加了相对标志。

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)

从上面可以看出，相对运动使用了所选参考系的 z 轴，这导致了 tcp 的不同运动，除了 tcp 框架本身。在 tcp 框架的情况下，相对运动和绝对运动是相同的。

使用示例

为了展示如何以及何时使用这个选项，我们来看一个小例子。

该图像应该显示对刚性对象的一系列拾取操作。产品放置在产品托盘上，因此相对于托盘参考系具有固定位置。

为了使机器人处于与此图像中机器人相似的位置，我们可以使用带有自定义 reference_frame 的移动命令。

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

这将导致一个场景，看起来有点像上图。（需要绕x轴旋转才能达到当前tcp旋转）

序列中的下一个命令将是：

1. 靠近以抓住物体

2. 直接向上举起它

3. 移动到下一个对象

对于第一个任务，我们可以轻松地使用 tcp ref，因为它的旋转已经符合我们的目标。

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

第二个命令 - 提升物体 - 最好通过使用托盘框架的相对运动来实现。（在这种情况下，我们也可以使用全局系统，但是当托盘倾斜时，如上图所示，这样做可能会有问题。）

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

对于第三次移动，我们应该再次使用“托盘”参考系中的相对移动。

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

如果我们想将产品放在其他地方，在移动到下一个对象之前，我们将改为在托盘参考框架中使用绝对命令，或者为托盘上的每个对象添加新框架，并针对每个对象执行与其框架相关的所有操作。

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)

要连接多个轨迹并一次规划轨迹，您可以使用 Sequence 命令。

作为可选参数，可以为 Sequence 命令指定混合半径。融合半径表示机器人轨迹可以偏离原始轨迹（没有融合的轨迹）多少，以将机器人运动从一个轨迹融合到下一个轨迹。将混合半径设置为零对应于没有像上面那样混合的序列。如果给定的混合半径大于零，机器人将从一个轨迹移动到下一个轨迹，而不会停止。

# 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)

注意：序列的最后一个命令必须具有零混合半径，这可以通过省略混合半径参数来实现。

注意：机器人总是在夹持器和非夹持器命令之间停止。

注意：Gripper 命令不能混合在一起。

Gripper夹爪：

如果启动文件是用真实机器人启动的，并且参数为夹持器：= pg70，夹持器可以通过以下方式打开或关闭：

r.move(Gripper(gripper_pos=0.02))

将 Gripper_pos 参数设置为以米为单位的距离。PG+70 夹爪的两个夹爪手指移动相同的距离，因此夹爪张开程度是指定的两倍。

您还可以将 Gripper 附加到序列。

Current TCP pose and current joint values当前TCP位姿和当前关节值：

start_joint_values = r.get_current_joint_states()

pose_after_relative = r.get_current_pose()

API 提供了允许用户确定机器人当前关节值和当前 TCP 位姿的函数。这两个函数的返回值可以直接用于创建新的运动命令：

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

r.move(Ptp(goal=start_joint_values))

函数 get_current_pose 还可以返回相对于另一坐标系的当前姿势。为此，请将基本参数设置为相应的参考系。

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

Brake Test制动测试：

方法 is_brake_test_required() 将检查机器人是否需要执行制动测试。所以把它放在你的程序中的某个地方，以便反复检查。方法 execute_brake_test() 执行制动测试并抛出异常，如果它失败。

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移动控制命令

用户可以调用服务以控制机器人的运动。可以通过键入以下内容暂停正在运行的程序

rosservice call pause_movement

如果机器人当前正在移动，则停止。暂停的执行可以通过

rosservice call resume_movement

这也会从停止的位置恢复上一次机器人运动。没有暂停的恢复订单没有效果。还存在使用中止程序的可能性

rosservice call stop_movement

Multithreading多线程：

当 move() 在单独的线程中运行时，可以通过机器人对象的以下方法直接发出移动控制命令：

r.pause()

r.resume()

r.stop()

在这种情况下，stop() 只结束移动线程。

API reference

用于轻松使用 Pilz 机器人命令的 API。

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

• Bases: object

API 的主要组件，允许用户执行机器人运动命令以及暂停、恢复或停止执行。目前支持以下命令：

• Ptp

• Lin

• Circ

• Sequence

• Gripper

有关各个命令的更详细说明，请参阅相应命令的文档。尤其是查看 pilz_industrial_motion_planner 包的文档以获取有关可以在 MoveIt！插件 中配置的其他参数的更多信息！ 

命令是在 Moveit 的帮助下执行的。

Note：

在任何给定时间内，仅允许存在一个 Robot 类实例。

Note：

在创建 Robot 实例之前，请确保 MoveIt 已启动并正在运行，因为在与 Moveit 的所有必要连接建立之前，构造函数会阻塞。目前，必须在功能完成之前建立与以下主题的连接：

• • move_group

• • sequence_move_group

Note：

目前API不支持在同一程序中删除旧的Robot实例后创建新的Robot实例。然而，这可以通过在删除之前调用 _release() 来实现。

Parameters：

version – 为确保始终使用正确的 API 版本，有必要说明预期的 API 版本。如果给定的版本与底层 API 的版本不匹配，则会引发异常。只考虑主要版本号。

Raises：

RobotVersionError – 如果给定的版本字符串与模块版本不匹配。

RobotMultiInstancesError – 如果 Robot 类的实例已经存在。

get_planning_frame()

获取机器人正在规划的框架的名称。

get_active_joints(planning_group)

返回包含在指定规划组中的关节

get_current_joint_states(planning_group='manipulator')

返回机器人当前的关节状态值。:param Planning_group：计划组的名称，默认值为“manipulator”。:return: 以数组形式返回当前关节值 :rtype: 浮点数组 :raise RobotCurrentStateError 如果给定的计划组不存在。

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

返回参考系中目标链接的当前标记姿势。:param target_link：target_link 的名称，默认值为“prbt_tcp”。:param base: 姿势的目标参考系统，默认为“prbt_base”。:return: 返回给定帧的标记姿势 :rtype: geometry_msgs.msg.PoseStamped :如果给定帧的姿势未知，则引发 RobotCurrentStateError

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

返回参考系中目标链接的当前位姿。:param target_link：target_link 的名称，默认值为“prbt_tcp”。:param base: 姿势的目标参考系统，默认为“prbt_base”。:return: 返回给定帧的位姿 :rtype: geometry_msgs.msg.Pose :raise RobotCurrentStateError 如果给定帧的位姿未知

Move（cmd）

允许用户启动/执行机器人运动命令。

函数阻塞直到指定的命令被完全执行。

命令是在 Moveit 的帮助下执行的。

stop（）

停止功能允许用户取消当前正在运行的机器人运动命令和。对于暂停的命令也是如此。

pause（）

暂停功能允许用户停止当前运行的机器人运动命令。move() 函数然后等待恢复。仍然可以使用 stop() 取消运动。

resume（）

该功能恢复暂停的机器人运动。如果运动命令未暂停或没有运动命令处于活动状态，则不会产生任何影响。

is_brake_test_required()

检查当前是否需要进行制动测试。

execute_brake_test()

执行制动测试。如果成功，函数无异常退出。

__weakref__

对对象的弱引用列表（如果已定义）

用于轻松使用 Pilz 机器人命令的 API。

• class pilz_robot_programming.commands.BaseCmd(goal=None, planning_group='manipulator', target_link='prbt_tcp', vel_scale=0.1, acc_scale=0.1, relative=False, reference_frame='prbt_base', *args, **kwargs)

• Bases: pilz_robot_programming.commands._AbstractCmd

所有单个命令的基类。

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

• Bases: pilz_robot_programming.commands.BaseCmd

表示单个点对点 (Ptp) 命令。Ptp 命令允许用户快速将机器人从其当前位置移动到空间中的指定点（目标）。达到目标的轨迹由底层规划算法定义，不能由用户定义。

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

• Bases: pilz_robot_programming.commands.BaseCmd

代表一个线性命令。Lin 命令允许用户将机器人从其当前位置移动到空间中的指定点（目标）。到达目标的轨迹是一条直线（在笛卡尔空间中）。

• 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

代表一个循环命令。Circ 命令允许用户将机器人从其当前位置移动到空间中的指定点（目标）。到达目标的轨迹代表一个圆（在笛卡尔空间中）。圆由机器人的当前位置、指定的中间点/中心点和目标位置定义。

• class pilz_robot_programming.commands.Sequence(*args, **kwargs)

• Bases: pilz_robot_programming.commands._AbstractCmd

代表一个整体的序列命令。一个序列由一个或多个机器人运动命令组成。序列中的所有命令都是首先计划的。在计划好一个序列中的所有命令之后，它们就会被执行。

如果两个或多个命令之间的融合半径大于零，则将这些命令融合在一起，即机器人不会在每个命令结束时停止。为了允许从一个轨迹到下一个轨迹的平滑过渡（在混合的情况下），原始轨迹在由混合半径定义的球体内略有改变。

  append(cmd, blend_radius=0)

    将给定的机器人运动命令添加到序列中。

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

• Bases: pilz_robot_programming.commands.BaseCmd

表示用于打开和关闭夹具的夹具命令。夹持器命令允许用户将夹持器手指移动到所需的开口宽度。

    pilz_robot_programming.commands.from_euler(a, b, c)

将欧拉角转换为 geometry.msg.Quaternion。

以固有的 ZYZ 约定（以弧度为单位）传递欧拉角 a、b、c。

使用此函数来填充 Ptp / Lin 命令的位姿值：

r.move(Ptp(goal=Pose(position=Point(0.6, -0.3, 0.2), orientation=from_euler(0, pi, 0))))

参考：

https://docs.ros.org/en/melodic/api/pilz_robot_programming/html/

https://github.com/PilzDE/pilz_industrial_motion/tree/kinetic-devel

https://github.com/PilzDE/pilz_robots/tree/melodic-devel

https://github.com/PilzDE/pilz_industrial_motion/blob/kinetic-devel/pilz_robot_programming/Readme.rst