Python Gripper.close方法代碼示例

# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import close [as 別名]
class Abbe_Gripper(object):

	def __init__(self):
		self._gripper = Gripper('left')
		self._gripper.calibrate()
        #don't want it to drop rfid reader so disabling right gripperq
		self._gripper_right = Gripper('right')
		self._gripper_right.calibrate()

	def close(self,left=True):
		if left:
			self._gripper.close(block=True)
		else:
			self._gripper_right.close(block=True)

	def open(self,left=True):
		if left:
			self._gripper.open(block=True)
		else:
			self._gripper_right.open(block=True)

	def gripping(self,left=True):
		if left:
			return self._gripper.gripping()
		else:
			return self._gripper_right.gripping()

# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import close [as 別名]
class Abbe_Gripper(object):

	def __init__(self):
		self._gripper = Gripper('left')
		self._gripper.calibrate()
		#right gripper isn't calibrated so it won't drop the RFID reader
		#self._gripper_right = Gripper('right')
		#self._gripper_right.calibrate()

	def close(self,left=True):
		if left:
			self._gripper.close(block=True)
		else:
			self._gripper_right.close(block=True)

	def open(self,left=True):
		if left:
			self._gripper.open(block=True)
		else:
			self._gripper_right.open(block=True)

	def gripping(self,left=True):
		if left:
			return self._gripper.gripping()
		else:
			return self._gripper_right.gripping()

# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import close [as 別名]
class GripperConnect(object):
    """
    Connects wrist button presses to gripper open/close commands.

    Uses the DigitalIO Signal feature to make callbacks to connected
    action functions when the button values change.
    """

    def __init__(self, arm, lights=True):
        """
        @type arm: str
        @param arm: arm of gripper to control {left, right}
        @type lights: bool
        @param lights: if lights should activate on cuff grasp
        """
        self._arm = arm
        # inputs
        self._close_io = DigitalIO('%s_upper_button' % (arm,))  # 'dash' btn
        self._open_io = DigitalIO('%s_lower_button' % (arm,))   # 'circle' btn
        self._light_io = DigitalIO('%s_lower_cuff' % (arm,))    # cuff squeeze
        # outputs
        self._gripper = Gripper('%s' % (arm,))
        self._nav = Navigator('%s' % (arm,))

        # connect callback fns to signals
        if self._gripper.type() != 'custom':
            self._gripper.calibrate()
            self._open_io.state_changed.connect(self._open_action)
            self._close_io.state_changed.connect(self._close_action)
        else:
            msg = (("%s (%s) not capable of gripper commands."
                   " Running cuff-light connection only.") %
                   (self._gripper.name.capitalize(), self._gripper.type()))
            rospy.logwarn(msg)

        if lights:
            self._light_io.state_changed.connect(self._light_action)

        rospy.loginfo("%s Cuff Control initialized...",
                      self._gripper.name.capitalize())

    def _open_action(self, value):
        if value:
            rospy.logdebug("gripper open triggered")
            self._gripper.open()

    def _close_action(self, value):
        if value:
            rospy.logdebug("gripper close triggered")
            self._gripper.close()

    def _light_action(self, value):
        if value:
            rospy.logdebug("cuff grasp triggered")
        else:
            rospy.logdebug("cuff release triggered")
        self._nav.inner_led = value
        self._nav.outer_led = value

# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import close [as 別名]
def main():
    factor=9
    print("Right Init node")
    rospy.init_node("Right_Ik_service_client", anonymous=True)
    rate= rospy.Rate(1000)

    right_gripper=Gripper('right')
    right_gripper.calibrate()
    right_gripper.open()
    while not rospy.is_shutdown():
            
        initial_position(round(6000*factor))#10ms
        go_position(round(10000*factor),0.25)
        for x in range (0,int(round(2000*factor))):
            right_gripper.close()
            #initial_position(10000)#3ms
        go_box(round(250*factor))
        for x in range (0,int(round(800*factor))):
            right_gripper.open()
        print "Objecto dejado"

# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import close [as 別名]
class BlockStackerNode(object):
    RATE = 250 

    ############################################################################

    def __init__(self):
        self._rs = RobotEnable()
        self._cvbr = CvBridge()
        self._sm = RobotStateMachine(self)

        self.ik = IKHelper()
        self.ik.set_right(0.5, 0.0, 0.0, wait=True)

        self.left_img = None
        self.right_img = None
        self._left_camera_sub = rospy.Subscriber(
            '/cameras/left_hand_camera/image_rect_avg', 
            Image,
            self.on_left_imagemsg_received)
        self._right_camera_sub = rospy.Subscriber(
            '/cameras/right_hand_camera/image_rect_avg', 
            Image,
            self.on_right_imagemsg_received)
        self._display_pub = rospy.Publisher(
            '/robot/xdisplay', 
            Image, 
            tcp_nodelay=True,
            latch=True)

        self.range = None
        self._range_sub = rospy.Subscriber(
            '/robot/range/right_hand_range/state',
            Range,
            self.on_rangemsg_received)

        self.itb = None
        self._itb_sub = rospy.Subscriber(
            '/robot/itb/right_itb/state',
            ITBState,
            self.on_itbmsg_received)

        self.gripper = Gripper('right')
        self.gripper.calibrate()
        self.gripper.close(block=True)

    ############################################################################

    def start(self):
        self._rs.enable()
        self._sm.start()

        rate = rospy.Rate(BlockStackerNode.RATE)
        while not rospy.is_shutdown():
            self._sm.run_step()
            rate.sleep()

    ############################################################################

    def on_left_imagemsg_received(self, img_msg):
        img = self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8')
        img = cv2.resize(img, (640, 400))

        self.left_img = img.copy()
        self._sm.on_left_image_received(img)

    def on_right_imagemsg_received(self, img_msg):
        img = self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8')
        img = cv2.resize(img, (640, 400))

        self.right_img = img.copy()
        self._sm.on_right_image_received(img)

    def on_rangemsg_received(self, range_msg):
        self.range = range_msg.range

    def on_itbmsg_received(self, itb_msg):
        self.itb = itb_msg

    def display_image(self, img):
        img = cv2.resize(img, (1024, 600))

        print img.shape
        if img.shape[2] == 1:
            img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)

        img_msg = self._cvbr.cv2_to_imgmsg(img, 'bgr8')
        self._display_pub.publish(img_msg)

# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import close [as 別名]
#!/usr/bin/env python
import rospy

from baxter_interface import Gripper

if __name__ == '__main__':
    rospy.init_node('gripperTools', anonymous=True)
    gripper = Gripper('right')
    gripper.close(True)

# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import close [as 別名]

