生成阶段

生成器阶段（generator stage）不从相邻阶段获取输入，而是自行计算结果并传递给相邻阶段。

MTC 提供以下生成器阶段：

• CurrentState

• FixedState

• 监控生成器（Monitoring Generators）—— GeneratePose、GenerateGraspPose、GeneratePlacePose 和 GenerateRandomPose。

CurrentState

CurrentState 阶段通过 get_planning_scene 服务获取当前的规划场景（PlanningScene）。

该阶段通常用于 MTC 任务管线的开头，将当前机器人状态设为起始状态。

示例代码：

  auto current_state = std::make_unique<moveit::task_constructor::stages::CurrentState>("current_state");

CurrentState 的 API 文档

FixedState

FixedState 阶段生成一个预定义的规划场景状态。

  moveit::task_constructor::Task t;
  auto node = rclcpp::Node::make_shared("node_name");
  t.loadRobotModel(node);

  auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
  auto& state = scene->getCurrentStateNonConst();
  state.setToDefaultValues();  // initialize state
  state.setToDefaultValues(state.getJointModelGroup("left_arm"), "home");
  state.setToDefaultValues(state.getJointModelGroup("right_arm"), "home");
  state.update();
spawnObject(scene); // User defined function to add a CollisionObject to planning scene

  auto initial = std::make_unique<stages::FixedState>();
  initial->setState(scene);

FixedState 的 API 文档

监控生成器（Monitoring Generators）

监控生成器负责监控并使用另一个阶段的解（solutions）。

GeneratePose

GeneratePose 是一种监控生成器，可根据被监控阶段提供的解来生成位姿（pose）。

GenerateGraspPose

GenerateGraspPose 阶段派生自 GeneratePose，属于监控生成器。

该阶段通常监控 CurrentState 阶段，因为它需要最新的 PlanningScene 来确定待生成抓取位姿（grasp pose）的对象位置。

通过设置相应属性即可生成抓取位姿。同一个属性可能有多种设置方式——以预抓取位姿（pre-grasp pose）为例，用户既可以使用 SRDF 中定义的命名状态（group state）字符串来设置，也可以显式指定一个 RobotState，如下表所示：

属性名称	设置属性的函数	描述
eef	void setEndEffector(std::string eef)	末端执行器的名称
object	void setObject(std::string object)	要抓取的对象。该对象应已存在于规划场景中。
angle_delta	void setAngleDelta(double delta)	角度步长（弧度）。目标抓取位姿围绕对象的 z 轴采样。
pregrasp	void setPreGraspPose(std::string pregrasp)	预抓取位姿。例如，抓取前夹爪应处于张开状态。此处的字符串对应 SRDF 中定义的命名状态。
pregrasp	void setPreGraspPose(moveit_msgs/RobotState pregrasp)	预抓取位姿
grasp	void setGraspPose(std::string grasp)	抓取位姿
grasp	void setGraspPose(moveit_msgs/RobotState grasp)	抓取位姿

关于代码的最新状态，请参阅 API 文档。

GenerateGraspPose 的 API 文档

示例代码：

  auto initial_stage = std::make_unique<stages::CurrentState>("current state");
  task->add(initial_stage);

  auto gengrasp = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
  gengrasp->setPreGraspPose("open");
  gengrasp->setObject("object");
  gengrasp->setAngleDelta(M_PI / 10.);
  gengrasp->setMonitoredStage(initial_stage);
  task->add(gengrasp);

GeneratePlacePose

GeneratePlacePose 阶段派生自 GeneratePose，属于监控生成器。

该阶段用于为放置（place）管线生成位姿。需要注意的是，GenerateGraspPose 以 angle_delta 为间隔生成位姿，而 GeneratePlacePose 则根据对象的形状采样固定数量的位姿。

示例代码：

  // Generate Place Pose
  auto stage = std::make_unique<stages::GeneratePlacePose>("generate place pose");
  stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" });
  stage->properties().set("marker_ns", "place_pose");
  stage->setObject(params.object_name);

  // Set target pose
  geometry_msgs::msg::PoseStamped p;
  p.header.frame_id = params.object_reference_frame;
  p.pose = vectorToPose(params.place_pose);
  p.pose.position.z += 0.5 * params.object_dimensions[0] + params.place_surface_offset;
  stage->setPose(p);
  stage->setMonitoredStage(pick_stage_ptr);  // hook into successful pick solutions

GeneratePlacePose 的 API 文档

GenerateRandomPose

GenerateRandomPose 阶段派生自 GeneratePose，属于监控生成器。

该阶段为 PoseDimension（X/Y/Z/ROLL/PITCH/YAW）配置一个随机数分布（RandomNumberDistribution）采样器（参见 https://en.cppreference.com/w/cpp/numeric/random），用于对位姿进行随机化处理。

属性名称	设置属性的函数	描述
max_solution	void setMaxSolution(size_t max_solution)	启用随机采样时，生成解数量的上限。

FixedCartesianPose

FixedCartesianPose 生成一个固定的笛卡尔位姿，不进行采样。该阶段适用于从预期的未来状态开始规划，可用于实现规划与执行同步（simultaneous planning and execution）等功能。