diff --git a/include/grasp_util.h b/include/grasp_util.h index 8953a48c703085c5d9ea45eba269487a3b7bccb5..66bdb7f373f7a92aaa51e3f2490c0ea6aa3eda33 100644 --- a/include/grasp_util.h +++ b/include/grasp_util.h @@ -90,6 +90,13 @@ public: 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); + /** + * 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 e8d90c28627ad6140f7255b79374ebeafc7d1585..d6ab94d3f7f6ff70541b29365a8ef00ee1e779a7 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); @@ -158,3 +160,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; + } +}