From 5a0059dc83e60307797787b0f2d594e0ca30db48 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 29 Apr 2025 14:23:31 +0200 Subject: [PATCH] Fix pick+place: connect should plan both, arm and hand motion --- demo/src/pick_place_task.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 92c4f72e..39f8e356 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -220,8 +220,9 @@ bool PickPlaceTask::init() { ***************************************************/ // Connect initial open-hand state with pre-grasp pose defined in the following { - auto stage = std::make_unique( - "move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); + stages::Connect::GroupPlannerVector planners = { { arm_group_name_, sampling_planner }, + { hand_group_name_, sampling_planner } }; + auto stage = std::make_unique("move to pick", planners); stage->setTimeout(5.0); stage->properties().configureInitFrom(Stage::PARENT); t.add(std::move(stage));