包装器

包装器用于封装另一个阶段，以修改或过滤其结果。

MTC 提供以下包装器阶段：

• ComputeIK
• PredicateFilter
• PassThrough

ComputeIK

ComputeIK 是任何位姿生成器阶段的包装器，用于计算由 GeneratePose 阶段生成的笛卡尔位姿的逆运动学。

需要用户设置的属性

属性名称	设置属性的函数	描述
eef	void setEndEffector(std::string eef)	末端执行器组的名称
group	void setGroup(std::string group)	活动组的名称。若未提供，则从 eef 派生。
max_ik_solutions	void setMaxIKSolutions(uint32_t n)	默认值为 1
ignore_collisions	void setIgnoreCollisions(bool flag)	默认值为 false。
min_solution_distance	void setMinSolutionDistance(double distance)	同一目标的不同 IK 解之间的最小距离。默认值为 0.1。

ComputeIK 的 API 文档

代码示例：

auto stage = std::make_unique<moveit::task_constructor::stages::GenerateVacuumGraspPose>("generate pose");
auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("pose IK", std::move(stage));
wrapper->setTimeout(0.05);
wrapper->setIKFrame("tool_frame");
wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" }); // Property value derived from parent stage
wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" }); // Property value derived from child stage

// Users can add a callback function when grasp solutions are generated.
// Here we have added a custom function called publishGraspSolution which can publish the grasp solution to a certain topic.
wrapper->addSolutionCallback(
    [this](const moveit::task_constructor::SolutionBase& solution) { return publishGraspSolution(solution); });

PredicateFilter

PredicateFilter 是一个阶段包装器，用于按自定义条件过滤生成的解。

需要用户设置的属性

属性名称	设置属性的函数	描述
predicate	void setPredicate(std::function<bool(const SolutionBase, std::string)> predicate)	用于过滤解的谓词
ignore_filter	void setIgnoreFilter(bool ignore)	忽略谓词并转发所有解

PredicateFilter 的 API 文档

代码示例：

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

// Use Predicate filter to fail the MTC task if any links are in collision in planning scene
auto applicability_filter =
    std::make_unique<moveit::task_constructor::stages::PredicateFilter>("current state", std::move(current_state));

applicability_filter->setPredicate([this](const moveit::task_constructor::SolutionBase& s, std::string& comment) {
  if (s.start()->scene()->isStateColliding())
  {
    // Get links that are in collision
    std::vector<std::string> colliding_links;
    s.start()->scene()->getCollidingLinks(colliding_links);

    // Publish the results
    publishLinkCollisions(colliding_links);

    comment = "Links are in collision";
    return false;
  }
  return true;
});

PassThrough

PassThrough 是一个通用包装器，用于直接传递解。它可用于通过 Stage::setCostTerm 设置自定义的 CostTransform，从而在不丢失原始值的情况下改变解的成本。