def __init__(self, speed=0.28, accuracy=0.003726646, timeout=5): self.sawyer_arm = intera_interface.Limb('right') self.joint_speed = speed self.accuracy = accuracy self.timeout = timeout self.waypoints = list() self._is_recording_waypoints = False self.rs = intera_interface.RobotEnable() self._init_state = self.rs.state().enabled self.rs.enable() # Create Navigator self.navigator_io = intera_interface.Navigator() self.sawyer_lights = intera_interface.Lights() # Create Gripper & Cuff self.sawyer_gripper = intera_interface.Gripper() self.sawyer_cuff = intera_interface.Cuff(limb='right') self.sawyer_cuff.register_callback(self.cuff_open_gripper_callback, '{0}_button_upper'.format('right')) self.sawyer_cuff.register_callback(self.cuff_close_gripper_callback, '{0}_button_lower'.format('right')) # print(self.navigator_io.list_all_items()) self.sawyer_arm.move_to_neutral(timeout=5, speed=self.joint_speed) self.sawyer_gripper.open() self.set_light_status('red', False) self.set_light_status('green', False) self.set_light_status('blue', True)
def _init_nodes(self, limb, light): try: self._limb=intera_interface.Limb(limb) self._head=intera_interface.Head() self._light=SawyerLight(light) #self._head_display=intera_interface.HeadDisplay() self._display=SawyerDisplay() self._cuff=intera_interface.Cuff() self._limits=intera_interface.JointLimits() self._navigator=intera_interface.Navigator() self._joint_names=self._limb.joint_names() self._velocity_limits=self._limits.joint_velocity_limits() self._stop_cmd={} for i,name in enumerate(self._joint_names): self._stop_cmd[name]=0.0 except: print("Warning caught exception...") traceback.print_exc() pass
def __init__(self): super(LimbPlus, self).__init__() self.nav = intera_interface.Navigator() self.icc_pub = rospy.Publisher( '/robot/limb/right/interaction_control_command', InteractionControlCommand, queue_size=1)
def __init__(self, control_rate, robot_name, print_debug): self.max_release = 0 self._print_debug = print_debug RobotController.__init__(self) self.sem_list = [Semaphore(value=0)] self._status_mutex = Lock() self.robot_name = robot_name self._desired_gpos = GRIPPER_OPEN self.gripper_speed = 300 self._force_counter = 0 self._integrate_gripper_force, self._last_integrate = 0., None self.num_timeouts = 0 self._cmd_publisher = rospy.Publisher( '/robot/limb/right/joint_command', JointCommand, queue_size=100) self.gripper_pub = rospy.Publisher('/wsg_50_driver/goal_position', Cmd, queue_size=10) rospy.Subscriber("/wsg_50_driver/status", Status, self._gripper_callback) print("waiting for first status") self.sem_list[0].acquire() print('gripper initialized!') self.control_rate = rospy.Rate(control_rate) self._navigator = intera_interface.Navigator() self._navigator.register_callback(self._close_gripper_handler, 'right_button_ok')
def main(): parser = argparse.ArgumentParser() parser.add_argument('-m', '--markers', nargs='+', dest='markers', help='Valid Marker Points') parser.add_argument('-c', '--cameras', nargs='+', dest='cameras', help='Operating Cameras') parser.add_argument('--validate_calib', action='store_true', help='Validate Calibration') args = parser.parse_args() marker_list = args.markers camera_list = args.cameras v_check = args.validate_calib marker_set = {0, 1, 2} if marker_list is not None: marker_set = set([int(m) for m in marker_list]) camera_set = {0, 1} if camera_list is not None: camera_set = set([int(c) for c in camera_list]) rospy.init_node('get_ar_points') #initialize robot rs = intera_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled limb_right = intera_interface.Limb("right") name_of_service = "ExternalTools/right/PositionKinematicsNode/FKService" fksvc = rospy.ServiceProxy(name_of_service, SolvePositionFK) #initialize tracking code camera_tracker = CameraRegister(camera_set, marker_set, limb_right, name_of_service, fksvc, validate=v_check) #init robot ui navigator = intera_interface.Navigator() cam_collect = navigator.register_callback(camera_tracker.toggle_collection, 'right_button_ok') cam_print = navigator.register_callback(camera_tracker.print_callback, 'right_button_square') robot_point_print = navigator.register_callback( camera_tracker.print_robot_eep, 'right_button_show') # rospy.Subscriber("/ar_pose_marker", AlvarMarkers, camera_tracker.camera_track_callback) print("BEGINNING SPIN") rospy.spin()
def __init__(self): self.ctrl = RobotController(self.CONTROL_RATE) self.recorder = RobotRecorder(save_dir='', use_aux=False, save_actions=False, save_images=False) self.image_publisher = rospy.Publisher('/robot/head_display', Image_msg, queue_size=2) self.load_splashes() self.control_rate = rospy.Rate(self.PREDICT_RATE) self.ui_rate = rospy.Rate(self.UI_RATE) self.robot_predictor = setup_predictor( self.ROBOT_MODEL, self.ROBOT_BIAS, self.CROP_H_MIN, self.CROP_H_MAX, self.CROP_W_MIN, self.CROP_W_MAX) self.human_predictor = setup_predictor_human( self.HUMAN_MODEL, self.HUMAN_BIAS, self.CROP_H_MIN, self.CROP_H_MAX, self.CROP_W_MIN, self.CROP_W_MAX) self.is_human = False self.predictor = self.robot_predictor rospy.Subscriber("/keyboard/keyup", Key, self.keyboard_up_listener) self.weiss_pub = rospy.Publisher('/wsg_50_driver/goal_position', Cmd, queue_size=10) # rospy.Subscriber("/wsg_50_driver/status", Status, self.weiss_status_listener) self._navigator = intera_interface.Navigator() self.demo_collect = self._navigator.register_callback( self.record_demo, 'right_button_ok') self.swap_key = self._navigator.register_callback( self.swap_sawyer_cuff, 'right_button_back') self.start_key = self._navigator.register_callback( self.start_traj_cuff, 'right_button_square') self.neutral_key = self._navigator.register_callback( self.neutral_cuff, 'right_button_show') self.x_button = self._navigator.register_callback( self.gripper_cuff, 'right_button_triangle') self.calc = EE_Calculator() print 'ONE SHOT READY!' self.running = False self.demo_imgs = None self.move_netural() rospy.spin()
def __init__(self, constraint_id, cuff_button): """ Parameters ---------- constraint_id : int The id of the constraint for the trigger. cuff_button : str The cuff button name. """ self.constraint_id = constraint_id self.button = cuff_button self.nav = intera_interface.Navigator()
def __init__(self): # Initialize Sawyer #rospy.init_node('image', anonymous=True) # Publishing topics #suppress_cuff_interaction = rospy.Publisher('/robot/limb/right/suppress_cuff_interaction', Empty, queue_size=1) self.received = False # rospy.Subscriber('/Image', Image, self.get_image) self.img = np.zeros((480, 752, 3), np.uint8) self.navigator = intera_interface.Navigator()
def __init__(self, arm, name, start): uri = "mongodb://*****:*****@sawyer-mongo.documents.azure.com:10255/?ssl=true" client = MongoClient(uri) self.db = client.SawyerDB self.collection = self.db.Commands self.commandName = name self.commandNumber = start rp = RobotParams() self._lastJoin = {} self.lastButtonPress = datetime.datetime.now() self._rs = intera_interface.RobotEnable() self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() self._navigator_io = intera_interface.Navigator() head_display = intera_interface.HeadDisplay() head_display.display_image( "/home/microshak/Pictures/Sawyer_Navigator_Main.png", False, 1.0) valid_limbs = rp.get_limb_names() self._arm = rp.get_limb_names()[0] self._limb = intera_interface.Limb(arm) if start == None: limb = intera_interface.Limb("right") limb.move_to_neutral() print(self._arm) # inputs self._cuff = Cuff(limb='right') self._navigator = Navigator() # connect callback fns to signals self._lights = None self._lights = Lights() self._cuff.register_callback(self._light_action, '{0}_cuff'.format('right')) try: self._gripper = Gripper(self._arm) #if not (self._gripper.is_calibrated() or self._gripper.calibrate() == True): # rospy.logerr("({0}_gripper) calibration failed.".format(self._gripper.name)) # raise except: self._gripper = None msg = ("{0} Gripper is not connected to the robot." " Running cuff-light connection only.").format( arm.capitalize()) rospy.logwarn(msg)
def main_sawyer(): import intera_interface from visual_mpc.envs.robot_envs.sawyer.sawyer_impedance import SawyerImpedanceController controller = SawyerImpedanceController('sawyer', True, gripper_attached='none') # doesn't initial gripper object even if gripper is attached def print_eep(value): if not value: return xyz, quat = controller.get_xyz_quat() yaw, roll, pitch = [np.rad2deg(x) for x in controller.quat_2_euler(quat)] logging.getLogger('robot_logger').info("XYZ IS: {}, ROTATION IS: yaw={} roll={} pitch={}".format(xyz, yaw, roll, pitch)) navigator = intera_interface.Navigator() navigator.register_callback(print_eep, 'right_button_show') rospy.spin()
def echo_input(nav_name='right'): nav = intera_interface.Navigator() def back_pressed(v): print("Button 'Back': {0}".format(nav.button_string_lookup(v))) def rethink_pressed(v): print("Button 'Rethink': {0}".format(nav.button_string_lookup(v))) def circle_pressed(v): print("Button 'Circle': {0}".format(nav.button_string_lookup(v))) def square_pressed(v): print("Button 'Square': {0}".format(nav.button_string_lookup(v))) def x_pressed(v): print("Button 'X': {0}".format(nav.button_string_lookup(v))) def ok_pressed(v): print("Button 'OK': {0}".format(nav.button_string_lookup(v))) def wheel_moved(v): print("Wheel value: {0}".format(v)) nav.register_callback(back_pressed, '_'.join([nav_name, 'button_back'])) nav.register_callback(rethink_pressed, '_'.join([nav_name, 'button_show'])) nav.register_callback(circle_pressed, '_'.join([nav_name, 'button_circle'])) nav.register_callback(square_pressed, '_'.join([nav_name, 'button_square'])) nav.register_callback(x_pressed, '_'.join([nav_name, 'button_triangle'])) nav.register_callback(ok_pressed, '_'.join([nav_name, 'button_ok'])) nav.register_callback(wheel_moved, '_'.join([nav_name, 'wheel'])) print("Press input buttons on the right navigator, " "input will be echoed here.") rate = rospy.Rate(1) i = 0 while not rospy.is_shutdown() and i < 10: rate.sleep() i += 1
def __init__(self, control_rate, robot_name): self.max_release = 0 RobotController.__init__(self) self.sem_list = [Semaphore(value=0)] self._status_mutex = Lock() self.robot_name = robot_name self._desired_gpos = GRIPPER_OPEN self.gripper_speed = 300 self._force_counter = 0 self._integrate_gripper_force = 0. self.num_timeouts = 0 self.gripper_pub = rospy.Publisher('/wsg_50_driver/goal_position', Cmd, queue_size=10) rospy.Subscriber("/wsg_50_driver/status", Status, self._gripper_callback) print("waiting for first status") self.sem_list[0].acquire() print('gripper initialized!') self.imp_ctrl_publisher = rospy.Publisher('/desired_joint_pos', JointState, queue_size=1) self.imp_ctrl_release_spring_pub = rospy.Publisher('/release_spring', Float32, queue_size=10) self.imp_ctrl_active = rospy.Publisher('/imp_ctrl_active', Int64, queue_size=10) self.control_rate = rospy.Rate(control_rate) self.imp_ctrl_release_spring(100) self.imp_ctrl_active.publish(1) self._navigator = intera_interface.Navigator() self._navigator.register_callback(self._close_gripper_handler, 'right_button_ok')
def __init__(self): """ Records joint data to a file at a specified rate. rate: recording frequency in Hertz """ print("Initializing node... ") rospy.init_node("pushback_recorder") parser = argparse.ArgumentParser( description='select whehter to record or replay') parser.add_argument('robot', type=str, help='robot name') args = parser.parse_args() self.robotname = args.robot self.file = '/'.join( str.split(visual_mpc_rospkg.__file__, "/")[:-1]) + '/src/utils/pushback_traj_{}.pkl'.format( self.robotname) self.rs = intera_interface.RobotEnable(CHECK_VERSION) self.init_state = self.rs.state().enabled self.limb = intera_interface.Limb("right") self._navigator = intera_interface.Navigator() self.start_callid = self._navigator.register_callback( self.start_recording, 'right_button_ok') # Navigator Rethink button press self.stop_callid = self._navigator.register_callback( self.stop_recording, 'right_button_show') self.control_rate = rospy.Rate(800) self.collect_active = False rospy.on_shutdown(self.clean_shutdown) self.joint_pos = [] print('ready for recording!') rospy.spin()
def __init__(self, item_id, reference_pose, base_frame="world", child_frame="right_gripper_tip", service_name="transform_lookup_service"): """ Parameters ---------- robot_id : int Id of robot assigned in the config.json configuration files. reference_pose : dict Dictionary with position and orientation fields base_frame : str The base / world frame from which to calculate the transformation to the child frame. child_frame : str The ending TF frame used to generate the pose / orientation of the robot (usually the end effector tip). service_name : str Name of transformation lookup service used by _get_transform() to calculate the transformation between world_frame and child_frame """ self.id = item_id self.reference_pose = reference_pose self._limb = intera_interface.Limb("right") self._cuff = intera_interface.Cuff("right") self._navigator = intera_interface.Navigator() try: self._gripper = intera_interface.Gripper("right") rospy.loginfo("Electric gripper detected.") if self._gripper.has_error(): rospy.loginfo("Gripper error...rebooting.") self._gripper.reboot() if not self._gripper.is_calibrated(): rospy.loginfo("Calibrating gripper.") self._gripper.calibrate() except Exception as e: self._gripper = None rospy.loginfo("No electric gripper detected.") self.base_frame = base_frame self.child_frame = child_frame self.tlc = TransformLookupClient(service_name)
def __init__(self, speed, accuracy, limb="right"): # Create intera_interface limb instance self._arm = limb self._limb = intera_interface.Limb(self._arm) # Parameters which will describe joint position moves self._speed = speed self._accuracy = accuracy # Recorded waypoints self._waypoints = list() # Recording state self._is_recording = False # Verify robot is enabled print("Getting robot state... ") self._rs = intera_interface.RobotEnable() self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() # Create Navigator I/O self._navigator = intera_interface.Navigator()
def __init__(self): self._navigator = intera_interface.Navigator() self.cmd_pub = rospy.Publisher('/cairo_lfd/record_commands', String, queue_size=10)
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, robot_type, file_name): """ Records joint data to a file at a specified rate. rate: recording frequency in Hertz """ if robot_type == 'sawyer': import intera_interface from visual_mpc.envs.robot_envs.sawyer.sawyer_impedance import SawyerImpedanceController self._controller = SawyerImpedanceController( 'recorder_bot', False, gripper_attached='none') self._controller.move_to_neutral() # Navigator Rethink button press self._navigator = intera_interface.Navigator() self.start_callid = self._navigator.register_callback( self.start_recording, 'right_button_ok') self._control_rate = rospy.Rate(800) self.stop_callid = self._navigator.register_callback( self.stop_recording, 'right_button_square') elif robot_type == 'widowx': import threading from visual_mpc.envs.robot_envs.widowx.widowx_controller import WidowXController from keyboard.msg import Key self._controller = WidowXController('recorder_bot', False) self._control_rate = rospy.Rate(50) self._controller.move_to_neutral() def keyboard_listener(msg): if msg.code == 115: rec_thread = threading.Thread(target=self.start_recording, args=(1, )) rec_thread.start() else: self.stop_recording(1) rospy.Subscriber("/keyboard/keydown", Key, keyboard_listener) elif robot_type == 'baxter': from pynput import mouse from pynput import keyboard import baxter_interface from visual_mpc.envs.robot_envs.baxter.baxter_impedance import BaxterImpedanceController self._controller = BaxterImpedanceController( 'baxter', False, gripper_attached='none', limb='right') self._controller.move_to_neutral() # Navigator Rethink button press self._navigator = baxter_interface.Navigator('right') self._navigator1 = baxter_interface.Navigator('left') self.start_callid = self._navigator.button0_changed.connect( self.start_recording) self.stop_callid = self._navigator1.button0_changed.connect( self.stop_recording) else: raise NotImplementedError self._collect_active = False self._joint_pos = [] self._file = file_name logging.getLogger('robot_logger').info('ready for recording!') rospy.spin()
def __init__(self): print "Initializing Node" rospy.init_node('poirot_rrhost') print "Enabling Robot" rs = intera_interface.RobotEnable() rs.enable() self.rs1 = intera_interface.RobotEnable() rospy.Subscriber("spacenav/joy", Joy, self.SpaceNavJoyCallback) rospy.Subscriber("/robot/digital_io/right_lower_cuff/state", DigitalIOState, self.CuffCallback) rospy.Subscriber("/robot/digital_io/right_button_ok/state", DigitalIOState, self.NavWheelCallback) rospy.Subscriber("/robot/digital_io/right_button_triangle/state", DigitalIOState, self.XCallback) self.DIOpub = rospy.Publisher('/io/comms/io/command', IOComponentCommand, queue_size=10) self.h = IOComponentCommand() self.h.time = rospy.Time.now() self.h.op = "set" self.h.args = "{ \"signals\" : { \"port_sink_0\" : { \"format\" : { \"type\" : \"int\", \"dimensions\" : [ 1] }, \"data\" : [ 0] } } }" self.nav = intera_interface.Navigator() #self.nav.register_callback(self.ok_pressed, '_'.join([nav_name, 'button_ok'])) #intera_interface/robot/digital_io/right_lower_cuff/state # self._valid_limb_names = {'left': 'left', # 'l': 'left', # 'right': 'right', # 'r': 'right'} self._valid_limb_names = {'right': 'right', 'r': 'right'} # get information from the SDK # self._left = intera_interface.Limb('left') self._right = intera_interface.Limb('right') #self._l_jnames = self._left.joint_names() self._r_jnames = self._right.joint_names() self.kin = sawyer_kinematics('right') # data initializations self._jointpos = [0] * 7 self._jointvel = [0] * 7 self._jointtor = [0] * 7 self._ee_pos = [0] * 3 self._ee_or = [0] * 4 self._ee_tw = [0] * 6 self._ee_wr = [0] * 6 self._ee_vel = [0] * 6 self._pijac = [ ] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0') self._jac = [ ] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0') self._inertia_mat = [ ] #numpy.matrix('0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0') self.Jxl_raw = 0 self.Jyl_raw = 0 self.Jzl_raw = 0 self.Jxa_raw = 0 self.Jya_raw = 0 self.Jza_raw = 0 self.leftbutton = 0 self.rightbutton = 0 self.spacenav = [] #self._l_joint_command = dict(zip(self._l_jnames,[0.0]*7)) self._r_joint_command = dict(zip(self._r_jnames, [0.0] * 7)) self.MODE_POSITION = 0 self.MODE_VELOCITY = 1 self.MODE_TORQUE = 2 self._mode = self.MODE_POSITION self.RMH_delay = .01 # initial joint command is current pose self.readJointPositions() #self.setJointCommand('left',self._jointpos[0:7]) self.setJointCommand('right', self._jointpos[0:7]) self.head_display = intera_interface.HeadDisplay() print('head display!') self.image = "/home/rachel/ros_ws/src/sawyer_simulator/sawyer_sim_examples/models/cafe_table/materials/textures/Wood_Floor_Dark.jpg" self.head_display.display_image(self.image, display_rate=100) print('finished first head display') # Start background threads self._running = True self._t_joints = threading.Thread(target=self.jointspace_worker) self._t_joints.daemon = True self._t_joints.start() self._t_effector = threading.Thread(target=self.endeffector_worker) self._t_effector.daemon = True self._t_effector.start() self._t_command = threading.Thread(target=self.command_worker) self._t_command.daemon = True self._t_command.start() self._t_display = threading.Thread(target=self.display_worker) self._t_display.daemon = True self._t_display.start() self._t_dio = threading.Thread(target=self.dio_worker) self._t_dio.daemon = True self._t_dio.start()
def __init__(self): print "Initializing Node" rospy.init_node('sawyer_peripherals') self._running = True # self._valid_limb_names = {'left': 'left', # 'l': 'left', # 'right': 'right', # 'r': 'right'} self._valid_limb_names = {'right': 'right', 'r': 'right'} # gripper initialization #self._grippers = {'right': intera_interface.Gripper('right')} # self._grippers = {#'left': intera_interface.Gripper('left'), # 'right': intera_interface.Gripper('right')} # Set grippers to defaults # self._grippers['left'].set_parameters( # self._grippers['left'].valid_parameters()) #self._grippers['right'].set_parameters( #self._grippers['right'].valid_parameters()) # ranger initialization #self._rangers = {'right': intera_interface.AnalogIO('right_hand_range')} # self._rangers = {'left': intera_interface.AnalogIO('left_hand_range'), # 'right': intera_interface.AnalogIO('right_hand_range')} # accelerometer initialization #self._accelerometers = {'left': [0.0]*3, 'right': [0.0]*3} self._accelerometers = {'right': [0.0] * 3} # rospy.Subscriber("/robot/accelerometer/left_accelerometer/state", # Imu, # self.left_accel_callback) rospy.Subscriber("/robot/accelerometer/right_accelerometer/state", Imu, self.right_accel_callback) # head control initialization self._head = intera_interface.Head() # sonar initialization self._sonar_pointcloud = RR.RobotRaconteurNode.s.NewStructure( 'SawyerPeripheral_interface.SonarPointCloud') self._sonar_state_sub = rospy.Subscriber( "/robot/sonar/head_sonar/state", PointCloud, self.sonar_callback) self._sonar_enable_pub = rospy.Publisher( "/robot/sonar/head_sonar/set_sonars_enabled", UInt16, latch=True) # initially all sonar sensors on self._sonar_enabled = True self._sonar_enable_pub.publish(4095) # suppressions self._suppress_body_avoidance = {'right': False} #self._suppress_body_avoidance = {'left': False, 'right': False} self._supp_body_avoid_pubs = { 'left': rospy.Publisher("/robot/limb/left/suppress_body_avoidance", Empty, latch=True), 'right': rospy.Publisher("/robot/limb/right/suppress_body_avoidance", Empty, latch=True) } self._supp_body_avoid_pubs = { 'right': rospy.Publisher("/robot/limb/right/suppress_body_avoidance", Empty, latch=True) } self._suppress_collision_avoidance = {'right': False} # self._suppress_collision_avoidance = {'left': False, 'right': False} self._supp_coll_avoid_pubs = { 'right': rospy.Publisher("/robot/limb/right/suppress_collision_avoidance", Empty, latch=True) } # self._supp_coll_avoid_pubs = {'left': # rospy.Publisher("/robot/limb/left/suppress_collision_avoidance", # Empty, # latch=True), # 'right': # rospy.Publisher("/robot/limb/right/suppress_collision_avoidance", # Empty, # latch=True)} self._suppress_contact_safety = {'right': False} # self._suppress_contact_safety = {'left': False, 'right': False} self._supp_con_safety_pubs = { 'right': rospy.Publisher("/robot/limb/right/suppress_contact_safety", Empty, latch=True) } # self._supp_con_safety_pubs = {'left': # rospy.Publisher("/robot/limb/left/suppress_contact_safety", # Empty, # latch=True), # 'right': # rospy.Publisher("/robot/limb/right/suppress_contact_safety", # Empty, # latch=True)} self._suppress_cuff_interaction = {'right': False} # self._suppress_cuff_interaction = {'left': False, 'right': False} self._supp_cuff_int_pubs = { 'right': rospy.Publisher("/robot/limb/right/suppress_cuff_interaction", Empty, latch=True) } # self._supp_cuff_int_pubs = {'left': # rospy.Publisher("/robot/limb/left/suppress_cuff_interaction", # Empty, # latch=True), # 'right': # rospy.Publisher("/robot/limb/right/suppress_cuff_interaction", # Empty, # latch=True)} self._suppress_gravity_compensation = {'right': False} # self._suppress_gravity_compensation = {'left': False, 'right': False} self._supp_grav_comp_pubs = { 'right': rospy.Publisher("/robot/limb/right/suppress_gravity_compensation", Empty, latch=True) } # self._supp_grav_comp_pubs = {'left': # rospy.Publisher("/robot/limb/left/suppress_gravity_compensation", # Empty, # latch=True), # 'right': # rospy.Publisher("/robot/limb/right/suppress_gravity_compensation", # Empty, # latch=True)} # start suppressions background thread self._t_suppressions = threading.Thread( target=self.suppressions_worker) self._t_suppressions.daemon = True self._t_suppressions.start() # gravity compensation subscription self._grav_comp_lock = threading.Lock() self._gravity_compensation_torques = OrderedDict( zip(intera_interface.Limb('right').joint_names(), [0.0] * 7)) # self._gravity_compensation_torques = OrderedDict( # zip(intera_interface.Limb('left').joint_names() + \ # intera_interface.Limb('right').joint_names(), # [0.0]*14)) # rospy.Subscriber("/robot/limb/left/gravity_compensation_torques", # SEAJointState, self.grav_comp_callback) rospy.Subscriber("/robot/limb/right/gravity_compensation_torques", SEAJointState, self.grav_comp_callback) # navigators self._navigators = {'right': intera_interface.Navigator('right')} # self._navigators = {'left': intera_interface.Navigator('left'), # 'right': intera_interface.Navigator('right'), # 'torso_left': # intera_interface.Navigator('torso_left'), # 'torso_right': # intera_interface.Navigator('torso_right')} # initialize frame transform self._listener = tf.TransformListener()
def __init__(self, file=None): """ Records joint data to a file at a specified rate. rate: recording frequency in Hertz """ print("Initializing node... ") rospy.init_node("pushback_recorder") parser = argparse.ArgumentParser( description='select whehter to record or replay') parser.add_argument('--record', type=str, default='True', help='') args = parser.parse_args() self.rs = intera_interface.RobotEnable(CHECK_VERSION) self.init_state = self.rs.state().enabled self.limb = intera_interface.Limb("right") self.joint_names = self.limb.joint_names() # self.gripper = intera_interface.Gripper("right") self.name_of_service = "ExternalTools/right/PositionKinematicsNode/FKService" self.fksvc = rospy.ServiceProxy(self.name_of_service, SolvePositionFK) self._navigator = intera_interface.Navigator() self.start_callid = self._navigator.register_callback( self.start_recording, 'right_button_ok') # Navigator Rethink button press self.stop_callid = self._navigator.register_callback( self.stop_recording, 'right_button_show') # self.delete_callid = self._navigator.register_callback(self.delete_recording, 'right_button_back') self.imp_ctrl_publisher = rospy.Publisher('desired_joint_pos', JointState, queue_size=1) self.imp_ctrl_release_spring_pub = rospy.Publisher('release_spring', Float32, queue_size=10) self.imp_ctrl_active = rospy.Publisher('imp_ctrl_active', Int64, queue_size=10) # self.gripper.set_velocity(self.gripper.MAX_VELOCITY) # "set 100% velocity"), # self.gripper.open() self.control_rate = rospy.Rate(20) self.imp_ctrl_active.publish(0) self.collect_active = False rospy.on_shutdown(self.clean_shutdown) self.joint_pos = [] self.set_neutral() self.recorder = DemoRobotRecorder('/home/sudeep/outputs', self.N_SAMPLES, use_aux=False) self.record_iter = 0 if args.record == 'False': self.playback(file) if args.record == 'True': print 'ready for recording!' rospy.spin() raise ValueError('wrong argument!')
def __init__(self): print "Initializing Node" rospy.init_node('poirot_rrhost') print "Enabling Robot" rs = intera_interface.RobotEnable() rs.enable() self.rs1 = intera_interface.RobotEnable() rospy.Subscriber("spacenav/joy", Joy, self.SpaceNavJoyCallback) rospy.Subscriber("/robot/digital_io/right_lower_cuff/state", DigitalIOState, self.CuffCallback) rospy.Subscriber("/robot/digital_io/right_button_ok/state", DigitalIOState, self.NavWheelCallback) rospy.Subscriber("/robot/digital_io/right_button_triangle/state", DigitalIOState, self.XCallback) rospy.Subscriber("/robot/digital_io/right_lower_button/state", DigitalIOState, self.GripperCloseCallback) rospy.Subscriber("/robot/digital_io/right_upper_button/state", DigitalIOState, self.GripperOpenCallback) rospy.Subscriber("/pushed1", Bool, self.pushed1callback) rospy.Subscriber("/pushed2", Bool, self.pushed2callback) rospy.Subscriber("/pushed3", Bool, self.pushed3callback) rospy.Subscriber("/pushed4", Bool, self.pushed4callback) self.DIOpub = rospy.Publisher('/io/comms/io/command', IOComponentCommand, queue_size=10) self.h = IOComponentCommand() self.h.time = rospy.Time.now() self.h.op = "set" self.h.args = "{ \"signals\" : { \"port_sink_0\" : { \"format\" : { \"type\" : \"int\", \"dimensions\" : [ 1] }, \"data\" : [ 0] } } }" rospy.Subscriber("Robotiq2FGripperRobotInput", inputMsg.Robotiq2FGripper_robot_input, self.GripperStatusCallback) self.gripperpub = rospy.Publisher( 'Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output) self.grippercommand = outputMsg.Robotiq2FGripper_robot_output() self.gripperstatus = inputMsg.Robotiq2FGripper_robot_input() self.grip_des_pos = 444 #self.des_gripper_pos=0 #activate-reset-activate gripper self.grippercommand.rACT = 1 self.grippercommand.rGTO = 1 self.grippercommand.rSP = 255 self.grippercommand.rFR = 150 self.gripperpub.publish(self.grippercommand) time.sleep(.5) self.grippercommand = outputMsg.Robotiq2FGripper_robot_output() self.grippercommand.rACT = 0 self.gripperpub.publish(self.grippercommand) time.sleep(.5) self.grippercommand = outputMsg.Robotiq2FGripper_robot_output() self.grippercommand.rACT = 1 self.grippercommand.rGTO = 1 self.grippercommand.rSP = 255 self.grippercommand.rFR = 150 self.pb1 = 1 self.pb2 = 1 self.pb3 = 1 self.pb4 = 1 time.sleep(.5) self.nav = intera_interface.Navigator() self.head = intera_interface.Head() self.headangle = 0 self._valid_limb_names = {'right': 'right', 'r': 'right'} # get information from the SDK # self._left = intera_interface.Limb('left') self._right = intera_interface.Limb('right') #self._l_jnames = self._left.joint_names() self._r_jnames = self._right.joint_names() self.kin = sawyer_kinematics('right') # data initializations self._jointpos = [0] * 7 self._jointvel = [0] * 7 self._jointtor = [0] * 7 self._ee_pos = [0] * 3 self._ee_or = [0] * 4 self._ee_tw = [0] * 6 self._ee_wr = [0] * 6 self._ee_vel = [0] * 6 self._pijac = [ ] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0') self._jac = [ ] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0') self._inertia_mat = [ ] #numpy.matrix('0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0') self.Jxl_raw = 0 self.Jyl_raw = 0 self.Jzl_raw = 0 self.Jxa_raw = 0 self.Jya_raw = 0 self.Jza_raw = 0 self.leftbutton = 0 self.rightbutton = 0 self.spacenav = [] #self._l_joint_command = dict(zip(self._l_jnames,[0.0]*7)) self._r_joint_command = dict(zip(self._r_jnames, [0.0] * 7)) self.MODE_POSITION = 0 self.MODE_VELOCITY = 1 self.MODE_TORQUE = 2 self._mode = self.MODE_POSITION self.RMH_delay = .01 # initial joint command is current pose self.readJointPositions() #self.setJointCommand('left',self._jointpos[0:7]) self.setJointCommand('right', self._jointpos[0:7]) self.hascollided = 0 self.head_display = intera_interface.HeadDisplay() print('head display!') #self.image="/home/rachel/rawhide/rawhide_ws/src/sawyer_simulator/sawyer_sim_examples/models/cafe_table/materials/textures/Wood_Floor_Dark.jpg" self.image = "/home/rachel/rawhide/rmh_code/puppy.png" self.head_display.display_image(self.image, display_rate=100) print('finished first head display') # Start background threads self._running = True self._t_joints = threading.Thread(target=self.jointspace_worker) self._t_joints.daemon = True self._t_joints.start() self._t_effector = threading.Thread(target=self.endeffector_worker) self._t_effector.daemon = True self._t_effector.start() self._t_command = threading.Thread(target=self.command_worker) self._t_command.daemon = True self._t_command.start() self._t_display = threading.Thread(target=self.display_worker) self._t_display.daemon = True self._t_display.start() self._t_dio = threading.Thread(target=self.dio_worker) self._t_dio.daemon = True self._t_dio.start()