Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found
Select Git revision

Target

Select target project
  • ceti/ros/packages/panda_grasping
1 result
Select Git revision
Show changes
Commits on Source (2)
......@@ -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
......@@ -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;
}
}