Wrappers

Wrappers encapsulate another stage to modify or filter the results.

MTC provides the following wrapper stages:

• ComputeIK

• PredicateFilter

• PassThrough

ComputeIK

The ComputeIK is a wrapper for any pose generator stage to compute the inverse kinematics for the poses in Cartesian space generated by the Generate Pose stage.

Properties to be set by user

Property Name	Function to set property	Description
eef	void setEndEffector(std::string eef)	Name of end effector group
group	void setGroup(std::string group)	Name of active group. Derived from eef if not provided.
max_ik_solutions	void setMaxIKSolutions(uint32_t n)	Default is 1
ignore_collisions	void setIgnoreCollisions(bool flag)	Default is false.
min_solution_distance	void setMinSolutionDistance(double distance)	Minimum distance between separate IK solutions for the same target. Default is 0.1.

API doc for ComputeIK.

Code Example

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 is a stage wrapper to filter generated solutions by custom criteria.

Properties to be set by user

Property Name	Function to set property	Description
predicate	void setPredicate(std::function<bool(const SolutionBase, std::string)> predicate)	Predicate to filter solutions
ignore_filter	void setIgnoreFilter(bool ignore)	Ignore predicate and forward all solutions

API doc for PredicateFilter.

Code Example

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 is a generic wrapper that passes on a solution. This is useful to set a custom CostTransform via Stage::setCostTerm to change a solutions’s cost without losing the original value.