Skip to content
Anima Mundi Anima Mundi

传播阶段

传播器（Propagator）从相邻状态接收解，求解一个问题，然后将结果传播给另一侧的相邻状态。根据实现的不同，该阶段可以向前、向后或双向传递解。

MTC 提供以下传播阶段：

  • ModifyPlanningScene

  • MoveRelative

  • MoveTo

  • FixCollisionObjects

ModifyPlanningScene

Section titled “ModifyPlanningScene”

ModifyPlanningScene 阶段在不移动机器人的情况下对规划场景（PlanningScene）进行修改。默认情况下，该阶段双向传播结果，默认代价项为常数 0。

该阶段支持以下功能：

  • 启用和禁用连杆之间的碰撞检测
  • 在机器人连杆上附着和分离物体
  • 在场景中生成和移除物体

附着物体的示例代码：

  auto stage = std::make_unique<stages::ModifyPlanningScene>("attach object");
  stage->attachObject("object_name", "gripper_frame_names");

启用碰撞的示例代码：

  auto stage = std::make_unique<stages::ModifyPlanningScene>("Allow collision between object and gripper");
  stage->allowCollisions("object_name", "gripper_frame_name", true);

ModifyPlanningScene 的 API 文档

MoveRelative

Section titled “MoveRelative”

MoveRelative 阶段用于执行笛卡尔运动。默认情况下，该阶段双向传播结果，默认规划时间为 1.0 秒，默认代价项取决于路径长度。

需要由用户设置的属性：

属性名设置属性的函数描述
groupvoid setGroup(std::string group)规划组（planning group）的名称。
ik_framevoid setIKFrame(std::string group)要在笛卡尔方向上移动的坐标系（frame）。
min_distancevoid setMinDistance(double distance)移动的最小距离。默认为 -1.0。
max_distancevoid setMaxDistance(double distance)移动的最大距离。默认为 0.0。
path_constaintsvoid setPathConstraints(moveit_msgs/Constraints path_constaints)在轨迹执行期间需要满足的约束。
directionvoid setDirection(geometry_msgs/TwistStamped twist)在指定连杆上执行 twist（旋量）运动。
directionvoid setDirection(geometry_msgs/Vector3Stamped direction)沿给定方向平移连杆。
directionvoid setDirection(std::map<std::string, double> direction)将指定的关节变量移动给定的量。

MoveRelative 的 API 文档

示例代码：

  const auto cartesian_planner = std::make_shared<moveit::task_constructor::solvers::CartesianPath>();
  auto approach_pose =
      std::make_unique<moveit::task_constructor::stages::MoveRelative>("Approach", cartesian_planner);
  // Propagate the solution backward only
  stage_approach_grasp->restrictDirection(moveit::task_constructor::stages::MoveRelative::BACKWARD);
  stage_approach_grasp->setGroup("manipulator");
  stage_approach_grasp->setIKFrame("tool_frame");


  // Move the end effector by 0.15m in the z direction.
  const Eigen::Vector3d approach{ 0.0, 0.0, 0.15 };
  geometry_msgs::msg::Vector3Stamped approach_vector;
  tf2::toMsg(approach, approach_vector.vector);
  approach_vector.header.frame_id = "tool_frame";
  stage_approach_grasp->setDirection(approach_vector);

MoveTo

Section titled “MoveTo”

MoveTo 阶段用于移动到关节状态或笛卡尔目标位姿。默认情况下，该阶段双向传播结果，默认规划时间为 1.0 秒，默认代价项取决于路径长度。

该阶段需要设置的属性列于下表，目标可以用不同的格式指定。

需要由用户设置的属性：

属性名设置属性的函数描述
groupvoid setGroup(std::string group)规划组（planning group）的名称。
ik_framevoid setIKFrame(geometry_msgs/PoseStamped pose)要向目标位姿移动的坐标系（frame）。
goalvoid setGoal(geometry_msgs/PoseStamped pose)将连杆移动到给定的位姿。
goalvoid setGoal(geometry_msgs/PointStamped point)将连杆移动到给定的点，保持当前姿态。
goalvoid setGoal(std::string named_joint_pose)将关节模型组移动到给定的命名位姿。该命名位姿应在 SRDF 文件中定义。
goalvoid setGoal(moveit_msgs/RobotState robot_state)将消息中指定的关节移动到其目标值。
goalvoid setGoal(std::map<std::string, double> joints)按名称将关节移动到其映射的目标值。
path_constaintsvoid setPathConstraints(moveit_msgs/::Constraints path_constaints)在轨迹执行期间需要满足的约束。

MoveTo 的 API 文档

示例代码：

  const auto joint_interpolation_planner =
      std::make_shared<moveit::task_constructor::solvers::JointInterpolationPlanner>();
  auto stage =
        std::make_unique<moveit::task_constructor::stages::MoveTo>("close gripper", joint_interpolation_planner);
  // Set trajectory execution info. This will contain the list of controllers used to actuate gripper and arm.
  // Since this property is set during task initialization, we can inherit from it.
  stage->properties().set("trajectory_execution_info",
                          std::any_cast<moveit::task_constructor::TrajectoryExecutionInfo>(task->properties().get("trajectory_execution_info")));
  stage->setGroup("gripper"));
  stage->setGoal("closed"); // Group state named in SRDF
  stage->setTimeout(2.0);

FixCollisionObjects

Section titled “FixCollisionObjects”

FixCollisionObjects 阶段用于检查碰撞，并在适用的情况下进行修复。默认情况下，该阶段双向传播结果，默认代价项为常数 0。

需要由用户设置的属性：

属性名设置属性的函数描述
directionvoid setDirection(geometry_msgs/Vector3 direction)通过沿修正方向移动物体来修复碰撞的方向向量。若未显式设置，则会计算默认方向。
penetrationvoid setMaxPenetration(double penetration)碰撞物体可被修复的最大穿透深度。如果物体的碰撞深度大于所设值，则不会修复该碰撞。

示例代码：

  auto stage = std::make_unique<stages::FixCollisionObjects>();
  stage->setMaxPenetration(0.04);