Pilz 工业运动规划器

pilz_industrial_motion_planner 提供轨迹生成器，使用 MoveIt 规划标准机器人运动，如点对点、线性和圆形。

通过加载相应的规划管道（*_moveit_config 包中的 pilz_industrial_motion_planner_planning_planner.yaml），可以通过 move_group 节点提供的用户界面（C++、Python 或 RViz）访问轨迹生成功能，例如 /plan_kinematic_path 服务和 /move_action 操作。有关详细使用教程，请参阅 MoveItCpp 教程和移动组 C++ 接口。

关节限制

规划器使用操作 Pilz 规划管道的 ROS 节点的参数中的最大速度和加速度。

使用 MoveIt 设置助手，文件 joint_limits.yaml 会自动生成并具有适当的默认值，并在启动期间加载。

指定的限制会覆盖 URDF 机器人描述中的限制。

请注意，虽然可以在 URDF 和参数文件中设置位置限制和速度限制，但只能使用参数文件设置加速度限制。除了常见的 has_acceleration 和 max_acceleration 参数外，我们还添加了设置 has_deceleration 和 max_deceleration(<0.0) 的功能。

限制合并的前提是节点参数的限制必须更严格或至少等于 URDF 中设置的参数。

目前，计算出的轨迹将通过使用所有限制的最严格组合作为所有关节的共同限制来遵守限制。

笛卡尔限制

对于笛卡尔轨迹生成 (LIN/CIRC)，规划器需要有关 3D 笛卡尔空间中最大速度的信息。即，需要在节点参数中设置平移/旋转速度/加速度/减速度，如下所示：

cartesian_limits:
  max_trans_vel: 1
  max_trans_acc: 2.25
  max_trans_dec: -5
  max_rot_vel: 1.57

您可以在 *_moveit_config 包中名为 pilz_cartesian_limits.yaml 的文件中指定笛卡尔速度和加速度限制。

规划器假设平移和旋转梯形具有相同的加速度比。旋转加速度计算为 ``max_trans_acc / max_trans_vel * max_rot_vel``（减速也如此）。

规划接口

此包使用 moveit_msgs::msgs::MotionPlanRequest 和 moveit_msgs::msg::MotionPlanResponse 作为运动规划的输入和输出。每个规划算法所需的参数如下所述。

有关如何填写 MotionPlanRequest 的一般介绍，请参阅 Planning to a Pose goal。

您可以将 “PTP”、“LIN” 或 “CIRC” 指定为 MotionPlanRequest 的 planner_id。

PTP 运动命令

此规划器生成具有梯形关节速度曲线的完全同步的点对点轨迹。假定所有关节具有相同的最大关节速度/加速度/减速度限制。如果不是，则采用最严格的限制。选择到达目标时间最长的轴作为引导轴。其他轴减速，以便它们与引导轴共享相同的加速/恒定速度/减速阶段。

`moveit_msgs::MotionPlanRequest` 中的 PTP 输入参数

planner_id："PTP"
group_name：规划组的名称
max_velocity_scaling_factor：最大关节速度的缩放因子
max_acceleration_scaling_factor：最大关节加速度/减速度的缩放因子
start_state/joint_state/(name、position 和 velocity)：起始状态的关节名称/位置/速度（可选）。
goal_constraints：（目标可以在关节空间或笛卡尔空间中给出）
对于关节空间中的目标
goal_constraints/joint_constraints/joint_name：目标关节名称
goal_constraints/joint_constraints/position：目标关节位置
对于笛卡尔空间中的目标
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：偏移量（在链接中目标链接上的目标点的坐标（可选）

“moveit_msg::MotionPlanResponse”中的 PTP 规划结果

trajectory_start：规划轨迹的第一个机器人状态
trajectory/joint_trajectory/joint_names：生成的关节轨迹的关节名称列表
trajectory/joint_trajectory/points/(positions,velocities,accelerations,time_from_start)：生成的路径点列表。每个点都有所有关节的位置/速度/加速度（与关节名称的顺序相同）和从开始的时间。最后一个点的速度和加速度为零。
group_name：规划组的名称
error_code/val：运动规划的错误代码

LIN 运动命令

此规划器在目标和起始姿势之间生成线性笛卡尔轨迹。规划器使用笛卡尔极限在笛卡尔空间中生成梯形速度分布。平移运动是起始和目标位置向量之间的线性插值。旋转运动是起始和目标方向之间的四元数线性插值。平移和旋转运动在时间上同步。此规划器仅接受速度为零的起始状态。规划结果是关节轨迹。如果运动规划由于违反关节空间限制而失败，用户需要调整笛卡尔速度/加速度缩放因子。

`moveit_msgs::MotionPlanRequest` 中的 LIN 输入参数

planner_id："LIN"
group_name：规划组的名称
max_velocity_scaling_factor：最大笛卡尔平移/旋转速度的缩放因子
max_acceleration_scaling_factor：最大笛卡尔平移/旋转加速度/减速度的缩放因子
start_state/joint_state/(name、position 和 velocity：起始状态的关节名称/位置。
``goal_constraints``（目标可以在关节空间或笛卡尔空间中给出）
用于关节空间中的目标
goal_constraints/joint_constraints/joint_name：目标关节名称
goal_constraints/joint_constraints/position：目标关节位置
用于目标在笛卡尔空间中
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：目标点在目标链接上的偏移量（在链接框架中）（可选）

`moveit_msg::MotionPlanResponse` 中的 LIN 规划结果

trajectory_start：规划轨迹的第一个机器人状态
trajectory/joint_trajectory/joint_names：生成的关节轨迹的关节名称列表
trajectory/joint_trajectory/points/(positions,velocities,accelerations,time_from_start)：生成的路径点列表。每个点都有所有关节的位置/速度/加速度（与关节名称的顺序相同）和从开始的时间。最后一个点的速度和加速度为零。
group_name：规划组的名称
error_code/val：运动规划的错误代码

CIRC 运动命令

该规划器在目标和起始姿势之间的笛卡尔空间中生成圆弧轨迹。有两种方式可以给出路径约束：

圆的中心点：规划器始终在起始和目标之间生成较短的圆弧，并且不能生成半圆，
圆弧上的中间点：生成的轨迹始终经过中间点。规划器无法生成完整的圆。

需要设置笛卡尔极限，即平移/旋转速度/加速度/减速度，规划器使用这些极限在笛卡尔空间中生成梯形速度分布。旋转运动是起始和目标方向之间的四元数线性插值。平移和旋转运动在时间上是同步的。该规划器仅接受速度为零的起始状态。规划结果是关节轨迹。如果运动计划因违反关节限制而失败，则用户需要调整笛卡尔速度/加速度缩放因子。

“moveit_msgs::MotionPlanRequest”中的 CIRC 输入参数

planner_id："CIRC"
group_name：规划组的名称
max_velocity_scaling_factor：最大笛卡尔平移/旋转速度的缩放因子
max_acceleration_scaling_factor：最大笛卡尔平移/旋转加速度/减速度的缩放因子
start_state/joint_state/(name、position 和 velocity：起始状态的关节名称/位置。
``goal_constraints``（目标可以在关节空间或笛卡尔空间中给出）
用于关节空间中的目标
goal_constraints/joint_constraints/joint_name：目标关节名称
goal_constraints/joint_constraints/position：目标关节位置
用于笛卡尔空间中的目标
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：目标点在目标链接上的偏移量（在链接框架中）（可选）

``path_constraints``（中间/中心点的位置）
path_constraints/name：中间或中心
path_constraints/position_constraints/constraint_region/primitive_poses/point：

点的位置

“moveit_msg::MotionPlanResponse”中的 CIRC 规划结果

trajectory_start：规划轨迹的第一个机器人状态
trajectory/joint_trajectory/joint_names：生成的关节轨迹的关节名称列表
trajectory/joint_trajectory/points/(positions,velocities,accelerations,time_from_start)：生成的路径点列表。每个点都有所有关节的位置/速度/加速度（与关节名称的顺序相同）和从开始的时间。最后一个点的速度和加速度为零。
group_name：规划组的名称
error_code/val：运动规划的错误代码

示例

通过运行

ros2 launch moveit_resources_panda_moveit_config demo.launch.py

您可以通过 RViz MotionPlanning 面板与规划器进行交互。

要通过 MoveGroup 接口使用规划器，请参阅 MoveGroup 接口 C++ 示例。要运行此示例，请在单独的终端中执行以下命令：

ros2 launch moveit2_tutorials pilz_moveit.launch.py
ros2 run moveit2_tutorials pilz_move_group

要使用 MoveIt 任务构造函数来使用规划器，请参阅 MoveIt 任务构造函数 C++ 示例。要运行此示例，请在单独的终端中执行以下命令：

ros2 launch moveit2_tutorials mtc_demo.launch.py
ros2 launch moveit2_tutorials pilz_mtc.launch.py

使用规划器

pilz_industrial_motion_planner::CommandPlanner 作为 MoveIt 运动规划管道提供，因此可与使用 MoveIt 的所有其他操纵器一起使用。加载插件需要在启动 move_group 节点之前将参数 /move_group/<pipeline_name>/planning_plugins 设置为 [pilz_industrial_motion_planner/CommandPlanner]。例如，panda_moveit_config 包具有设置如下的 pilz_industrial_motion_planner 管道：

ros2 param get /move_group pilz_industrial_motion_planner.planning_plugins

String value is: pilz_industrial_motion_planner/CommandPlanner

要使用命令规划器，必须定义笛卡尔极限。

极限应位于命名空间 <robot_description>_planning 下，其中 <robot_description> 指的是加载 URDF 的参数名称。

例如，如果 URDF 加载到 /robot_description 中，则必须在 /robot_description_planning 中定义笛卡尔极限。

您可以使用 *_moveit_config 包中的 pilz_cartesian_limits.yaml 文件设置这些。

可以在 panda_moveit_config 中找到显示此文件的示例。

要验证限制是否设置正确，您可以检查“move_group”节点的参数。例如，

ros2 param list /move_group --filter .*cartesian_limits

/move_group:
    robot_description_planning.cartesian_limits.max_rot_vel
    robot_description_planning.cartesian_limits.max_trans_acc
    robot_description_planning.cartesian_limits.max_trans_dec
    robot_description_planning.cartesian_limits.max_trans_vel

多段序列

要连接多条轨迹并一次性规划轨迹，您可以使用序列功能。这可以减少规划开销并允许遵循预先定义的路径而不在中间点停止。

**请注意：**如果序列中命令的规划失败，则不会执行序列中的任何命令。

请注意： 序列命令可以包含多个组的命令（例如“操纵器”、“夹持器”）

用户界面序列功能

称为命令列表管理器的专用 MoveIt 功能以 moveit_msgs::msg::MotionSequenceRequest 作为输入。该请求包含如上所述的后续目标列表和附加的 blend_radius 参数。如果给定的 ``blend_radius``（以米为单位）大于零，则相应的轨迹将与以下目标合并，以使机器人不会停在当前目标处。当 TCP 比给定的“blend_radius”更接近目标时，它就可以向下一个目标行进。当离开当前目标周围的球体时，机器人将返回到它不进行混合时所走的轨迹上。

实施细节已公布 as PDF.

`MotionSequenceRequest` 的限制

只有第一个目标可以有开始状态。后续轨迹

从前一个目标开始。 - 两个后续 blend_radius 球体不得重叠。 blend_radius(i) + blend_radius(i+1) 必须小于目标之间的距离。

服务接口

服务 plan_sequence_path 允许用户为 moveit_msgs::msg::MotionSequenceRequest 生成关节轨迹。轨迹被返回但不执行。

要使用“MoveGroupSequenceService”和“MoveGroupSequenceAction”，请参阅:codedir:“Pilz Motion Planner 序列示例 <how_to_guides/pilz_industrial_motion_planner/src/pilz_sequence.cpp>”。要运行此示例，请在单独的终端中执行以下命令：

ros2 launch moveit2_tutorials pilz_moveit.launch.py
ros2 run moveit2_tutorials pilz_sequence

对于此服务和操作，需要修改 move_group 启动文件以包含这些 Pilz Motion Planner 功能。

而是使用新的 pilz_moveit.launch.py ：

moveit_config = (
   MoveItConfigsBuilder("moveit_resources_panda")
   .robot_description(file_path="config/panda.urdf.xacro")
   .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
   .planning_scene_monitor(
      publish_robot_description=True, publish_robot_description_semantic=True
   )
   .planning_pipelines(
      pipelines=["pilz_industrial_motion_planner"]
   )
   .to_moveit_configs()
)

# Starts Pilz Industrial Motion Planner MoveGroupSequenceAction and MoveGroupSequenceService servers
move_group_capabilities = {
   "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService"
}

pilz_sequence.cpp 文件创建了两个将按顺序达到的目标姿势。

// ----- Motion Sequence Item 1
// Create a MotionSequenceItem
moveit_msgs::msg::MotionSequenceItem item1;

// Set pose blend radius
item1.blend_radius = 0.1;

// MotionSequenceItem configuration
item1.req.group_name = PLANNING_GROUP;
item1.req.planner_id = "LIN";
item1.req.allowed_planning_time = 5.0;
item1.req.max_velocity_scaling_factor = 0.1;
item1.req.max_acceleration_scaling_factor = 0.1;

moveit_msgs::msg::Constraints constraints_item1;
moveit_msgs::msg::PositionConstraint pos_constraint_item1;
pos_constraint_item1.header.frame_id = "world";
pos_constraint_item1.link_name = "panda_hand";

// Set a constraint pose
auto target_pose_item1 = [] {
   geometry_msgs::msg::PoseStamped msg;
   msg.header.frame_id = "world";
   msg.pose.position.x = 0.3;
   msg.pose.position.y = -0.2;
   msg.pose.position.z = 0.6;
   msg.pose.orientation.x = 1.0;
   msg.pose.orientation.y = 0.0;
   msg.pose.orientation.z = 0.0;
   msg.pose.orientation.w = 0.0;
   return msg;
} ();
item1.req.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints("panda_link8", target_pose_item1));

服务客户端需要初始化：

// MoveGroupSequence service client
using GetMotionSequence = moveit_msgs::srv::GetMotionSequence;
auto service_client = node->create_client<GetMotionSequence>("/plan_sequence_path");

// Verify that the action server is up and running
while (!service_client->wait_for_service(std::chrono::seconds(10)))
{
   RCLCPP_WARN(LOGGER, "Waiting for service /plan_sequence_path to be available...");
}

然后，创建请求：

// Create request
auto service_request = std::make_shared<GetMotionSequence::Request>();
service_request->request.items.push_back(item1);
service_request->request.items.push_back(item2);

服务调用完成后，方法“future.wait_for(timeout_duration)”将阻塞，直到指定的“timeout_duration”已过或结果可用（以先到者为准）。返回值标识结果的状态。此操作每秒执行一次，直到 Future 准备就绪。

// Call the service and process the result
auto service_future = service_client->async_send_request(service_request);

// Function to draw the trajectory
auto const draw_trajectory_tool_path =
   [&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup("panda_arm")](
      auto const& trajectories) {
   for (const auto& trajectory : trajectories) {
      moveit_visual_tools.publishTrajectoryLine(trajectory, jmg);
   }
};

// Wait for the result
std::future_status service_status;
do
{
   switch (service_status = service_future.wait_for(std::chrono::seconds(1)); service_status)
   {
      case std::future_status::deferred:
      RCLCPP_ERROR(LOGGER, "Deferred");
      break;
      case std::future_status::timeout:
      RCLCPP_INFO(LOGGER, "Waiting for trajectory plan...");
      break;
      case std::future_status::ready:
      RCLCPP_INFO(LOGGER, "Service ready!");
      break;
   }
} while (service_status != std::future_status::ready);

使用“future.get()”方法读取未来的响应。

auto service_response = service_future.get();
if (service_response->response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
{
   RCLCPP_INFO(LOGGER, "Planning successful");

   // Access the planned trajectory
   auto trajectory = service_response->response.planned_trajectories;
   draw_trajectory_tool_path(trajectory);
   moveit_visual_tools.trigger();
}
else
{
   RCLCPP_ERROR(LOGGER, "Planning failed with error code: %d", service_response->response.error_code.val);

   rclcpp::shutdown();
   return 0;
}

在这种情况下，绘制了规划轨迹。以下是第一条轨迹和第二条轨迹的混合半径分别为 0 和 0.1 的比较。

Action interface

与 MoveGroup 操作接口类似，用户可以通过位于 /sequence_move_group 的操作服务器规划和执行 moveit_msgs::MotionSequenceRequest。

MoveGroupSequenceAction 与标准 MoveGroup 功能有一点不同：如果机器人已经处于目标位置，则仍会执行路径。底层 PlannerManager 可以检查单个 moveit_msgs::msg::MotionPlanRequest 的约束是否已经得到满足，但 MoveGroupSequenceAction 功能不会实现这样的检查以允许在圆形或类似路径上移动。

操作客户端需要初始化：

// MoveGroupSequence action client
using MoveGroupSequence = moveit_msgs::action::MoveGroupSequence;
auto action_client = rclcpp_action::create_client<MoveGroupSequence>(node, "/sequence_move_group");

// Verify that the action server is up and running
if (!action_client->wait_for_action_server(std::chrono::seconds(10)))
{
   RCLCPP_ERROR(LOGGER, "Error waiting for action server /sequence_move_group");
   return -1;
}