#.........這裏部分代碼省略.........
            dt.trajectory.append(rt)
        elif isinstance(trajectory, RobotState):
            dt.trajectory.append(js_to_rt(trajectory.joint_state))
        elif isinstance(trajectory, JointState):
            dt.trajectory.append(js_to_rt(trajectory))
        else:
            raise NotImplementedError("ArmCommander.display() expected type RobotTrajectory, JointTrajectory, RobotState or JointState, got {}".format(str(type(trajectory))))
        self._display_traj.publish(dt)

    def execute(self, trajectory, test=None, epsilon=0.1):
        """
        Safely executes a trajectory in joint space on the robot or simulate through RViz and its Moveit plugin (File moveit.rviz must be loaded into RViz)
        This method is BLOCKING until the command succeeds or failure occurs i.e. the user interacted with the cuff or collision has been detected
        Non-blocking needs should deal with the JointTrajectory action server
        :param trajectory: either a RobotTrajectory or a JointTrajectory
        :param test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
        :param epsilon: distance threshold on the first point. If distance with first point of the trajectory is greater than espilon execute a controlled trajectory to the first point. Put float(inf) value to bypass this functionality
        :return: True if execution ended successfully, False otherwise
        """
        def distance_to_first_point(point):
            joint_pos = np.array(point.positions)
            return np.linalg.norm(current_array - joint_pos)

        self.display(trajectory)
        with self._stop_lock:
            self._stop_reason = ''
        if isinstance(trajectory, RobotTrajectory):
            trajectory = trajectory.joint_trajectory
        elif not isinstance(trajectory, JointTrajectory):
            raise TypeError("Execute only accept RobotTrajectory or JointTrajectory")
        ftg = FollowJointTrajectoryGoal()
        ftg.trajectory = trajectory

        # check if it is necessary to move to the first point
        current = self.get_current_state()
        current_array = np.array([current.joint_state.position[current.joint_state.name.index(joint)] for joint in trajectory.joint_names])

        if distance_to_first_point(trajectory.points[0]) > epsilon:
            # convert first point to robot state
            rs = RobotState()
            rs.joint_state.name = trajectory.joint_names
            rs.joint_state.position = trajectory.points[0].positions
            # move to the first point
            self.move_to_controlled(rs)

        # execute the input trajectory
        self.client.send_goal(ftg)
        # Blocking part, wait for the callback or a collision or a user manipulation to stop the trajectory

        while self.client.simple_state != SimpleGoalState.DONE:
            if callable(test) and test():
                self.client.cancel_goal()
                return True

            if self._stop_reason!='':
                self.client.cancel_goal()
                return False

            self._rate.sleep()

        return True

    def close(self):
        """
        Open the gripper
        :return: True if an object has been grasped
        """
        return self._gripper.close(True)

    def open(self):
        """
        Close the gripper
        return: True if an object has been released
        """
        return self._gripper.open(True)

    def gripping(self):
        return self._gripper.gripping()

    def wait_for_human_grasp(self, threshold=1, rate=10, ignore_gripping=True):
        """
        Blocks until external motion is given to the arm
        :param threshold:
        :param rate: rate of control loop in Hertz
        :param ignore_gripping: True if we must wait even if no object is gripped
        """
        def detect_variation():
            new_effort = np.array(self.get_current_state([self.name+'_w0',
                                                          self.name+'_w1',
                                                          self.name+'_w2']).joint_state.effort)
            delta = np.absolute(effort - new_effort)
            return np.amax(delta) > threshold
        # record the effort at calling time
        effort = np.array(self.get_current_state([self.name+'_w0',
                                                  self.name+'_w1',
                                                  self.name+'_w2']).joint_state.effort)
        # loop till the detection of changing effort
        rate = rospy.Rate(rate)
        while not detect_variation() and not rospy.is_shutdown() and (ignore_gripping or self.gripping()):
            rate.sleep()

# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import close [as 別名]
    def __init__(self):
        pygame.init()
        rospy.init_node('node_mouse', anonymous=True)
        right_gripper=Gripper('right')
        left_gripper=Gripper('left')
        #limb_0=baxter_interface.Limb('right')
        #limb_0.set_joint_positions({'right_w2': 0.00})

        def set_w2():
            limb=baxter_interface.Limb('left')
            current_position = limb.joint_angle('left_w2')
            #send=current_position+delta
            #if(send>2.80 or send<-2.80):
            #    delta=-delta
            #    time.sleep(0.15)
            #print send
            return current_position
            #joint_command = {joint_name: send}
            #limb.set_joint_positions(joint_command)
            #if(current_position-send>0):
            #    delta=-1
        def set_s1():
            limb=baxter_interface.Limb('left')
            current_position = limb.joint_angle('left_s1')
            #send=current_position+delta
            #if(send>2.80 or send<-2.80):
            #    delta=-delta
            #    time.sleep(0.15)
            #print send
            return current_position 
        def set_e1():
            limb=baxter_interface.Limb('left')
            current_position = limb.joint_angle('left_e1')
            #send=current_position+delta
            #if(send>2.80 or send<-2.80):
            #    delta=-delta
            #    time.sleep(0.15)
            #print send
            return current_position       
            
            
        pygame.display.set_caption('Game')
        pygame.mouse.set_visible(True)
        vec = Vector3(1, 2, 3)
        right_gripper.calibrate()
        left_gripper.calibrate()
        try:
            pub = rospy.Publisher('mouse', Vector3, queue_size=1)
            rate = rospy.Rate(1000) # 10hz
            screen = pygame.display.set_mode((640,480), 0, 32)
            pygame.mixer.init()
            a=0 ,0 ,0
            while not rospy.is_shutdown():
                for event in pygame.event.get():
                    if event.type == pygame.MOUSEBUTTONDOWN or event.type == pygame.MOUSEBUTTONUP:
                        a = pygame.mouse.get_pressed()
                print a
                weq=0
                if a[0]==1:
                    left_gripper.open()
                else:
                    left_gripper.close()
                if a[2]==1:
                    right_gripper.open()
                else:
                    right_gripper.close()

                vec.x=set_w2()
                vec.y=a[1]
                vec.z=0
                print vec
                pub.publish(vec)
            pygame.mixer.quit()
            pygame.quit()

        except rospy.ROSInterruptException:
            pass

# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import close [as 別名]
Head.set_pan(head, 0.45, speed = 1.0, timeout = 0.0) #looks at cookie section

#lifts arm above cookies
left.move_to_joint_positions({'left_w0': -0.177941771394708, 'left_w1': 1.131694326262464, 'left_w2': 3.0161897241796947, 'left_e0': 0.24965537322835107, 'left_e1': 0.8038059328519568, 'left_s0': -0.7320923310183137, 'left_s1': -0.4571262747898533}
)

#lowers to grab cookie
left.move_to_joint_positions({'left_w0': -1.2367720102326147, 'left_w1': 0.5027622032294443, 'left_w2': 3.008519820240268, 'left_e0': 0.33709227813781967, 'left_e1': 1.2870098810358621, 'left_s0': -0.5944175553055978, 'left_s1': -0.3739078170470696}
)

#swoops in
left.move_to_joint_positions({'left_w0': -1.4741555371578825, 'left_w1': 0.4049709280017492, 'left_w2': 3.0158062289827234, 'left_e0': 0.3324903357741634, 'left_e1': 1.2421409429902137, 'left_s0': -0.6270146470481629, 'left_s1': -0.27228158984966094}
)

#grippers close
leftg.close()

#lifts a little bit
left.move_to_joint_positions({'left_w0': -1.474539032354854, 'left_w1': 0.3278883934105072, 'left_w2': 3.0165732193766663, 'left_e0': 0.33325732616810616, 'left_e1': 1.2950632801722606, 'left_s0': -0.6645971763513555, 'left_s1': -0.362402961137929}
)

#lifts more
left.move_to_joint_positions({'left_w0': -1.476840003536682, 'left_w1': 0.26307770512234846, 'left_w2': 3.0108207914220957, 'left_e0': 0.35856800916821546, 'left_e1': 1.4848934026730805, 'left_s0': -0.8279661302611521, 'left_s1': -0.7827136970185323}
)

#head turns to customer
head.set_pan(0.0, speed = 0.8, timeout = 0.0)

#lowers
left.move_to_joint_positions({'left_w0': -1.5079031144913617, 'left_w1': 0.2067039111675595, 'left_w2': 3.008519820240268, 'left_e0': 0.3466796580621035, 'left_e1': 0.8847234194129123, 'left_s0': -0.9955535313376335, 'left_s1': -0.11543205428837738}
)

# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import close [as 別名]

