pilz运动规划demo
pilz_industrial_motion_planner 提供了一个轨迹生成器,用于通过 MoveIt PlannerManager 插件的接口来规划标准机器人运动,如 PTP、LIN、CIRC。
请注意,这些规划器只是运动生成器,即它们不考虑避障。 计算预期轨迹(笛卡尔空间中的线性或圆形,或 PTP)并最终仅测试碰撞的有效性。 如果发生碰撞,则拒绝整个轨迹。
一、 User Interface MoveGroup 用户接口
这个包实现了 MoveIt 的 Planning_interface::PlannerManager 接口。 通过加载相应的规划管道(*_moveit_config 包中的 pilz_industrial_motion_planner_planning_pipeline.launch.xml),可以通过 move_group 节点提供的用户接口(c++、python 或 rviz)访问轨迹生成功能,例如 /plan_kinematics_path 服务和 /move_group 动作。 详细的使用教程请参考Move Group C++ Interface。
Move Group C++ Interface — moveit_tutorials Noetic documentation (ros-planning.github.io)
二、 Joint Limits 关节极限
对于所有命令,规划器planner使用来自参数服务器的最大速度和加速度。 使用 MoveIt 设置助手,文件 joint_limits.yaml 会自动生成并使用适当的默认值并在启动期间加载。
参数服务器上的limits会覆盖 URDF 机器人描述中的limits。 请注意,虽然在 URDF 和参数服务器中都可以设置位置限制和速度限制,但只能通过参数服务器设置加速度限制。 作为对常见 has_acceleration 和 max_acceleration 参数的扩展,我们添加了设置 has_deceleration 和 max_deceleration(<0.0) 的能力。
limits合并的前提是来自参数服务器的限制必须更严格或至少等于 URDF 中设置的参数。
目前,计算出的轨迹将通过使用所有限制的最严格组合作为所有关节的公共限制来遵守限制。
三、 Cartesian Limits 笛卡尔极限
对于笛卡尔轨迹生成 (LIN/CIRC),规划器需要有关 3D 笛卡尔空间中最大速度的信息。 即平移/旋转速度/加速度/减速度需要在参数服务器上设置如下:
cartesian_limits:
max_trans_vel: 1
max_trans_acc: 2.25
max_trans_dec: -5
max_rot_vel: 1.57
对于梯形速度,规划器假定平移和旋转具有相同的加速比。 所以旋转加速度计算为 max\_trans\_acc / max\_trans\_vel \* max\_rot\_vel (以及相应的减速)。
四、 Planning Interface 规划接口
这个包使用 moveit_msgs::MotionPlanRequest 和 moveit_msgs::MotionPlanResponse 作为运动规划的输入和输出。 每个规划算法所需的参数解释如下。
有关如何填充 MotionPlanRequest 的一般介绍,请参阅规划pose goal。Move Group C++ Interface — moveit_tutorials Noetic documentation (ros-planning.github.io)
您可以指定“PTP”、“LIN”或“CIRC”作为 planner\_id``of
the
``MotionPlanRequest
五、 The PTP motion command 点到点运动命令
该规划器生成具有梯形关节速度曲线的完全同步的点对点轨迹。 假设所有关节具有相同的最大关节速度/加速度/减速度限制。 如果不是,则采用最严格的限制。 选择到达目标时间最长的轴作为引导轴。 其他轴减速,以便它们与引导轴共享相同的加速/等速/减速阶段。
请求:Input parameters in moveit_msgs::MotionPlanRequest
- planner_id:PTP (规划器id)
- group_name:规划组名称
- max_velocity_scaling_factor:最大关节速度的比例因子
- max_acceleration_scaling_factor:最大关节加速/减速的比例因子
- start_state/joint_state/(name、position and velocity:开始状态的关节名称/位置/速度(可选)。
- goal_constraints(目标 可以在关节空间或笛卡尔空间中给出)
- for a goal in joint space
goal_constraints/joint_constraints/joint_name:目标关节名称
goal_constraints/joint_constraints/position:目标关节位置
8. for a goal in Cartesian space
- goal_constraints/position_constraints/header/frame_id: 这个数据关联的坐标系
- goal_constraints/position_constraints/link_name: 目标连杆名
- goal_constraints/position_constraints/constraint_region: 目标点的包围体积
- goal_constraints/position_constraints/target_point_offset: (在连杆坐标系上) 目标连杆上目标点的偏移量(optional)
结果:Planning results in moveit_msg::MotionPlanResponse
- trajectory_start: 规划轨迹的第一个机器人状态
- trajectory/joint_trajectory/joint_names: 生成的关节轨迹的关节名称列表
- trajectory/joint_trajectory/points/(positions,velocities,accelerations,time_from_start): 生成的航点列表。 每个点都有所有关节的位置/速度/加速度(与关节名称的顺序相同)和从开始的时间。 最后一点的速度和加速度为零。.
- group_name: 规划组的名称
- error_code/val:运动规划的错误代码
六、 The LIN motion command 直线运动命令
该规划器在目标和开始姿势之间生成线性笛卡尔轨迹。 规划器使用笛卡尔极限在笛卡尔空间中生成梯形速度剖面。 平移运动是起点和目标位置矢量之间的线性插值。 旋转运动是开始和目标方向之间的四元数 slerp。 平移和旋转运动在时间上是同步的。 这个规划器只接受零速度的开始状态。 规划结果是一个关节轨迹。 如果运动规划由于违反关节空间限制而失败,则用户需要调整笛卡尔速度/加速度比例因子。
请求:Input parameters in moveit_msgs::MotionPlanRequest
- planner_id: LIN 规划器索引
- group_name: 规划组名称
- max_velocity_scaling_factor: 最大笛卡尔平移、旋转速度的缩放因子
- max_acceleration_scaling_factor: 最大笛卡尔平移/旋转加速/减速的比例因子
- start_state/joint_state/(name, position and velocity: 关节名称/起始状态的位置
- goal_constraints (目标可以在关节空间或笛卡尔空间中给出)
- 对于关节空间目标
- goal_constraints/joint_constraints/position:目标关节位
- goal_constraints/joint_constraints/joint_name: 目标关节名称
- 对于笛卡尔空间目标
- goal_constraints/position_constraints/target_point_offset: (在连杆坐标系上)目标连杆上目标点的偏移量 (optional)
- goal_constraints/position_constraints/constraint_region: 目标点的包围体积
- goal_constraints/position_constraints/link_name: 目标连杆名称
- goal_constraints/position_constraints/header/frame_id: 数据关联的坐标系
- 对于关节空间目标
结果:Planning results in moveit_msg::MotionPlanResponse
- trajectory_start: 规划轨迹的第一个机器人状态
- trajectory/joint_trajectory/joint_names: 生成的关节轨迹的关节名称列表
- trajectory/joint_trajectory/points/(positions,velocities,accelerations,time_from_start):生成的航点列表。每个点都有所有关节的位置/速度/加速度(与关节名称的顺序相同)和从开始的时间。最后一点的速度和加速度为零
- group_name: 规划组的名称
- error_code/val: 运动规划的错误代码。
七、 The CIRC motion command 圆弧运动命令
该规划器在目标和开始姿势之间的笛卡尔空间中生成圆弧轨迹。 提供路径约束有两种选择:
- the center point of the circle: 规划器总是在起点和目标之间生成较短的弧,不能生成半圆,,
- an interim point on the arc: 生成的轨迹总是经过中间点。 规划器不能生成一个完整的圆。.
需要设置笛卡尔限制,即平移/旋转 速度/加速度/减速度,规划器使用这些限制在笛卡尔空间中生成梯形速度剖面。 旋转运动是开始和目标方向之间的四元数 slerp。 平移和旋转运动在时间上是同步的。 这个规划器只接受零速度的开始状态。 规划结果是一个关节轨迹。 如果运动规划因违反关节限制而失败,则用户需要调整笛卡尔速度/加速度比例因子。
请求:Input parameters in moveit_msgs::MotionPlanRequest
- planner_id: CIRC
- group_name: 规划组的名称
- max_velocity_scaling_factor: 最大笛卡尔平移/旋转速度的比例因子
- max_acceleration_scaling_factor: 最大笛卡尔平移/旋转加速/减速的比例因子
- start_state/joint_state/(name, position and velocity: 关节名称/起始状态的位置.
- goal_constraints (目标可以在关节空间或笛卡尔空间中给出)
- 对于关节空间目标
- goal_constraints/joint_constraints/position: 目标关节位置
- goal_constraints/joint_constraints/joint_name: 目标关节名
- 对于关节空间目标
- 对于笛卡尔空间目标
- goal_constraints/position_constraints/target_point_offset: 目标连杆上目标点的偏移量(在;连杆坐标系中)(可选)
- goal_constraints/position_constraints/constraint_region: 目标点的包围体积
- goal_constraints/position_constraints/link_name: 目标连杆名
- goal_constraints/position_constraints/header/frame_id: 数据关联的坐标系
- path_constraints (中间点/中心点的位置)
- path_constraints/name: 中间点或中心 名
- path_constraints/position_constraints/constraint_region/primitive_poses/point: 点的位置
结果:Planning results in moveit_msg::MotionPlanResponse
- trajectory_start: 规划轨迹的第一个机器人状态
- trajectory/joint_trajectory/joint_names: 生成的关节轨迹的关节名称列表
- trajectory/joint_trajectory/points/(positions,velocities,accelerations,time_from_start): 生成的航点列表。 每个点都有所有关节的位置/速度/加速度(与关节名称的顺序相同)和从开始的时间。 最后一点的速度和加速度为零。
- group_name: 规划组名称
- error_code/val: 运动规划的错误代码
八、 示例Example
通过运行:
roslaunch moveit_resources_prbt_moveit_config demo.launch
roslaunch moveit_resources_prbt_moveit_config moveit_planning_execution.launch sim:=true pipeline:=pilz_industrial_motion_planner
<!--prbt_moveit_config move_group.launch-->
<launch>
<arg name="pipeline" default="ompl" />
<arg name="load_robot_description" default="true" />
<!-- GDB 调试选项 -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix"
value="gdb -x $(find moveit_resources_prbt_moveit_config)/launch/gdb_settings.gdb --ex run --args" />
<!-- 详细模式选项 -->
<arg name="info" default="$(arg debug)" />
<arg unless="$(arg info)" name="command_args" value="" />
<arg if="$(arg info)" name="command_args" value="--debug" />
<!-- move_group 设置 -->
<arg name="allow_trajectory_execution" default="true"/>
<arg name="fake_execution" default="false"/>
<arg name="execution_type" default="interpolate"/> <!-- 设置为“最后一点”以跳过假执行中的中间轨迹 -->
<arg name="max_safe_path_cost" default="1"/>
<arg name="jiggle_fraction" default="0.05" />
<arg name="publish_monitored_planning_scene" default="true"/>
<arg name="moveit_manage_controllers" default="true"/>
<arg name="capabilities" default=""/> <!-- 定义启动时加载的功能(空格分隔) -->
<arg name="disable_capabilities" default=""/> <!-- 抑制能力(空格分隔) -->
<!--启用/禁用 夹爪 -->
<arg name="gripper" default="" />
<!-- 加载机器人模型 -->
<include file="$(find moveit_resources_prbt_moveit_config)/launch/planning_context.launch" >
<arg name="gripper" value="$(arg gripper)" />
<arg name="load_robot_description" value="$(arg load_robot_description)" />
</include>
<!-- 规划管道 -->
<group ns="move_group/planning_pipelines">
<!-- OMPL运动规划器 -->
<include ns="ompl" file="$(find moveit_resources_prbt_moveit_config)/launch/planning_pipeline.launch.xml">
<arg name="pipeline" value="ompl" />
</include>
<!-- Pilz 工业运动规划器-->
<include ns="pilz_industrial_motion_planner" file="$(find moveit_resources_prbt_moveit_config)/launch/planning_pipeline.launch.xml">
<arg name="pipeline" value="pilz_industrial_motion_planner" />
</include>
<!-- 如果作为参数传递,则加载非默认规划器 -->
<include if="$(eval arg('pipeline') not in ['ompl', 'pilz_industrial_motion_planner'])" ns="$(arg pipeline)"
file="$(find moveit_resources_prbt_moveit_config)/launch/planning_pipeline.launch.xml">
<arg name="pipeline" value="$(arg pipeline)" />
</include>
</group>
<!-- 轨迹执行功能 -->
<include ns="move_group" file="$(find moveit_resources_prbt_moveit_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
<arg name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)" />
<arg name="moveit_controller_manager" value="prbt" unless="$(arg fake_execution)"/>
<arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
<arg name="gripper" value="$(arg gripper)" />
<arg name="execution_type" value="$(arg execution_type)" />
</include>
<!--传感器功能 -->
<include ns="move_group" file="$(find moveit_resources_prbt_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
<arg name="moveit_sensor_manager" value="prbt" />
</include>
<!-- 启动实际的 move_group 节点/动作服务器 -->
<node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
<!-- 设置显示变量,以防内部使用 OpenGL 代码 -->
<env name="DISPLAY" value="$(optenv DISPLAY :0)" />
<param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)" />
<param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
<param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
<param name="default_planning_pipeline" value="$(arg pipeline)" />
<param name="capabilities" value="$(arg capabilities)" />
<param name="disable_capabilities" value="$(arg disable_capabilities)" />
<!-- 发布实体机器人的规划场景,让rviz插件知道实际机器人 -->
<param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
</node>
</launch>
用户可以通过rviz与planner进行交互。
九、 Using the planner 使用规划器
pilz_industrial_motion_planner::CommandPlanner 作为 MoveIt 运动规划管道提供,因此可以与使用 MoveIt 的所有其他manipulators一起使用。 加载插件需要在启动 move_group 节点之前将参数 /move_group/planning_plugin 设置为 pilz_industrial_motion_planner::CommandPlanner。
要使用命令规划器,必须定义笛卡尔极限。 这些限制预计在namespace <robot_description>_planning下。 其中 <robot_description> 是指加载 URDF 的参数。 例如。 如果 URDF 已加载到 /robot_description 中,则必须在 /robot_description_planning 中定义笛卡尔限制。
可以在 prbt_moveit_config 中找到显示必须定义的笛卡尔限制的示例。moveit_resources/cartesian_limits.yaml at master · ros-planning/moveit_resources (github.com)
十、 Sequence of multiple segments多段序列
要连接多个轨迹并一次规划轨迹,您可以使用序列sequence功能。 这减少了规划开销并允许遵循预先描述的路径而无需在中间点停止。
请注意:如果序列中的命令规划失败,则不会执行序列中的任何命令。
请注意:序列命令可以包含多个组的命令(例如“Manipulator”, “Gripper” “机械手”、“抓手”)
十一、 User interface sequence capability序列功能的用户接口
专门的 MoveIt 功能将 moveit_msgs::MotionSequenceRequest 作为输入。 该请求包含如上所述的后续(subsequent) 目标列表和额外的 blend_radius 参数。 如果给定的blend_radius(以米为单位)大于零,则相应的轨迹与以下目标合并在一起,这样机器人就不会在当前目标处停下来。 当 TCP 比给定的 blend_radius 更接近目标时,它已经被允许朝着下一个目标前进。 当在当前目标周围留下一个球体时,机器人将返回到它本来不会混合的轨迹上。
实施细节以 pdf 格式提供。moveit/MotionBlendAlgorithmDescription.pdf at master · ros-planning/moveit (github.com)
Restrictions for MotionSequenceRequest 运动序列请求的限制
只有第一个目标可能有一个开始状态。 跟随轨迹从前一个目标开始。
两个后续的 blend_radius 球体不得重叠。 blend_radius(i) + blend_radius(i+1) 必须小于目标之间的距离。
- Only the first goal may have a start state. Following trajectories start at the previous goal.
- Two subsequent blend_radius spheres must not overlap. blend_radius(i) + blend_radius(i+1) has to be smaller than the distance between the goals.
Action interface 动作接口
与 MoveGroup 操作接口类似,用户可以通过位于 /sequence_move_group 的动作服务器规划和执行 moveit_msgs::MotionSequenceRequest。
在某一点上,MoveGroupSequenceAction 与标准的 MoveGroup 功能不同:如果机器人已经在目标位置,则仍会执行路径。 底层 PlannerManager 可以检查是否已经满足单个 moveit_msgs::MotionPlanRequest 的约束,但 MoveGroupSequenceAction 功能没有实现此类检查以允许在圆形或类似路径上移动。
请参阅 pilz_robot_programming 包以获取显示如何使用该功能的示例 Python 脚本。
#https://github.com/PilzDE/pilz_industrial_motion/tree/melodic-devel
#https://github.com/PilzDE/pilz_industrial_motion/blob/melodic-devel/pilz_robot_programming/examples/demo_program.py
from geometry_msgs.msg import Point
from pilz_robot_programming import *
import math
import rospy
__REQUIRED_API_VERSION__ = "1"
def start_program():
print("Executing " + __file__)
r = Robot(__REQUIRED_API_VERSION__)
# 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()
# 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))
# 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)
# 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 命令,但关节值越界
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()
Service interface 服务接口
服务 plan_sequence_path 允许用户为 moveit_msgs::MotionSequenceRequest 生成关节轨迹。 轨迹被返回而不被执行。
PilzDE/pilz_industrial_motion at melodic-devel (github.com)
noetic-devel
is considered to be the active development branch. Relevant changes are cherry-picked into melodic-devel
or kinetic-devel
on a case-by-case basis.
noetic-devel 被认为是活跃的开发分支。 相关更改会根据具体情况精心挑选到melodic-devel
或 kinetic-devel
中。
十二、 附录:
Planners | MoveIt (ros.org)https://moveit.ros.org/documentation/planners/
MoveIt 中可用的规划器
MoveIt is designed to work with many different types of planners, which is ideal for benchmarking improved planners against previous methods. Below is a list of planners that have been used with MoveIt, in descending order of popularity/support within MoveIt:
MoveIt 旨在与许多不同类型的规划器一起使用,非常适合根据以前的方法对改进的规划器进行基准测试。 以下是已与 MoveIt 一起使用的规划器列表,按 MoveIt 中受欢迎程度/支持程度的降序排列:
Open Motion Planning Library (OMPL)
OMPL 是一个开源运动规划库,主要实现随机运动规划器。 MoveIt 直接与 OMPL 集成,并使用该库中的运动规划器作为其主要/默认规划器集。 OMPL 中的规划器是抽象的; 即 OMPL 没有机器人的概念。 相反,MoveIt 配置 OMPL 并为 OMPL 提供后端以处理机器人技术中的问题。 完全支持。 更多信息The Open Motion Planning Library (kavrakilab.org)
Pilz Industrial Motion Planner
Pilz 工业运动规划器是用于圆周运动和线性运动的确定性发生器。 此外,它还支持使用 MoveIt 功能将多个运动段混合在一起。
Pilz industrial motion planner is a deterministic generator for circular and linear motions. Additionally, it supports blending multiple motion segments together using a MoveIt capability.
Stochastic Trajectory Optimization for Motion Planning (STOMP)
STOMP(运动规划的随机轨迹优化)是一种基于优化的运动规划器,它基于 PI^2(路径积分策略改进,Theodorou 等人,2010)算法。 它可以为机械臂规划平滑的轨迹、避开障碍物并优化约束。 该算法不需要梯度,因此可以优化成本函数中的任意项,如电机功率。 部分支持。 更多信息stomp_motion_planner - ROS Wiki
Search-Based Planning Library (SBPL)
一组通用的运动规划器,使用基于搜索的规划来离散化空间。 正在集成到最新版本的 MoveIt 中。 更多信息sbpl - ROS Wiki
Covariant Hamiltonian Optimization for Motion Planning (CHOMP)
运动规划的协变哈密顿优化 (CHOMP) 是一种新颖的基于梯度的轨迹优化程序,它使许多日常运动规划问题既简单又可训练(Ratliff 等人,2009c)。 虽然大多数高维运动规划器将轨迹生成分为不同的规划和优化阶段,但该算法在优化阶段利用协变梯度和函数梯度方法来设计完全基于轨迹优化的运动规划算法。 给定不可行的初始轨迹,CHOMP 会对周围环境做出反应,以快速将轨迹从碰撞中拉出,同时优化关节速度和加速度等动力学量。 它迅速收敛到可以在机器人上高效执行的平滑无碰撞轨迹。 正在集成到最新版本的 MoveIt 中。 更多信息CHOMP - Optimized Robotics (nathanratliff.com)
参考: