diff --git a/include/grasp_util.h b/include/grasp_util.h index 325a826306b0463eb56c2cda7adad2491e62a7e9..aaa7e2f517d636eec4ce3739a70d959d08d6a57f 100644 --- a/include/grasp_util.h +++ b/include/grasp_util.h @@ -179,4 +179,12 @@ private: tf2::Quaternion approach_orientation, float y_distance_diff, float x_distance_diff, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2); }; + + /** + * Homes the gripper (in case it was abused before and is in an inconsistent state) + * This method is blocking with a maximum runtime of ~30s + * @return true if homing succeeded, false otherwise + */ + static bool homeGripper(); + #endif //PANDA_GRASPING_GRASP_UTIL_H diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp index 7d9924561a137645e096641c6a4dbb3cb198b6fe..4698c3dc0902cc0680f1890f57e0987cb2f905f4 100644 --- a/src/grasping/grasp_util.cpp +++ b/src/grasping/grasp_util.cpp @@ -3,6 +3,8 @@ // #include "grasp_util.h" #include <tf2/transform_datatypes.h> +#include <franka_gripper/HomingAction.h> +#include <franka_gripper/HomingGoal.h> void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount) { posture.joint_names.resize(2); @@ -333,3 +335,21 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val); return false; } + +bool GraspUtil::homeGripper() { + actionlib::SimpleActionClient<franka_gripper::HomingAction> ac("/franka_gripper/homing", true); + if (!ac.waitForServer(ros::Duration(10.0))) { + ROS_ERROR("Gripper homing failed (action server not available)."); + return false; + } + franka_gripper::HomingGoal goal; + ac.sendGoal(goal); + if (ac.waitForResult(ros::Duration(20.0))) { + actionlib::SimpleClientGoalState state = ac.getState(); + ROS_INFO_STREAM("Homing finished: " << state.toString()); + return state == actionlib::SimpleClientGoalState::SUCCEEDED; + } else { + ROS_ERROR("Gripper homing failed (with timeout)."); + return false; + } +}