mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
pick_place_task: monitor last state before Connect
... to prune solutions as much as possible
This commit is contained in:
parent
4b5de15e1b
commit
3b05949be9
@ -179,7 +179,6 @@ bool PickPlaceTask::init() {
|
||||
* 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");
|
||||
|
||||
@ -193,8 +192,6 @@ bool PickPlaceTask::init() {
|
||||
}
|
||||
return true;
|
||||
});
|
||||
|
||||
current_state_ptr = applicability_filter.get();
|
||||
t.add(std::move(applicability_filter));
|
||||
}
|
||||
|
||||
@ -203,10 +200,12 @@ bool PickPlaceTask::init() {
|
||||
* Open Hand *
|
||||
* *
|
||||
***************************************************/
|
||||
Stage* initial_state_ptr = nullptr;
|
||||
{ // Open Hand
|
||||
auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
|
||||
stage->setGroup(hand_group_name_);
|
||||
stage->setGoal(hand_open_pose_);
|
||||
initial_state_ptr = stage.get(); // remember start state for monitoring grasp pose generator
|
||||
t.add(std::move(stage));
|
||||
}
|
||||
|
||||
@ -228,7 +227,7 @@ bool PickPlaceTask::init() {
|
||||
* 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");
|
||||
t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
|
||||
@ -263,7 +262,7 @@ bool PickPlaceTask::init() {
|
||||
stage->setPreGraspPose(hand_open_pose_);
|
||||
stage->setObject(object);
|
||||
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
|
||||
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");
|
||||
stage->attachObject(object, hand_frame_);
|
||||
attach_object_stage = stage.get();
|
||||
grasp->insert(std::move(stage));
|
||||
}
|
||||
|
||||
@ -337,11 +335,13 @@ bool PickPlaceTask::init() {
|
||||
.... * 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);
|
||||
grasp->insert(std::move(stage));
|
||||
}
|
||||
|
||||
pick_stage_ptr = grasp.get(); // remember for monitoring place pose generator
|
||||
|
||||
// Add grasp container to task
|
||||
t.add(std::move(grasp));
|
||||
}
|
||||
@ -403,7 +403,7 @@ bool PickPlaceTask::init() {
|
||||
p.pose = place_pose_;
|
||||
p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_;
|
||||
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
|
||||
auto wrapper = std::make_unique<stages::ComputeIK>("place pose IK", std::move(stage));
|
||||
|
||||
Loading…
Reference in New Issue
Block a user