Moveit Task constructor properties propagation

asked 2020-07-08 04:36:07 -0600

Madcreator gravatar image

updated 2020-07-08 04:37:54 -0600

ROS distro : kinetic

Moveit task constructor branch : master

Moveit branch : master


I don't manage to really understand how the properties propagate through the different stages. In a simple example, I want to define some task properties (like in the demo) : group, eef, object, etc. Then I create a simple pick :

eef = "right_gripper";
ikFrame = "r_gripper_tool_frame";

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

    // connect to pick
    stages::Connect::GroupPlannerVector planners = {{eef, pipeline}, {planGroup, pipeline}};
    auto connect = std::make_unique<stages::Connect>("connect", planners);

    // grasp generator
    auto grasp_generator = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
    //grasp_generator->setProperty("object", object);

    auto grasp = std::make_unique<stages::SimpleGrasp>(std::move(grasp_generator));
    Eigen::Isometry3d tr = Eigen::Isometry3d::Identity();
    //tr.translation() = Eigen::Vector3d(0.0,0.0,0.0);
    grasp->setIKFrame(tr, ikFrame);

    // pick container, using the generated grasp generator
    auto pick = std::make_unique<stages::Pick>(std::move(grasp),"pick");
    //pick->setProperty("eef", "right_gripper");
    //pick->setProperty("object", object);
    geometry_msgs::TwistStamped approach;
    approach.header.frame_id = ikFrame;
    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.x =  0.0;
    lift.twist.linear.y =  0.0;
    lift.twist.linear.z =  1.0;
    pick->setLiftMotion(lift, 0.05, 0.10);
    current_state = pick.get();

What would be the best way to use the task properties in the different stages without redefinition (with multiple setProperty for each stages) ? I saw in the demo that the exposeTo function is used to take properties defined in the task to the propertymap of the stage. I tried with exposeTo but the properties doesn't seems to propagate well in the SimpleGrasp's computeIk substage. Let me precise what I am talking about. If I modify the grasp_generator stage by adding :>properties(), { "group", "eef","pregrasp","grasp","object" });
grasp_generator->setForwardedProperties({ "group", "eef", "pregrasp", "grasp", "object" });

I get an error :

generate grasp pose: Property 'pregrasp': undefined

in stage 'compute ik': undeclared

in stage 'grasp': undeclared

in stage 'pick': undeclared

in stage 'myTask': defined

How do I propagate the property in nested stages (Pick is taking simple_grasp and simple_grasp is taking grasp generator) ?

Thank you for the help,


edit retag flag offensive close merge delete


Where did you put the two above lines? Btw, current_state is the PARENT stage of grasp_generator stage. So can you try declare property at current_state instead?

DangTran gravatar image DangTran  ( 2020-09-04 15:07:44 -0600 )edit