def _get_gripper(self, gripper_reverse): try: self._gripper=intera_interface.get_current_gripper_interface() self._is_clicksmart = isinstance(self._gripper, intera_interface.SimpleClickSmartGripper) self._gripper_reverse=gripper_reverse if self._is_clicksmart: if self._gripper.needs_init(): self._gripper.initialize() _signals=self._gripper.get_ee_signals() if 'grip' in _signals: self._gripper_type='grip' elif 'vacuumOn' in _signals: self._gripper_type='vacuum' else: self._gripper_type='unknown' else: if not (self._gripper.is_calibrated() or self._gripper.calibrate() == True): raise except: self._gripper=None self._is_clicksmart=False self._gripper_type=None self._gripper_reverse=None
def handle_request(req): try: gripper = get_current_gripper_interface() is_clicksmart = isinstance(gripper, SimpleClickSmartGripper) if is_clicksmart: if gripper.needs_init(): gripper.initialize() else: if not (gripper.is_calibrated() or gripper.calibrate() == True): raise return if(req.command=="open"): try: gripper.set_ee_signal_value('grip', True) print "Gripper is open" except Exception as e: print e return OpenGripperResponse(True) elif(req.command=="close"): try: gripper.set_ee_signal_value('grip', False) print "Gripper is closed" except Exception as e: print e return OpenGripperResponse(False) except: gripper = None is_clicksmart = False else: raise rospy.ServiceException("invalid command")
def is_gripper_removed(): """ Verify grippers are removed for calibration. """ try: gripper = intera_interface.get_current_gripper_interface() except Exception, e: return True
def is_gripper_removed(): """ Verify grippers are removed for calibration. """ try: gripper = intera_interface.get_current_gripper_interface() except Exception as e: return True rospy.logerr("Calibration Client: Cannot calibrate with grippers attached." " Remove grippers before calibration!") return False
def init(): global g_sub_open, g_sub_close, g_gripper, g_is_pneumatic g_sub_open = rospy.Subscriber('/cairo/sawyer_gripper_open', std_msgs.msg.Empty, open_grip) g_sub_close = rospy.Subscriber('/cairo/sawyer_gripper_close', std_msgs.msg.Empty, close_grip) g_gripper = intera_interface.get_current_gripper_interface() g_is_pneumatic = isinstance(g_gripper, intera_interface.SimpleClickSmartGripper) if g_is_pneumatic: if g_gripper.needs_init(): g_gripper.initialize() if g_gripper.needs_init(): return False else: if not g_gripper.is_calibrated(): g_gripper.calibrate() if not g_gripper.is_calibrated(): return False return True
def handle_request(req): try: gripper = get_current_gripper_interface() isOpen = gripper.__dict__['gripper_io'].__dict__['signals'].get( 'open_B1ZwR2PmT7').get('data') isClosed = gripper.__dict__['gripper_io'].__dict__['signals'].get( 'closed_H1GwChPmT7').get('data') is_clicksmart = isinstance(gripper, SimpleClickSmartGripper) return GripperGraspingResponse(isOpen, isClosed) except: gripper = None is_clicksmart = False else: raise rospy.ServiceException("invalid command")
def __init__(self, arm, lights=True): """ @type arm: str @param arm: arm of gripper to control @type lights: bool @param lights: if lights should activate on cuff grasp """ self._arm = arm # inputs self._cuff = Cuff(limb=arm) # connect callback fns to signals self._lights = None if lights: self._lights = Lights() self._cuff.register_callback(self._light_action, '{0}_cuff'.format(arm)) try: self._gripper = get_current_gripper_interface() self._is_clicksmart = isinstance(self._gripper, SimpleClickSmartGripper) if self._is_clicksmart: if self._gripper.needs_init(): self._gripper.initialize() else: if not (self._gripper.is_calibrated() or self._gripper.calibrate() == True): raise self._cuff.register_callback(self._close_action, '{0}_button_upper'.format(arm)) self._cuff.register_callback(self._open_action, '{0}_button_lower'.format(arm)) rospy.loginfo("{0} Cuff Control initialized...".format( self._gripper.name)) except: self._gripper = None self._is_clicksmart = False msg = ("{0} Gripper is not connected to the robot." " Running cuff-light connection only.").format( arm.capitalize()) rospy.logwarn(msg)
def main(): """Save / Load EndEffector Config utility Read current config off of ClickSmart and save to file. Or load config from file and reconfigure and save it to ClickSmart device. """ arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) parser.add_argument('-s', '--save', metavar='PATH', help='save current EE config to given file') parser.add_argument('-l', '--load', metavar='PATH', help='load config from given file onto EE') args = parser.parse_args(rospy.myargv()[1:]) print("Initializing node... ") rospy.init_node('ee_config_editor', anonymous=True) ee = intera_interface.get_current_gripper_interface() if not ee: rospy.logerr("Could not detect an attached EndEffector!") return if args.save: rospy.loginfo("Saving EE config to {}".format(args.save)) save_config(ee, args.save) if args.load: rospy.loginfo( "Loading config and writing config to ClickSmart from {}".format( args.load)) load_config(ee, args.load) def clean_shutdown(): print("\nExiting example...") rospy.on_shutdown(clean_shutdown)
def __init__(self): self.old_time = time.time() self.learner_quotient = 0 self.interaction_counter = 0 self.total_interactions = 0 self.time_elapsed = 0 self.learner_interaction = False self.old_time = time.time() self.new_time = time.time() self.waypoints = [] self.waypoints_back = [] self.waypoint_limit = 99 self.effort = [] self.new_angle = 0 self.old_angle = 0 self.changed_angle = 0 self.eventnumber = 0 self.iframe_sequence_pub = rospy.Publisher('/iframe_sequence', Int32, queue_size=10) self.unique_input_pub = rospy.Publisher('/unique_input', String, queue_size=10) self.zeroG_pub = rospy.Publisher('/zeroG_topic', String, queue_size=10) self.gripper_pub = rospy.Publisher('/gripper_control', Int32, queue_size=10) self.lights_pub = rospy.Publisher('/cuff_light', String, queue_size=10) self.interaction_pub = rospy.Publisher('/interaction_topic', Int32, queue_size=10) rospy.Subscriber('/upper_cuff_button', Int32, self.upper_cuff_button_callback) rospy.Subscriber('/lower_cuff_button', Int32, self.lower_cuff_button_callback) rospy.Subscriber('/bot_sequence', Int32, self.callback) rospy.Subscriber('/save_info', String, self.save_info_callback) rospy.Subscriber('/right_navigator_button', String, self.navigator_callback) rospy.init_node('Sawyer_Sparrow_comm_node', anonymous=True) self.limb = intera_interface.Limb('right') self._gripper = get_current_gripper_interface() self._is_clicksmart = isinstance(self._gripper, SimpleClickSmartGripper) self.startPose = self.limb.endpoint_pose() self.startPose_container = cartPose() self.startPose_arg = go_to.cartesian_pose_arg() self.startJointAngles = self.limb.joint_angles() self.newJointAngles_arg = go_to.joint_angle_arg() self.newCartPose_arg = go_to.cartesian_pose_arg() self.newCartPose_arg2 = go_to.cartesian_pose_arg() self.newCartPose = cartPose() self.newJointAngles = jointAngles() self.waypoint_init_container = cartPose() self.waypoint_init_arg = go_to.cartesian_pose_arg() #Check if gripper is attached properly self.grip = gripper_cuff.GripperConnect() self.savedLocations = [] #Open gripper grip_msg.data = 1 self.gripper_pub.publish(grip_msg) #set constrained mode zeroG.constrained(zeroG_all_constraints) # self.startPose = self.limb.endpoint_pose() # print(self.limb.endpoint_pose()) # self.startPose_container = self.pose_to_cartclass(pose = self.startPose, cartclass = self.startPose_container) # print(self.startPose_container.position_z) # print(self.startPose_container.position_z) # self.startPose_arg = self.cartclass_to_cartarg(cartclass = self.startPose_container, cartarg = self.startPose_arg) # print(self.startPose_container.position_z) # go_to.cartesian_pose(self.startPose_arg) #Check files and set text file to save to if (file_save): self.home = os.path.expanduser("~") self.dir_path = self.home + '/Learner_Responses/module' + str( module_number) + '/' if not os.path.exists(self.dir_path): os.makedirs(self.dir_path) self.file_number = 0 self.file_path = self.dir_path + 'test_' + str( self.file_number) + '.txt' while (os.path.exists(self.file_path)): self.file_number += 1 self.file_path = self.dir_path + 'test_' + str( self.file_number) + '.txt' self.file = open(self.file_path, 'w') self.file.write('pretest \r\n' + str(self.old_time) + '\r\n') self.file.close() go_to.joint_angles(empty_angle_arg) # #Complete grip # grip_msg.data = 0 # self.gripper_pub.publish(grip_msg) #Test code here #effort testing # self.waypoints = [] # self.waypoint_limit = 5 #initialize navigator for user inputs navigator.right()
from vision import * from move import * from neutral_position import neutral_position from intera_interface import (get_current_gripper_interface) # Initialise node rospy.init_node('robot_main') # Initialise sleep time rate = rospy.Rate(10) command = [] gripper = get_current_gripper_interface() gripper.calibrate() def callback(data): if data.data == True: global start start = True # Read command text file and add it to the command list with open("command.txt", "r") as inputfile: for line in inputfile: command.append(line.split(" ")) try:
def __init__(self): # Initialize Sawyer rospy.init_node('Sawyer_comm_node', anonymous=True) intera_interface.HeadDisplay().display_image('logo.png') # Publishing topics suppress_cuff_interaction = rospy.Publisher( '/robot/limb/right/suppress_cuff_interaction', Empty, queue_size=1) self.position_topic = rospy.Publisher('/position', JointInfo, queue_size=1) self.velocity_topic = rospy.Publisher('/velocity', JointInfo, queue_size=1) self.effort_topic = rospy.Publisher('/effort', JointInfo, queue_size=1) self.dev_topic = rospy.Publisher('/dev_topic', Int32, queue_size=10) self.scroll_wheel_button_topic = rospy.Publisher( '/scroll_wheel_button_topic', Empty, queue_size=10) self.command_complete_topic = rospy.Publisher('/command_complete', Empty, queue_size=1) self.wheel_delta_topic = rospy.Publisher('/wheel_delta', Int32, queue_size=10) self.clicked = rospy.Publisher('scroll_wheel_pressed', Bool, queue_size=10) self.toggle_completed_topic = rospy.Publisher('/toggle_completed', Empty, queue_size=1) self.highTwo_success_topic = rospy.Publisher('/highTwo_success', Bool, queue_size=1) self.endpoint_topic = rospy.Publisher('/EndpointInfo', EndpointInfo, queue_size=10) self.interaction_control_topic = rospy.Publisher('/InteractionControl', InteractionControl, queue_size=10) self.pickedup_topic = rospy.Publisher('/pickedup', Bool, queue_size=1) self.pos_orient_topic = rospy.Publisher('/pos_orient', String, queue_size=1) # Subscribing topics rospy.Subscriber('/audio_duration', Float64, self.rx_audio_duration) rospy.Subscriber('/robot/joint_states', sensor_msgs.msg.JointState, self.forwardJointState) rospy.Subscriber('/GoToJointAngles', GoToJointAngles, self.cb_GoToJointAngles) rospy.Subscriber('/wheel_subscription', Bool, self.cb_WheelSubscription) rospy.Subscriber('/joint_move', joint_move, self.cb_joint_move) rospy.Subscriber('/InteractionControl', InteractionControl, self.cb_interaction) rospy.Subscriber('/JointAngle', String, self.cb_joint_angle) rospy.Subscriber('/JointImpedance', JointImpedance, self.cb_impedance) rospy.Subscriber('/multiple_choice', Bool, self.cb_multiple_choice) rospy.Subscriber('/highTwo', Bool, self.cb_highTwo) rospy.Subscriber('/robot/limb/right/endpoint_state', intera_core_msgs.msg.EndpointState, self.forwardEndpointState) rospy.Subscriber('/adjustPoseTo', adjustPoseTo, self.cb_adjustPoseTo) rospy.Subscriber('/closeGripper', Bool, self.cb_closeGripper) rospy.Subscriber('/openGripper', Bool, self.cb_openGripper) rospy.Subscriber('/GoToCartesianPose', GoToCartesianPose, self.cb_GoToCartesianPose) rospy.Subscriber('/cuffInteraction', cuffInteraction, self.cb_cuffInteraction) rospy.Subscriber('/check_pickup', Bool, self.cb_check_pickup) rospy.Subscriber('/adjustPoseBy', adjustPoseBy, self.cb_adjustPoseBy) rospy.Subscriber('/camera', Bool, self.cb_camera) rospy.Subscriber('/robot/digital_io/right_lower_button/state', intera_core_msgs.msg.DigitalIOState, self.cb_cuff_lower) rospy.Subscriber('/robot/digital_io/right_upper_button/state', intera_core_msgs.msg.DigitalIOState, self.cb_cuff_upper) # Global Vars self.audio_duration = 0 self.finished = False self.startPos = 0 self.devMode = False self.seqArr = [] self.p_command = lambda self: self.pub_num(180 / math.pi * self.limb. joint_angle(shoulder)) wheel = self.finished b_less = lambda self: self.limb.joint_angle(shoulder) < point_b[0] b_more = lambda self: self.limb.joint_angle(shoulder) > point_b[0] dot_2 = lambda self: self.limb.joint_angle(shoulder) < joint_dot_2[0] #endpoint = lambda self : self.endpoints_equal(self.limb.endpoint_pose(),dot_3,tol) or self.finished # Limb self.limb = LimbPlus() self.limb.go_to_joint_angles() # Lights self.lights = intera_interface.Lights() for light in self.lights.list_all_lights(): self.lights.set_light_state(light, False) # Navigator self.navigator = intera_interface.Navigator() self.multi_choice_callback_ids = self.BUTTON.copy() for key in self.multi_choice_callback_ids: self.multi_choice_callback_ids[key] = -1 self.wheel_callback_id = -1 self.wheel_state = self.navigator.get_wheel_state('right_wheel') self.navigator.register_callback(self.rx_finished, 'right_button_ok') self.navigator.register_callback(self.rx_cheat_code, 'head_button_ok') # Gripper self.gripper = intera_interface.get_current_gripper_interface() if isinstance(self.gripper, intera_interface.SimpleClickSmartGripper): if self.gripper.needs_init(): self.gripper.initialize() else: if not (self.gripper.is_calibrated() or self.gripper.calibrate() == True): raise self.open_gripper() # Cuff self.cuff = intera_interface.Cuff() self.cuff_callback_ids = self.CUFF_BUTTON.copy() for key in self.cuff_callback_ids: self.cuff_callback_ids[key] = -1 # Initialization complete. Spin. rospy.loginfo('Ready.') r = rospy.Rate(10) while not rospy.is_shutdown(): suppress_cuff_interaction.publish() r.sleep()
def __init__(self): self.slideEvent = True self.slideClose = False self.movedBoxes = 0 self.ready = False self.last_msg = Int32() self.zeroG_pub = rospy.Publisher('/zeroG_topic', String, queue_size=10) self.gripper_pub = rospy.Publisher('/gripper_control', Int32, queue_size=10) self.lights_pub = rospy.Publisher('/cuff_light', String, queue_size=10) self.return_pub = rospy.Publisher('/sawyer_slider_return', Int32, queue_size=2) rospy.Subscriber('/sawyer_sequence', Int32, self.callback) #rospy.Subscriber('/sawyer_mode', Int32, self.sawyerMode_callback) #rospy.Subscriber('/sawyer_time', Float32, self.sawyerTime_callback) rospy.init_node('Sawyer_node', anonymous=True) self.limb = intera_interface.Limb('right') self._gripper = get_current_gripper_interface() self._is_clicksmart = isinstance(self._gripper, SimpleClickSmartGripper) self.startPose = self.limb.endpoint_pose() self.startPose_container = cartPose() self.startPose_arg = go_to.cartesian_pose_arg() self.startJointAngles = self.limb.joint_angles() self.newCartPose_container = cartPose() self.newJointAngles_arg = go_to.joint_angle_arg() self.newCartPose_arg = go_to.cartesian_pose_arg() self.newCartPose_arg2 = go_to.cartesian_pose_arg() self.newCartPose = cartPose() self.newJointAngles = jointAngles() self.waypoint_init_container = cartPose() self.waypoint_init_arg = go_to.cartesian_pose_arg() #Check if gripper is attached properly self.grip = gripper_cuff.GripperConnect() #Open gripper grip_msg.data = 1 self.gripper_pub.publish(grip_msg) if (go_to.joint_angles(empty_angle_arg)): int_msg.data = 1 self.return_pub.publish(int_msg) # go_to.joint_angles(step2_conveyor_arg) # self.startPose = self.limb.endpoint_pose() # self.startPose_container = self.pose_to_cartclass(self.startPose, self.startPose_container) # self.startPose_arg = self.cartclass_to_cartarg(self.startPose_container, self.startPose_arg) # self.startPose_container.position_z = z_conveyor + box_height - gripper_width # self.newCartPose_arg = self.cartclass_to_cartarg(self.startPose_container, self.newCartPose_arg) # if(go_to.cartesian_pose(self.newCartPose_arg)): # grip_msg.data = 0 # self.gripper_pub.publish(grip_msg) # rospy.sleep(1) # go_to.cartesian_pose(self.startPose_arg) # go_to.cartesian_pose(self.newCartPose_arg) # grip_msg.data = 1 # self.gripper_pub.publish(grip_msg) # go_to.cartesian_pose(self.startPose_arg) navigator.right()