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 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_; diff --git a/deep_grasp_task/src/deep_pick_place_task.cpp b/deep_grasp_task/src/deep_pick_place_task.cpp index b017948..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)); } @@ -238,7 +241,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 +369,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)); } @@ -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)); }