Python baxter_interface.Gripper類代碼示例

    def __init__(
            self, 
            camera_averaging=False):
        self._cvbr = CvBridge()

        self.rs = RobotEnable()
        self.ik = IKHelper()

        camera_topic = '/cameras/{0}_camera/image_rect'
        if camera_averaging:
            camera_topic += '_avg'

        self.left_img = None
        self._left_camera_sub = rospy.Subscriber(
            camera_topic.format('left_hand'), 
            Image,
            self._on_left_imagemsg_received)

        self.right_img = None
        self._right_camera_sub = rospy.Subscriber(
            camera_topic.format('right_hand'), 
            Image,
            self._on_right_imagemsg_received)

        self.head_img = None
        self._head_camera_sub = rospy.Subscriber(
            camera_topic.format('head'), 
            Image,
            self._on_head_imagemsg_received)

        self.left_itb = None
        self._left_itb_sub = rospy.Subscriber(
            '/robot/itb/left_itb/state',
            ITBState,
            self._on_left_itbmsg_received)

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

        self.left_gripper = Gripper('left')
        self.right_gripper = Gripper('right')

        self.head = Head()

        self._display_pub = rospy.Publisher(
            '/robot/xdisplay', 
            Image, 
            tcp_nodelay=True,
            latch=True)

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

    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.gripper = Gripper('right')
        self.gripper.calibrate()
        self.gripper.close(block=True)

        self.right_img = None
        self._right_camera_sub = rospy.Subscriber(
            '/cameras/right_hand_camera/image_rect_avg', 
            Image,
            self.on_right_imagemsg_received)

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

    	self._display_pub = rospy.Publisher(
            '/robot/xdisplay', 
            Image, 
            tcp_nodelay=True,
            latch=True)

 def __init__(self, pr_init, pl_init):
     
     self.centerx = 365
     self.centery = 140
     self.coefx = 0.1/(526-369)
     self.coefy = 0.1/(237-90)
     self.count = 0
     self.right = MoveGroupCommander("right_arm")
     self.left = MoveGroupCommander("left_arm")
     self.gripper = Gripper("right")
     self.gripper.calibrate()
     self.gripper.set_moving_force(0.1)
     self.gripper.set_holding_force(0.1)
     self.pr = pr_init
     self.pl = pl_init
     self.angle = 0
     self.position_list = (0.7,-0.3,-0.06,0,0.0,self.angle)
     self.busy = False
     self.blank_image = np.zeros((400,640,3),np.uint8)
     cv2.putText(self.blank_image,"DEMO", (180,200), cv2.FONT_HERSHEY_SIMPLEX, fontScale = 4, color = 255,  thickness = 10 )
     self.movenum = 0
     self.gripper_distance = 100
     self.subscribe_gripper()
     self.robotx = -1
     self.roboty = -1
     self.H = homography()
     self.framenumber = 0
     self.history = np.arange(0,20)*-1
     self.newPosition = True
     self.bowlcamera = None

 def __init__(self):
     
     self.centerx = 365
     self.centery = 120
     self.coefx = 0.1/(526-369)
     self.coefy = 0.1/(237-90)
     self.count = 0
     self.hdr = Header(stamp=rospy.Time.now(), frame_id='base')
     self.gripper = Gripper("right")
     self.gripper.calibrate()
     self.gripper.set_moving_force(0.1)
     rospy.sleep(0.5)
     self.gripper.set_holding_force(0.1)
     rospy.sleep(0.5)
     self.busy = False
     self.gripper_distance = 100
     self.subscribe_gripper()
     self.robotx = -1
     self.roboty = -1
     self.framenumber = 0
     self.history = np.arange(0,20)*-1
     self.newPosition = True
     self.bowlcamera = None
     self.kin = baxter_kinematics('right')
     self.J = self.kin.jacobian()
     self.J_inv = self.kin.jacobian_pseudo_inverse()
     self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32)
     self.control_arm = Limb("right")
     self.control_joint_names = self.control_arm.joint_names()
     self.dx = 0
     self.dy = 0
     self.distance = 1000
     self.finish = False
     self.found = 0
     ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)

    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 __init__(self, name, rate=100, fk='robot', ik='trac', default_kv_max=1., default_ka_max=0.5):
        """
        :param name: 'left' or 'right'
        :param rate: Rate of the control loop for execution of motions
        :param fk: Name of the Forward Kinematics solver, "robot", "kdl", "trac" or "ros"
        :param ik: Name of the Inverse Kinematics solver, "robot", "kdl", "trac" or "ros"
        :param default_kv_max: Default K maximum for velocity
        :param default_ka_max: Default K maximum for acceleration
        """
        Limb.__init__(self, name)
        self._world = 'base'
        self.kv_max = default_kv_max
        self.ka_max = default_ka_max
        self._gripper = Gripper(name)
        self._rate = rospy.Rate(rate)
        self._tf_listener = TransformListener()
        self.recorder = Recorder(name)

        # Kinematics services: names and services (if relevant)
        self._kinematics_names = {'fk': {'ros': 'compute_fk'},
                                  'ik': {'ros': 'compute_ik',
                                         'robot': 'ExternalTools/{}/PositionKinematicsNode/IKService'.format(name),
                                         'trac': 'trac_ik_{}'.format(name)}}

        self._kinematics_services = {'fk': {'ros': {'service': rospy.ServiceProxy(self._kinematics_names['fk']['ros'], GetPositionFK),
                                                    'func': self._get_fk_ros},
                                            'kdl': {'func': self._get_fk_pykdl},
                                            'robot': {'func': self._get_fk_robot}},
                                     'ik': {'ros': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['ros'], GetPositionIK),
                                                    'func': self._get_ik_ros},
                                            'robot': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['robot'], SolvePositionIK),
                                                      'func': self._get_ik_robot},
                                            'trac': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['trac'], GetConstrainedPositionIK),
                                                     'func': self._get_ik_trac},
                                            'kdl': {'func': self._get_ik_pykdl}}}
        self._selected_ik = ik
        self._selected_fk = fk

        # Kinematics services: PyKDL
        self._kinematics_pykdl = baxter_kinematics(name)

        if self._selected_ik in self._kinematics_names['ik']:
            rospy.wait_for_service(self._kinematics_names['ik'][self._selected_ik])
        if self._selected_fk in self._kinematics_names['fk']:
            rospy.wait_for_service(self._kinematics_names['fk'][self._selected_fk])

        # Execution attributes
        rospy.Subscriber('/robot/limb/{}/collision_detection_state'.format(name), CollisionDetectionState, self._cb_collision, queue_size=1)
        rospy.Subscriber('/robot/digital_io/{}_lower_cuff/state'.format(name), DigitalIOState, self._cb_dig_io, queue_size=1)
        self._stop_reason = ''  # 'cuff' or 'collision' could cause a trajectory to be stopped
        self._stop_lock = Lock()
        action_server_name = "/robot/limb/{}/follow_joint_trajectory".format(self.name)
        self.client = SimpleActionClient(action_server_name, FollowJointTrajectoryAction)

        self._display_traj = rospy.Publisher("/move_group/display_planned_path", DisplayTrajectory, queue_size=1)
        self._gripper.calibrate()

        self.client.wait_for_server()

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()

	def __init__(self):
		
		self.rightg = Gripper('right')
		self.rightg.set_holding_force(100)
		self.leftg = Gripper('left')
		self.right = Limb('right')
		self.left = Limb('left')
		self.head = Head()

		self.angles= list()

		
		rospy.Subscriber("command", String, self.command)
		rospy.spin()

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"

