def process_grasp_action(self, msg): gripper_command = WubbleGripperGoal() if msg.goal == GraspHandPostureExecutionGoal.GRASP: rospy.loginfo('Received GRASP request') # if not msg.goal.grasp.position: # msg = 'wubble gripper grasp execution: position vector empty in requested grasp' # rospy.logerr(msg) # self.action_server.set_aborted(text=msg) # return # gripper_command.command = WubbleGripperGoal.CLOSE_GRIPPER gripper_command.torque_limit = 0.4 gripper_command.dynamic_torque_control = True gripper_command.pressure_upper = 1900.0 gripper_command.pressure_lower = 1800.0 elif msg.goal == GraspHandPostureExecutionGoal.PRE_GRASP: rospy.loginfo('Received PRE_GRASP request') gripper_command.command = WubbleGripperGoal.OPEN_GRIPPER gripper_command.torque_limit = 0.6 elif msg.goal == GraspHandPostureExecutionGoal.RELEASE: rospy.loginfo('Received RELEASE request') gripper_command.command = WubbleGripperGoal.OPEN_GRIPPER gripper_command.torque_limit = 0.6 else: msg = 'wubble gripper grasp execution: unknown goal code (%d)' % msg.goal rospy.logerr(msg) self.action_server.set_aborted(text=msg) self.gripper_action_client.send_goal(gripper_command) self.gripper_action_client.wait_for_result() self.action_server.set_succeeded()
def gentle_close_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.GRASP self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() goal = WubbleGripperGoal() goal.command = WubbleGripperGoal.CLOSE_GRIPPER goal.torque_limit = 0.0 goal.dynamic_torque_control = False self.gripper_controller.send_goal(goal) self.gripper_controller.wait_for_result()