Moveit Task constructor properties propagation
ROS distro : kinetic
Moveit task constructor branch : master
Moveit branch : master
Hi,
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 :
pickTask.setProperty("group",planGroup);
pickTask.setProperty("eef","right_gripper");
pickTask.setProperty("pregrasp","right_open");
pickTask.setProperty("grasp","right_close");
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();
pickTask.add(std::move(initial));
{
// connect to pick
stages::Connect::GroupPlannerVector planners = {{eef, pipeline}, {planGroup, pipeline}};
auto connect = std::make_unique<stages::Connect>("connect", planners);
connect->properties().configureInitFrom(Stage::PARENT);
pickTask.add(std::move(connect));
}
{
// grasp generator
auto grasp_generator = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
grasp_generator->setAngleDelta(M_PI/2);
//grasp_generator->setPreGraspPose("right_open");
//grasp_generator->setGraspPose("right_close");
//grasp_generator->setProperty("object", object);
grasp_generator->setMonitoredStage(current_state);
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);
grasp->setMaxIKSolutions(10);
// 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("group","right_arm");
//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();
pickTask.add(std::move(pick));
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 :
pickTask.properties().exposeTo(grasp_generator->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 simplegrasp and simplegrasp is taking grasp generator) ?
Thank you for the help,
regards,
Asked by Madcreator on 2020-07-08 04:36:07 UTC
Comments
Where did you put the two above lines? Btw,
current_state
is thePARENT
stage ofgrasp_generator
stage. So can you try declare property atcurrent_state
instead?Asked by DangTran on 2020-09-04 15:07:44 UTC