From 81d584c090f39cfc302c10018ffc2c80ad5e507d Mon Sep 17 00:00:00 2001 From: Hongzhuo Liang Date: Thu, 24 Sep 2020 09:59:28 +0200 Subject: [PATCH 1/4] Update deep_pick_place_task.cpp fix group name undefined bug similar in https://github.com/ros-planning/moveit_task_constructor/pull/205 --- deep_grasp_task/src/deep_pick_place_task.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/deep_grasp_task/src/deep_pick_place_task.cpp b/deep_grasp_task/src/deep_pick_place_task.cpp index b017948..ec1ac92 100644 --- a/deep_grasp_task/src/deep_pick_place_task.cpp +++ b/deep_grasp_task/src/deep_pick_place_task.cpp @@ -238,7 +238,7 @@ void DeepPickPlaceTask::init() ***************************************************/ { auto stage = std::make_unique("close hand", sampling_planner); - stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_group_name_); + stage->setGroup(hand_group_name_); stage->setGoal(hand_close_pose_); grasp->insert(std::move(stage)); } @@ -366,7 +366,7 @@ void DeepPickPlaceTask::init() *****************************************************/ { auto stage = std::make_unique("open hand", sampling_planner); - stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_group_name_); + stage->setGroup(hand_group_name_); stage->setGoal(hand_open_pose_); place->insert(std::move(stage)); } From f3840e4c3367d92ad6b36675634989618945633d Mon Sep 17 00:00:00 2001 From: Hongzhuo Liang Date: Fri, 25 Sep 2020 11:43:07 +0200 Subject: [PATCH 2/4] make the approach vector as a parameter make the approach vector as a parameter --- deep_grasp_task/src/deep_pick_place_task.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/deep_grasp_task/src/deep_pick_place_task.cpp b/deep_grasp_task/src/deep_pick_place_task.cpp index ec1ac92..4fe73b6 100644 --- a/deep_grasp_task/src/deep_pick_place_task.cpp +++ b/deep_grasp_task/src/deep_pick_place_task.cpp @@ -81,6 +81,9 @@ void DeepPickPlaceTask::loadParameters() errors += !rosparam_shortcuts::get(LOGNAME, pnh, "action_name", action_name_); // Pick/Place metrics + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_vector_x", approach_vector_.x); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_vector_y", approach_vector_.y); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_vector_z", approach_vector_.z); errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_min_dist", approach_object_min_dist_); errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_max_dist", approach_object_max_dist_); errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_min_dist", lift_object_min_dist_); @@ -195,7 +198,7 @@ void DeepPickPlaceTask::init() // Set hand forward direction geometry_msgs::Vector3Stamped vec; vec.header.frame_id = hand_frame_; - vec.vector.z = 1.0; + vec.vector = approach_vector_; stage->setDirection(vec); grasp->insert(std::move(stage)); } @@ -402,7 +405,10 @@ void DeepPickPlaceTask::init() stage->properties().set("marker_ns", "retreat"); geometry_msgs::Vector3Stamped vec; vec.header.frame_id = hand_frame_; - vec.vector.z = -1.0; + retreat_vector_.x = -approach_vector_.x; + retreat_vector_.y = -approach_vector_.y; + retreat_vector_.z = -approach_vector_.z; + vec.vector = retreat_vector_; stage->setDirection(vec); place->insert(std::move(stage)); } From a852a0a3d6dfab36fb3f2d0f8f95ed25f30bb887 Mon Sep 17 00:00:00 2001 From: Hongzhuo Liang Date: Fri, 25 Sep 2020 11:45:58 +0200 Subject: [PATCH 3/4] make the approach vector as a parameter make the approach vector as a parameter --- deep_grasp_task/include/deep_grasp_task/deep_pick_place_task.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/deep_grasp_task/include/deep_grasp_task/deep_pick_place_task.h b/deep_grasp_task/include/deep_grasp_task/deep_pick_place_task.h index f2fc514..ac1ca8e 100644 --- a/deep_grasp_task/include/deep_grasp_task/deep_pick_place_task.h +++ b/deep_grasp_task/include/deep_grasp_task/deep_pick_place_task.h @@ -117,6 +117,8 @@ class DeepPickPlaceTask // Pick metrics Eigen::Isometry3d grasp_frame_transform_; + geometry_msgs::Vector3 approach_vector_; + geometry_msgs::Vector3 retreat_vector_; double approach_object_min_dist_; double approach_object_max_dist_; double lift_object_min_dist_; From 397982ac87d8ec262d4825dcde38e13b1d08677a Mon Sep 17 00:00:00 2001 From: Hongzhuo Liang Date: Fri, 25 Sep 2020 11:48:05 +0200 Subject: [PATCH 4/4] make the approach vector as a parameter make the approach vector as a parameter --- deep_grasp_task/config/panda_object.yaml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/deep_grasp_task/config/panda_object.yaml b/deep_grasp_task/config/panda_object.yaml index a242495..147f74f 100644 --- a/deep_grasp_task/config/panda_object.yaml +++ b/deep_grasp_task/config/panda_object.yaml @@ -61,6 +61,11 @@ grasp_frame_transform: [0, 0, 0.1, 1.571, 0.785, 1.571] place_pose: [0.6, -0.15, 0, 0, 0, 0] place_surface_offset: 0.0001 # place offset from table +# Approach vector in the hand frame +approach_vector_x: 0.0 +approach_vector_y: 0.0 +approach_vector_z: 1.0 + # Valid distance range when approaching an object for picking approach_object_min_dist: 0.1 approach_object_max_dist: 0.15