class Baxter_Deli(object):
	
	def __init__(self):
		
		self.rightg = Gripper('right')
		self.rightg.set_holding_force(100)
		self.leftg = Gripper('left')
		self.right = Limb('right')
		self.left = Limb('left')
		self.head = Head()

		self.angles= list()

		
		rospy.Subscriber("command", String, self.command)
		rospy.spin()

	def command(self, comm):
		if comm.data == "left":
			self.angles.append(self.left.joint_angles())
		elif comm.data == "right":
			self.angles.append(self.right.joint_angles())
		elif comm.data == "done":
			print self.angles

class track():
    
    def __init__(self):
        
        self.centerx = 365
        self.centery = 120
        self.coefx = 0.1/(526-369)
        self.coefy = 0.1/(237-90)
        self.count = 0
        self.hdr = Header(stamp=rospy.Time.now(), frame_id='base')
        self.gripper = Gripper("right")
        self.gripper.calibrate()
        self.gripper.set_moving_force(0.1)
        rospy.sleep(0.5)
        self.gripper.set_holding_force(0.1)
        rospy.sleep(0.5)
        self.busy = False
        self.gripper_distance = 100
        self.subscribe_gripper()
        self.robotx = -1
        self.roboty = -1
        self.framenumber = 0
        self.history = np.arange(0,20)*-1
        self.newPosition = True
        self.bowlcamera = None
        self.kin = baxter_kinematics('right')
        self.J = self.kin.jacobian()
        self.J_inv = self.kin.jacobian_pseudo_inverse()
        self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32)
        self.control_arm = Limb("right")
        self.control_joint_names = self.control_arm.joint_names()
        self.dx = 0
        self.dy = 0
        self.distance = 1000
        self.finish = False
        self.found = 0
        ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)


    def _read_distance(self,data):
        self.distance = data.range

    def vision_request_right(self, controlID, requestID):
        
        try:
            
            rospy.wait_for_service('vision_server_vertical')
            vision_server_req = rospy.ServiceProxy('vision_server_vertical', VisionVertical)
            return vision_server_req(controlID, requestID)
        
        except (rospy.ServiceException,rospy.ROSInterruptException), e:
            print "Service call failed: %s" % e
            self.clean_shutdown_service()

  def __init__(self, dmp_record_file_dir, dmp_dims=7,
               lateral_record_file=LATERAL_RECORD_FILE,
               vertical_record_file=VERTICAL_RECORD_FILE,
               return_record_file=RETURN_RECORD_FILE, dummy=False):
      self.K = 100 # Spring constant, value taken from http://wiki.ros.org/dmp
      self.D = 2.0 * np.sqrt(self.K) # Damping constant, value taken from http://wiki.ros.org/dmp
      self.dt = 1.0 # Time resolution, Value taken from http://wiki.ros.org/dmp
      self.num_bases = 4 # No of basis functions, value taken from http://wiki.ros.org/dmp
 
      self.dmp_record_file_dir = dmp_record_file_dir
      self.dmp_dims = dmp_dims
      self.lateral_record_file = os.path.join(self.dmp_record_file_dir, lateral_record_file)
      self.vertical_record_file = os.path.join(self.dmp_record_file_dir, vertical_record_file)
      self.return_record_file = os.path.join(self.dmp_record_file_dir, return_record_file)
      self.dummy = dummy
      self.tf_listener = tf.TransformListener()
      # the init velocity and goal threshold doesn't change across dmps, so just store them now
      self.init_dot = []
      for i in range(self.dmp_dims):
          self.init_dot.append(0)
      self.goal_thresh = []
      for i in range(self.dmp_dims):
          self.goal_thresh.append(0)
      # Initialize move it group
      moveit_commander.roscpp_initialize(sys.argv)
      self.robot = moveit_commander.RobotCommander()
      self.group = moveit_commander.MoveGroupCommander("right_arm")
      self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                                          moveit_msgs.msg.DisplayTrajectory, queue_size=10)
      self.integrate_iter = 5       # value taken from http://wiki.ros.org/dmp
      self.seq_length = -1
      self.right_gripper = Gripper('right')
      # save the initial state
      try:
          self.init_pos, self.init_orient = self.tf_listener.lookupTransform('/base', '/right_gripper', rospy.Time(0))         #Plan to a different cup than demo
      except:
          time.sleep(1)
          self.init_pos, self.init_orient = self.tf_listener.lookupTransform('/base', '/right_gripper', rospy.Time(0))
      self.right_gripper.calibrate()

