diff --git a/include/grasp_util.h b/include/grasp_util.h index 411ca4b9fa58710476521ca5f044fabe4e54e2f2..33793a25d87a0123a1a6975ad8d866cd05b5b8ff 100644 --- a/include/grasp_util.h +++ b/include/grasp_util.h @@ -122,7 +122,7 @@ public: * @return */ geometry_msgs::Pose - getPickFromSidePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Quaternion gripper_orientation, tf2::Quaternion approach_orientation, int y_distance_diff, int x_distance_diff, + getPickFromSidePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Quaternion gripper_orientation, tf2::Quaternion approach_orientation, float y_distance_diff, float x_distance_diff, tf2::Quaternion object_orientation = {0, 0, 0, 1}, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2); /** * Util method to pick an object from above. @@ -138,18 +138,30 @@ public: geometry_msgs::Vector3 dimensions, double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only = false); + /** + * + * @param move_group + * @param pick_pose + * @param object_to_pick + * @param open_amount + * @param supporting_surface_id + * @param object_to_pick_id + * @param plan_only + * @return + */ bool pickFromSide(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose, - shape_msgs::SolidPrimitive &object_to_pick, + shape_msgs::SolidPrimitive &object_to_pick, geometry_msgs::Vector3 &approach_direction, double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only = false); /** - * Util method to place an object from above. - * @param move_group an initialized MoveGroupInterface instance - * @param place_pose target place pose for the object - * @param close_amount total close amount (space between "forks") - * @param open_amount total opening amount (space between "forks") - * @param supporting_surface_id ID of surface where object is placed on (e.g. the table) - * @param object_to_place_id ID of the object to place + * + * @param move_group + * @param place_pose + * @param open_amount + * @param supporting_surface_id + * @param object_to_place_id + * @param plan_only + * @return */ 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, bool plan_only = false); diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp index 195823f18343ca11d794530683924957be5fa0c7..64399434b5c074228af995fec9402c169336f2b5 100644 --- a/src/grasping/grasp_util.cpp +++ b/src/grasping/grasp_util.cpp @@ -35,7 +35,7 @@ GraspUtil::getPickFromMinusXSidePose(geometry_msgs::Pose &object_pose_msg, geome double approach_roll, approach_pitch, approach_yaw; approach_roll = -M_PI_2; approach_pitch = -M_PI_4; - approach_yaw = -M_PI_4 - M_PI_2; + approach_yaw = -M_PI_4; tf2::Quaternion approach_orientation; approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw); @@ -90,7 +90,7 @@ GraspUtil::getPickFromPlusYSidePose(geometry_msgs::Pose &object_pose_msg, geomet geometry_msgs::Pose GraspUtil::getPickFromSidePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Quaternion gripper_orientation_msg, - tf2::Quaternion approach_orientation, int y_distance_diff, int x_distance_diff, tf2::Quaternion object_orientation, + tf2::Quaternion approach_orientation, float y_distance_diff, float x_distance_diff, tf2::Quaternion object_orientation, double reach){ // object orientation @@ -156,7 +156,7 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m bool GraspUtil::pickFromSide(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose, - shape_msgs::SolidPrimitive &object_to_pick, + shape_msgs::SolidPrimitive &object_to_pick, geometry_msgs::Vector3 &approach_direction, double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only) { std::vector<moveit_msgs::Grasp> grasps; @@ -164,9 +164,12 @@ GraspUtil::pickFromSide(moveit::planning_interface::MoveGroupInterface &move_gro grasps[0].grasp_pose.header.frame_id = "panda_link0"; grasps[0].grasp_pose.pose = pick_pose; + geometry_msgs::Vector3Stamped stamped_approach_dir; + stamped_approach_dir.vector = approach_direction; + // Setup pre-grasp approach grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0"; - grasps[0].pre_grasp_approach.direction.vector.z = -1.0; + grasps[0].pre_grasp_approach.direction = stamped_approach_dir; grasps[0].pre_grasp_approach.min_distance = 0.095; grasps[0].pre_grasp_approach.desired_distance = 0.115; @@ -274,6 +277,7 @@ GraspUtil::placeFromSide(moveit::planning_interface::MoveGroupInterface &move_gr place_location[0].place_pose.header.frame_id = "panda_link0"; place_location[0].place_pose.pose.orientation.w = 1; place_location[0].place_pose.pose.position = place_pose.position; + place_location[0].place_pose.pose.position.z = place_location[0].place_pose.pose.position.z; place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0"; place_location[0].pre_place_approach.direction.vector.z = -1.0; @@ -281,7 +285,7 @@ GraspUtil::placeFromSide(moveit::planning_interface::MoveGroupInterface &move_gr place_location[0].pre_place_approach.desired_distance = 0.115; place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0"; - place_location[0].post_place_retreat.direction.vector.y = -1.0; + place_location[0].post_place_retreat.direction.vector.z = 1.0; place_location[0].post_place_retreat.min_distance = 0.1; place_location[0].post_place_retreat.desired_distance = 0.25;