#.........這裏部分代碼省略.........
                break
            try:
                tf_listener.waitForTransform('/base', '/circle', rospy.Time(0), rospy.Duration(1.0))
                break
            except (tf.Exception, tf.ConnectivityException, tf.LookupException):
                continue

        if not no_detection:
            time_delay = rospy.get_time() - tf_listener.getLatestCommonTime('/circle' , '/base').to_sec()
            rospy.loginfo("The time delay for the detection is: %f sec" , time_delay)
            if(time_delay > 3.0):
                rospy.loginfo("Time delay of the detection exceeds 2.0 second. Stop the process")
                break

            trans, rot = tf_listener.lookupTransform('/base', '/circle', rospy.Time(0))

            rospy.loginfo(trans)

            # move to the position that is above the lime
            above_pose = get_pose_with_sleep(group_left)
            above_pose.pose.position.x = trans[0]
            above_pose.pose.position.y = trans[1]

            #plan = plan_with_orientation_lock(group_left, above_pose, side = "left", tolerance=1000)
            #group_left.execute(plan)
            execute_valid_plan(group_left, above_pose)

            # set the orientation constraint during the approach

            pick_pose = get_pose_with_sleep(group_left)
            # pick_pose.pose.position.x = trans[0]
            # pick_pose.pose.position.y = trans[1]
            pick_pose.pose.position.z = -0.025 # -0.08 for the table
            #plan = plan_with_orientation_lock(group_left, pick_pose, side = "left", tolerance=1000)
            #group_left.execute(plan)
            execute_valid_plan(group_left, pick_pose, dx = 0.02, dy = 0.02)
            gripper_left.close()
            rospy.sleep(1)

            
            print gripper_left.position()

            if gripper_left.position() > gripper_position_threshold:

                count-=1
                
                if drop_lemon:
                    initial_pose = get_pose_with_sleep(group_left)

                    lift_pose = get_pose_with_sleep(group_left)
                    lift_pose.pose.position.z += 0.35
                    # plan = plan_with_orientation_lock(group_left, lift_pose, side = "left")
                    # group_left.execute(plan)
                    execute_valid_plan(group_left, lift_pose, dx = 0.05, dy = 0.05)

                    pre_drop_pose = get_pose_with_sleep(group_left)
                    pre_drop_pose.pose.position.x = 0.75
                    pre_drop_pose.pose.position.y = 0.5
                    # execute_valid_plan(group_left, pre_drop_pose)


                    drop_pose = get_pose_with_sleep(group_left)
                    drop_pose.pose.position.x = 0.76
                    drop_pose.pose.position.y = 0.00
                    drop_pose.pose.position.z += 0.05  
                    # plan = plan_with_orientation_lock(group_left, drop_pose, side = "left", threshold= 1)
                    # group_left.execute(plan)

                    execute_valid_plan(group_left, drop_pose)
                    gripper_left.open()
                    rospy.sleep(1)

                    # execute_valid_plan(group_left, pre_drop_pose)

                    lift_pose2 = get_pose_with_sleep(group_left)
                    lift_pose2.pose.position.x = lift_pose.pose.position.x
                    lift_pose2.pose.position.y = lift_pose.pose.position.y
                    lift_pose2.pose.position.z = lift_pose.pose.position.z
                    # plan = plan_with_orientation_lock(group_left, lift_pose2, side = "left", threshold= 1)
                    # group_left.execute(plan)
                    execute_valid_plan(group_left, lift_pose)

                    # fake motion
                else:
                    initial_pose = get_pose_with_sleep(group_left)

                    lift_pose = get_pose_with_sleep(group_left)
                    lift_pose.pose.position.z += 0.1
                    execute_valid_plan(group_left, lift_pose,  dx = 0.01, dy = 0.01)
                    #plan = plan_with_orientation_lock(group_left, lift_pose, side = "left")
                    #group_left.execute(plan)

                    execute_valid_plan(group_left, initial_pose,dx = 0.01, dy = 0.01)
                    gripper_left.open()
                    rospy.sleep(1)


            else:
                gripper_left.open()
                rospy.sleep(1)