MoveIt Task constructor - Merge example

edit

Hi,

I am learning how to use MoveIt task constructor and as there is few examples and documentation about it, I am having a hard time to create a merge task : move both arms of a PR2 robot at the same time (ie: lower them of about 10 cm at the same time).

void planTest(Task &t) {

t.loadRobotModel();

auto cartesian = std::make_shared<solvers::CartesianPath>();
cartesian->setProperty("jump_threshold", 0.0);

Stage* current_state = nullptr;
auto initial = std::make_unique<stages::CurrentState>("current state");
current_state = initial.get();
t.add(std::move(initial));

// planner used for connect
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
pipeline->setPlannerId("RRTConnectkConfigDefault");

auto merger = std::make_unique<Merger>();

{
        auto stage = std::make_unique<stages::MoveRelative>("lower object left", cartesian);
        stage->properties().set("marker_ns", "lower_object");
        stage->properties().set("link", "l_gripper_tool_frame");
        stage->properties().set("group","left_arm");
        stage->setMinMaxDistance(.03, .13);
        // Set downward direction
        geometry_msgs::Vector3Stamped vec;
        vec.header.frame_id = "base_footprint";
        vec.vector.z = -1.0;
        stage->setDirection(vec);

      merger->insert(std::move(stage));
}


{
        auto stage = std::make_unique<stages::MoveRelative>("lower object right", cartesian);
        stage->properties().set("marker_ns", "lower_object");
        stage->properties().set("link", "r_gripper_tool_frame");
        stage->properties().set("group","right_arm");
        stage->setMinMaxDistance(.03, .13);
        // Set downward direction
        geometry_msgs::Vector3Stamped vec;
        vec.header.frame_id = "base_footprint";
        vec.vector.z = -1.0;
        stage->setDirection(vec);

      merger->insert(std::move(stage));
}


t.add(std::move(merger));
}

The error returned when running is :

Error initializing stages: current state: required interface is not satisfied merger: start interface of 'lower object left' (↑) doesn't match mine (?) merger: start interface of 'lower object right' (↑) doesn't match mine (?) merger: no child provides expected push interface

I don't really manage to understand the "interface" mechanism of MoveIt task constructor and there isn't much information on internet...

I managed to understand and use "Alternatives" and normal task workflow by trial and error but merger seems to be trickier and no examples are available for now.

thanks for your help.

Regards,