class track():
    
    def __init__(self, pr_init, pl_init):
        
        self.centerx = 365
        self.centery = 140
        self.coefx = 0.1/(526-369)
        self.coefy = 0.1/(237-90)
        self.count = 0
        self.right = MoveGroupCommander("right_arm")
        self.left = MoveGroupCommander("left_arm")
        self.gripper = Gripper("right")
        self.gripper.calibrate()
        self.gripper.set_moving_force(0.1)
        self.gripper.set_holding_force(0.1)
        self.pr = pr_init
        self.pl = pl_init
        self.angle = 0
        self.position_list = (0.7,-0.3,-0.06,0,0.0,self.angle)
        self.busy = False
        self.blank_image = np.zeros((400,640,3),np.uint8)
        cv2.putText(self.blank_image,"DEMO", (180,200), cv2.FONT_HERSHEY_SIMPLEX, fontScale = 4, color = 255,  thickness = 10 )
        self.movenum = 0
        self.gripper_distance = 100
        self.subscribe_gripper()
        self.robotx = -1
        self.roboty = -1
        self.H = homography()
        self.framenumber = 0
        self.history = np.arange(0,20)*-1
        self.newPosition = True
        self.bowlcamera = None

    def vision_request_right(self, controlID, requestID):
        rospy.wait_for_service('vision_server_vertical')
        try:
            vision_server_req = rospy.ServiceProxy('vision_server_vertical', Vision)
            return vision_server_req(controlID, requestID)
        
        except rospy.ServiceException, e:
            print "Service call failed: %s" % e

#oatmeal raisin cookie( one ):
	#gripper: narrow (left)
	#holding force: 40

#replace cookie with drink 

#individually wrapped triangle sandwich will not do

#!/usr/bin/env python

import rospy, baxter_interface
from baxter_interface import Gripper, Head, Limb

rospy.init_node('deli_Baxter')
rightg = Gripper('right')
rightg.set_holding_force(100)
leftg = Gripper('left')
right = Limb('right')
left = Limb('left')
head = Head()
head.set_pan(0.0, speed = 0.8, timeout = 0.0)

Gripper.calibrate(rightg)
Gripper.calibrate(leftg)

neutral_right ={'right_s0': 0.5971020216843973, 'right_s1': -0.4237621926533455, 'right_w0': 0.4226117070624315, 'right_w1': 0.8471408901097197, 'right_w2': -0.9725438195193523, 'right_e0': -0.40727189918357737, 'right_e1': 1.161990446823201}

neutral_left = {'left_w0': -0.3006602344255411, 'left_w1': 0.5549175500175484, 'left_w2': 3.050704291907117, 'left_e0': 0.5161845351234418, 'left_e1': 1.1984224905354794, 'left_s0': -0.8904758473674826, 'left_s1': -0.38387869216832476}