diff --git a/include/grasp_util.h b/include/grasp_util.h index a6545b0a31a89e72ec0ac13dac459d760221e42a..8953a48c703085c5d9ea45eba269487a3b7bccb5 100644 --- a/include/grasp_util.h +++ b/include/grasp_util.h @@ -76,7 +76,7 @@ public: */ bool pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose, geometry_msgs::Vector3 dimensions, double open_amount, std::string supporting_surface_id, - std::string object_to_pick_id); + std::string object_to_pick_id, bool plan_only = false); /** * Util method to place an object from above. @@ -88,7 +88,7 @@ public: * @param object_to_place_id ID of the object to place */ bool placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose, - double open_amount, std::string supporting_surface_id, std::string object_to_place_id); + double open_amount, std::string supporting_surface_id, std::string object_to_place_id, bool plan_only = false); }; diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp index 64f79e29e3df4befb46ec8c513ade03e6b08fb31..0a8698574b19329ae7b94cc04b2d99ac7a8403fb 100644 --- a/src/grasping/grasp_util.cpp +++ b/src/grasping/grasp_util.cpp @@ -75,7 +75,7 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m bool GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose, geometry_msgs::Vector3 dimensions, - double open_amount, std::string supporting_surface_id, std::string object_to_pick_id) { + double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only) { std::vector<moveit_msgs::Grasp> grasps; grasps.resize(1); @@ -112,7 +112,14 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr } move_group.setSupportSurfaceName(supporting_surface_id); - moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps); + + moveit_msgs::MoveItErrorCodes miec; + + if(plan_only){ + miec = move_group.pick(object_to_pick_id, grasps, true); + }else{ + miec = move_group.pick(object_to_pick_id, grasps); + } if (miec.val == miec.SUCCESS) { return true; @@ -124,7 +131,7 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr bool GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose, - double open_amount, std::string supporting_surface_id, std::string object_to_place_id) { + double open_amount, std::string supporting_surface_id, std::string object_to_place_id, bool plan_only) { std::vector<moveit_msgs::PlaceLocation> place_location; place_location.resize(1); @@ -147,7 +154,14 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g setupOpenGripper(place_location[0].post_place_posture, open_amount); move_group.setSupportSurfaceName(supporting_surface_id); - moveit_msgs::MoveItErrorCodes miec = move_group.place(object_to_place_id, place_location); + + moveit_msgs::MoveItErrorCodes miec; + + if(plan_only){ + miec = move_group.place(object_to_place_id, place_location, true); + }else{ + miec = move_group.place(object_to_place_id, place_location); + } if (miec.val == miec.SUCCESS) { return true;