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()
Esempio n. 2
0
 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()
Esempio n. 3
0
    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()
    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()