User defined grasp pose for pick task

edit

Hi,

I am quite satisfied with the grasp generator that MoveIt task constructor offers, but I wanted to know if and also how it would be possible to define a custom grasp pose instead of a generated one for a pick task.

Normally you would do this stage to create a pick :

{
    // grasp generator
    auto grasp_generator = std::make_unique<stages::GenerateGraspPose>("generate grasp pose left");
    grasp_generator->setAngleDelta(M_PI/4);
    grasp_generator->setPreGraspPose("left_open");
    grasp_generator->setGraspPose("left_close");
    grasp_generator->setMonitoredStage(current_state);

    auto grasp = std::make_unique<stages::SimpleGrasp>(std::move(stage));
    grasp->setIKFrame(Eigen::Affine3d::Identity(), "l_gripper_tool_frame");
    grasp->properties().configureInitFrom(Stage::PARENT);
    grasp->setMaxIKSolutions(10);


    // pick container, using the generated grasp generator
    auto pick = std::make_unique<stages::Pick>(std::move(grasp));
    pick->setProperty("eef", "left_gripper");
    pick->setProperty("object", std::string("object"));
    pick->properties().configureInitFrom(Stage::PARENT);

    geometry_msgs::TwistStamped approach;
    approach.header.frame_id = "l_wrist_roll_link";
    approach.twist.linear.x = 1.0;
    pick->setApproachMotion(approach, 0.01, 0.10);

    geometry_msgs::TwistStamped lift;
    lift.header.frame_id = "base_footprint";
    lift.twist.linear.z =  1.0;
    pick->setLiftMotion(lift, 0.05, 0.10);
    current_state = pick.get();
    t.add(std::move(pick));
}

If I understand well, the grasp generator find possible grasp, these information are given to "SimpleGrasp" that assemble them with IK and gripper movement (in a serial container), then "Pick" is a serial container that compile previous information with approach and lift moves, right ?

I thought that I would need to generate a pose with "GeneratePose" stage (and also set the pregrasp and grasp property "manually") and then give it to "SimpleGrasp" but it doesn't seem to work.

Any idea on how I could use a custom grasp pose instead of using the grasp generator ?

Thanks in advance for the help,

Regards