pick_place_task: monitor last state before Connect

... to prune solutions as much as possible
This commit is contained in:
Robert Haschke 2023-02-13 15:05:35 +01:00
parent 4b5de15e1b
commit 3b05949be9

View File

@ -179,7 +179,6 @@ bool PickPlaceTask::init() {
* Current State * * Current State *
* * * *
***************************************************/ ***************************************************/
Stage* current_state_ptr = nullptr; // Forward current_state on to grasp pose generator
{ {
auto current_state = std::make_unique<stages::CurrentState>("current state"); auto current_state = std::make_unique<stages::CurrentState>("current state");
@ -193,8 +192,6 @@ bool PickPlaceTask::init() {
} }
return true; return true;
}); });
current_state_ptr = applicability_filter.get();
t.add(std::move(applicability_filter)); t.add(std::move(applicability_filter));
} }
@ -203,10 +200,12 @@ bool PickPlaceTask::init() {
* Open Hand * * Open Hand *
* * * *
***************************************************/ ***************************************************/
Stage* initial_state_ptr = nullptr;
{ // Open Hand { // Open Hand
auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner); auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
stage->setGroup(hand_group_name_); stage->setGroup(hand_group_name_);
stage->setGoal(hand_open_pose_); stage->setGoal(hand_open_pose_);
initial_state_ptr = stage.get(); // remember start state for monitoring grasp pose generator
t.add(std::move(stage)); t.add(std::move(stage));
} }
@ -228,7 +227,7 @@ bool PickPlaceTask::init() {
* Pick Object * * Pick Object *
* * * *
***************************************************/ ***************************************************/
Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator Stage* pick_stage_ptr = nullptr;
{ {
auto grasp = std::make_unique<SerialContainer>("pick object"); auto grasp = std::make_unique<SerialContainer>("pick object");
t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" }); t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
@ -263,7 +262,7 @@ bool PickPlaceTask::init() {
stage->setPreGraspPose(hand_open_pose_); stage->setPreGraspPose(hand_open_pose_);
stage->setObject(object); stage->setObject(object);
stage->setAngleDelta(M_PI / 12); stage->setAngleDelta(M_PI / 12);
stage->setMonitoredStage(current_state_ptr); // Hook into current state stage->setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions
// Compute IK // Compute IK
auto wrapper = std::make_unique<stages::ComputeIK>("grasp pose IK", std::move(stage)); auto wrapper = std::make_unique<stages::ComputeIK>("grasp pose IK", std::move(stage));
@ -302,7 +301,6 @@ bool PickPlaceTask::init() {
{ {
auto stage = std::make_unique<stages::ModifyPlanningScene>("attach object"); auto stage = std::make_unique<stages::ModifyPlanningScene>("attach object");
stage->attachObject(object, hand_frame_); stage->attachObject(object, hand_frame_);
attach_object_stage = stage.get();
grasp->insert(std::move(stage)); grasp->insert(std::move(stage));
} }
@ -337,11 +335,13 @@ bool PickPlaceTask::init() {
.... * Forbid collision (object support) * .... * Forbid collision (object support) *
***************************************************/ ***************************************************/
{ {
auto stage = std::make_unique<stages::ModifyPlanningScene>("forbid collision (object,surface)"); auto stage = std::make_unique<stages::ModifyPlanningScene>("forbid collision (object,support)");
stage->allowCollisions({ object }, support_surfaces_, false); stage->allowCollisions({ object }, support_surfaces_, false);
grasp->insert(std::move(stage)); grasp->insert(std::move(stage));
} }
pick_stage_ptr = grasp.get(); // remember for monitoring place pose generator
// Add grasp container to task // Add grasp container to task
t.add(std::move(grasp)); t.add(std::move(grasp));
} }
@ -403,7 +403,7 @@ bool PickPlaceTask::init() {
p.pose = place_pose_; p.pose = place_pose_;
p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_; p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_;
stage->setPose(p); stage->setPose(p);
stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage stage->setMonitoredStage(pick_stage_ptr); // hook into successful pick solutions
// Compute IK // Compute IK
auto wrapper = std::make_unique<stages::ComputeIK>("place pose IK", std::move(stage)); auto wrapper = std::make_unique<stages::ComputeIK>("place pose IK", std::move(stage));