def put_text(text, bg_color = (255,255,255), text_color = (0,0,0), scale=3, thickness=4): """ Write text to Sawyer's display screen :param text: string- test to displya :param bg_color: (b,g,r)- background color :param text_color: (b,g,r)- text color :param scale: float- font size :param thickness: float- font thickness :return: """ disp = intera_interface.HeadDisplay() img = np.zeros(shape=[600, 1024, 3], dtype=np.uint8) b, g, r = bg_color img[:,:,0] = b img[:, :, 1] = g img[:, :, 2] = r font = cv2.FONT_HERSHEY_DUPLEX textsize = cv2.getTextSize(text, font, scale, thickness)[0] textX = (img.shape[1] - textsize[0]) / 2 textY = (img.shape[0] + textsize[1]) / 2 img = cv2.putText(img, text, (textX, textY), font, scale, text_color, thickness) cv2.imwrite('temp.png', img) disp.display_image('temp.png') os.remove('temp.png')
def displayFace(mode): """ Displays a face on the sawyer robot corresponding to a mode: 0: display a face for a Tie 1: display a face for a Win 2: display a face for a Loss 11: display a face for Robot's turn 22: display a face for the Player's turn """ _head = intera_interface.Head() head_display = intera_interface.HeadDisplay() rospack = rospkg.RosPack() pkgpath = rospack.get_path('final-project-tic-tac-toe') if mode == 0: filepath = pkgpath + "/images/tie" + str(randrange(4)) + ".jpg" if mode == 1: filepath = pkgpath + "/images/win" + str(randrange(4)) + ".jpg" if mode == 2: filepath = pkgpath + "/images/lose.jpg" if mode == 11: filepath = pkgpath + "/images/board.jpg" if mode == 22: filepath = pkgpath + "/images/turn" + str(randrange(4)) + ".jpg" head_display.display_image(filepath)
def talker(): pub = rospy.Publisher('Pickup_object', String, queue_size=10) rospy.init_node('EMG_Interface', anonymous=True) rate = rospy.Rate(10) # 10hz head_display = intera_interface.HeadDisplay() listener() head_display.display_image(home_interface[0]) idx = 0 loop = False global start_time start_time = time.time() while not rospy.is_shutdown(): global data_received global val #pub.publish("none") if time.time() - start_time >= timeout: val = None if val is not None: if loop == False: if val.lower() == "next": #data_received = True if idx >= len(home_interface)-1: idx = 1 else: idx = idx+1 head_display.display_image(home_interface[idx]) #data_received = False if val.lower() == "select": idx1 = 0 #data_received = True head_display.display_image(selected_image[idx][idx1]) #data_received = False if idx != 0: loop = True else: rospy.loginfo("Nothing selected") loop = False else: #if time.time() - start_time >= timeout: # val = None if val is not None: if val.lower() == "next": if idx1 >= len(selected_image[idx])-1: idx1 = 0 else: idx1 = idx1+1 head_display.display_image(selected_image[idx][idx1]) if val.lower() == "select": if idx1 == 0: for i in range(0,10): pub.publish(products[idx]) #rate.sleep() head_display.display_image(home_interface[idx]) loop = False if idx1 == 1: data_received =False head_display.display_image(home_interface[idx]) loop = False
def __init__(self, config=None): """Initialize. See the parent class. """ super(FrankaPandaReal, self).__init__(config=config) if rospy.get_name() == '/unnamed': rospy.init_node('franka_panda') assert rospy.get_name() != '/unnamed', 'Must call rospy.init_node().' tic = time.time() rospy.loginfo('Initializing robot...') self._head = intera_interface.Head() self._display = intera_interface.HeadDisplay() self._lights = intera_interface.Lights() self._limb = intera_interface.Limb() self._joint_names = self._limb.joint_names() self.cmd = [] self._command_msg = JointCommand() self._command_msg.names = self._joint_names self._commanders = dict(velocity=None, torque=None) try: self._gripper = intera_interface.Gripper() self._has_gripper = True except Exception: self._has_gripper = False self._robot_enable = intera_interface.RobotEnable(True) self._params = intera_interface.RobotParams() self._motion_planning = self.config.MOTION_PLANNING if self._motion_planning == 'moveit': rospy.loginfo('Initializing moveit toolkit...') moveit_commander.roscpp_initialize(sys.argv) self._scene = moveit_commander.PlanningSceneInterface() self._group = moveit_commander.MoveGroupCommander('right_arm') toc = time.time() rospy.loginfo('Initialization finished after %.3f seconds.' % (toc - tic))
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 __init__(self): rospy.init_node("head_display", anonymous=True) self.head_display = intera_interface.HeadDisplay() # Load in camera image to show in display rospy.Subscriber('/me495/raw_image', Image, self.img_callback) # listen to button in Sawyer rospy.Subscriber('io/robot/navigator/state', IODeviceStatus, self.fun_callback) # Subscribe to control node self.state_camera = False rospy.Subscriber("/me495/command", String, self.cmd_callback) # Subscribe to camera data rospy.Subscriber('/camera_driver/image_raw', Image, self.camera_callback) # Show in display self.disp_pub = rospy.Publisher('robot/head_display', Image, latch=True, queue_size=10) # Load in fun images self.fun = False rospack = rospkg.RosPack() image_path = rospack.get_path('me495_vision') + '/images/' self.start_img = cv_bridge.CvBridge().cv2_to_imgmsg( cv2.imread(image_path + 'start.jpg'), encoding="bgr8") self.fun_img_1 = cv_bridge.CvBridge().cv2_to_imgmsg( cv2.imread(image_path + 'normal.jpg'), encoding="bgr8") self.fun_img_2 = cv_bridge.CvBridge().cv2_to_imgmsg( cv2.imread(image_path + 'blink.jpg'), encoding="bgr8") r = rospy.Rate(5) # Show in display self.useful_image = self.start_img self.fun_change = False #self.fun_time = rospy.Time.now() while not rospy.is_shutdown(): if self.fun: if self.fun_change == True: self.disp_pub.publish(self.fun_img_2) #if (rospy.Time.now() - self.fun_time).to_sec() > 0.2: self.fun_change = False else: self.disp_pub.publish(self.fun_img_1) else: self.disp_pub.publish(self.useful_image) r.sleep()
def __init__(self, Name): CONNECTION_STRING = "HostName=RobotForman.azure-devices.net;DeviceId=PythonTest;SharedAccessKey=oh9Fj0mAMWJZpNNyeJ+bSecVH3cBQwbzjDnoVmeSV5g=" self.protocol = IoTHubTransportProvider.HTTP self.client = IoTHubClient(CONNECTION_STRING, self.protocol) self.client.set_option("messageTimeout", MESSAGE_TIMEOUT) self.client.set_option("MinimumPollingTime", MINIMUM_POLLING_TIME) self.client.set_message_callback(receive_message_callback, RECEIVE_CONTEXT) moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('mid' + str(uuid.uuid4().hex), anonymous=True) print 'mid' + str(uuid.uuid4().hex) self.head_display = intera_interface.HeadDisplay() self.head_display.display_image("/home/microshak/Pictures/Ready.png", False, 1.0) self.head = intera_interface.Head() rp = RobotParams() valid_limbs = rp.get_limb_names() robot = moveit_commander.RobotCommander() rp.max_velocity_scaling_factor = .5 scene = moveit_commander.PlanningSceneInterface() self.group = moveit_commander.MoveGroupCommander("right_arm") display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.light = Lights() self.headLight("green") # rs = intera_interface.RobotEnable(CHECK_VERSION) self.group.clear_pose_targets() self.endeffector = intera_interface.Gripper("right") self.uri = "mongodb://*****:*****@sawyer-mongo.documents.azure.com:10255/?ssl=true" self.Mongoclient = MongoClient(self.uri) if Name == None: self.poleIoTHub() else: self.completeCommands(Name)
def init_sawyer(): """ basic setup stuff """ rospy.loginfo("initializing sawyer...") limb = intera_interface.Limb('right') hd = intera_interface.HeadDisplay() head = intera_interface.Head() limb.move_to_neutral(timeout=TIMEOUT) # TODO: Hardocoded to jon's computer hd.display_image( "/home/jon/catkin_ws/src/sawyer_simulator/sawyer_gazebo/share/images/sawyer_sdk_research.png" ) head.set_pan(0.0) rospy.loginfo("sawyer initialized") return limb, hd, head
def main(): """RSDK Head Display Example: Displays a given image file or multiple files on the robot's face. Pass the relative or absolute file path to an image file on your computer, and the example will read and convert the image using cv_bridge, sending it to the screen as a standard ROS Image Message. """ epilog = """ Notes: Max screen resolution is 1024x600. Images are always aligned to the top-left corner. Image formats are those supported by OpenCv - LoadImage(). """ arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__, epilog=epilog) required = parser.add_argument_group('required arguments') required.add_argument( '-f', '--file', nargs='+', help= 'Path to image file to send. Multiple files are separated by a space, eg.: a.png b.png' ) parser.add_argument( '-l', '--loop', action="store_true", help='Display images in loop, add argument will display images in loop' ) parser.add_argument( '-r', '--rate', type=float, default=1.0, help='Image display frequency for multiple and looped images.') args = parser.parse_args() rospy.init_node("head_display_example", anonymous=True) head_display = intera_interface.HeadDisplay() head_display.display_image(args.file, args.loop, args.rate)
def headText(self, q): t = 'x:{x} y:{y} z:{z} w:{w}' template = {'x': q.x, 'y': q.y, 'z': q.z, 'w': q.w} text = t.format(**template) h = 600 w = 1024 img = np.zeros((h, w, 3), np.uint8) img.fill(255) #cv2.putText(img,text, (50,200), cv2.FONT_HERSHEY_SIMPLEX, 6, 100,14) y0, dy = 150, 120 for i, line in enumerate(text.split(' ')): y = y0 + i * dy cv2.putText(img, line, (50, y), cv2.FONT_HERSHEY_SIMPLEX, 4, 2, 10) cv2.imwrite("/home/microshak/Pictures/head.png", img) head_display = intera_interface.HeadDisplay() head_display.display_image("/home/microshak/Pictures/head.png", False, 1.0)
def __init__(self): self._done = False self._head = intera_interface.Head() self._limb = intera_interface.Limb() self._robot_state = intera_interface.RobotEnable(CHECK_VERSION) self._init_state = self._robot_state.state().enabled self._robot_state.enable() self._display = intera_interface.HeadDisplay() self._last_angles = self._limb.joint_angles() self._last_head_pan = self._head.pan() move_arm_out_of_way_service = rospy.Service('arm/move_away', MoveArm, self.move_arm_out_of_way) return_to_last_position = rospy.Service('arm/return_to_position', MoveArm, self.return_to_last_position) move_to_rest = rospy.Service('arm/move_to_rest', MoveArm, self.move_to_rest) move_arm_to_rest = rospy.Service('arm/move_arm_to_rest', MoveArm, self.move_arm_to_rest)
def display_png(img_name, bg_color=(255,255,255)): """ Displays a png image from imagaes/display directory on Sawyer's screen. Cannot handle images larger than 1024 x 600. Good emojis here: https://emojiisland.com/pages/free-download-emoji-icons-png :param img_name: (string) - name of image to display :param bg_color: (b, g, r) color to make background """ disp = intera_interface.HeadDisplay() img = np.zeros(shape=[600, 1024, 3], dtype=np.uint8) b, g, r = bg_color img[:,:,0] = b img[:, :, 1] = g img[:, :, 2] = r # sloppy way to get paths to display images- don't use but may be useful in other nodes img_dir = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) img_dir = os.path.join(img_dir, 'images', 'display') img_path = os.path.join(img_dir, img_name) pic = cv2.imread(img_path, cv2.IMREAD_UNCHANGED) alpha = pic[:, :, 3] h, w = alpha.shape for x in range(h): for y in range(w): if not alpha[x, y]: pic[x, y] = (b, g, r, 1) offh = int((img.shape[0] - pic.shape[0]) / 2) offw = int((img.shape[1] - pic.shape[1]) / 2) img[offh:(offh + pic.shape[0]), offw:(offw + pic.shape[1]), :] = pic[:, :, :3] cv2.imwrite('temp.png', img) disp.display_image('temp.png') os.remove('temp.png')
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.endpoint_topic = rospy.Publisher('/EndpointInfo', EndpointInfo, queue_size=10) # Subscribing topics rospy.Subscriber('/robot/limb/right/endpoint_state', intera_core_msgs.msg.EndpointState, self.forwardEndpointState) # Lights self.lights = intera_interface.Lights() for light in self.lights.list_all_lights(): self.lights.set_light_state(light,False) # Initialization complete. Spin. rospy.loginfo('Ready.') r = rospy.Rate(10) while not rospy.is_shutdown(): suppress_cuff_interaction.publish() r.sleep()
def main(): rp = intera_interface.RobotParams() valid_limbs = rp.get_limb_names() rospy.init_node('doctor', log_level=rospy.DEBUG) rs = intera_interface.RobotEnable(CHECK_VERSION) head_display = intera_interface.HeadDisplay() head_display.display_image("./head_display_pics/redcross.png", False, 1.0) init_state = rs.state().enabled rs.enable() rospy.logdebug("Closing gripper...") #raw_input("Press any key to close gripper") right_gripper = intera_interface.gripper.Gripper('right') right_gripper.calibrate() #rospy.sleep(2.0) right_gripper.open() #raw_input("Press any key to close gripper") right_gripper.close() #rospy.sleep(2.0) rospy.logdebug("Gripper closed") doctor = doctor_sawyer(valid_limbs[0]) doctor.find_table() while (not rospy.is_shutdown()): action = raw_input("(P)oke, (M)easurement?") if (action == "P"): print(doctor.probe(2, 2, 0.02, 0.02)) if (action == "M"): doctor.start_measure() if (action == "P1"): doctor.probe(0, 0, 0.02, 0.02) rospy.spin()
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, config='cfg/sawyer_ctrl_config.yaml', controlType='EEImpedance', use_safenet=False, node_name='sawyer_interface'): """ Initialize Sawyer Robot for control Args: use_safenet: bool whether to use the Safenet to constrain ee positions and velocities node_name: str name of the node for ROS Attributes: ee_position ee_orientation """ self.log_start_times = [] self.log_end_times = [] self.cmd_start_times = [] self.cmd_end_times = [] # Connect to redis client # use default port 6379 at local host. self.config = YamlConfig(config) self.redisClient = RobotRedis(**self.config['redis']) self.redisClient.flushall() # Timing self.startTime = time.time() self.endTime = time.time() self.action_set = False self.model = Model() self.ee_name = self.config['sawyer']['end_effector_name'] if config is not None: if self.config['controller']['interpolator']['type'] == 'linear': interp_kwargs = { 'max_dx': 0.005, 'ndim': 3, 'controller_freq': 500, 'policy_freq': 20, 'ramp_ratio': 0.2 } self.interpolator_pos = LinearInterpolator(**interp_kwargs) self.interpolator_ori = LinearOriInterpolator(**interp_kwargs) rospy.loginfo( "Linear interpolator created with params {}".format( interp_kwargs)) else: self.interpolator_pos = None self.interpolator_ori = None self.interpolator_goal_set = False start = time.time() rospy.loginfo('Initializing Sawyer robot interface...') try: self._head = iif.Head() self._display = iif.HeadDisplay() self._lights = iif.Lights() self._has_head = True except: rospy.logerr('No head found, head functionality disabled') self._has_head = False self._limb = iif.Limb(limb="right", synchronous_pub=False) self._joint_names = self._limb.joint_names() self.cmd = [] try: self._gripper = iif.Gripper() self._has_gripper = True except: rospy.logerr('No gripper found, gripper control disabled') self._has_gripper = False self._robot_enable = iif.RobotEnable(True) self._params = iif.RobotParams() self.blocking = False # set up internal pybullet simulation for jacobian and mass matrix calcs. self._clid = pb.connect(pb.DIRECT) pb.resetSimulation(physicsClientId=self._clid) # TODO: make this not hard coded sawyer_urdf_path = self.config['sawyer']['arm']['path'] self._pb_sawyer = pb.loadURDF( fileName=sawyer_urdf_path, basePosition=self.config['sawyer']['arm']['pose'], baseOrientation=pb.getQuaternionFromEuler( self.config['sawyer']['arm']['orn']), globalScaling=1.0, useFixedBase=self.config['sawyer']['arm']['is_static'], flags=pb.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT | pb.URDF_USE_INERTIA_FROM_FILE, physicsClientId=self._clid) # For pybullet dof self._motor_joint_positions = self.get_motor_joint_positions() try: ns = "ExternalTools/right/PositionKinematicsNode/IKService" self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK) rospy.wait_for_service(ns, 5.0) self._ik_service = True except: rospy.logerr('IKService from Intera timed out') self._ik_service = False self._joint_names = self.config['sawyer']['limb_joint_names'] self.free_joint_dict = self.get_free_joint_dict() self.joint_dict = self.get_joint_dict() self._link_id_dict = self.get_link_dict() rospy.loginfo('Sawyer initialization finished after {} seconds'.format( time.time() - start)) # Set desired pose to initial self.neutral_joint_position = self.config['sawyer'][ 'neutral_joint_angles'] #[0, -1.18, 0.00, 2.18, 0.00, 0.57, 3.3161] self.prev_cmd = np.asarray(self.neutral_joint_position) self.current_cmd = self.prev_cmd self.reset_to_neutral() self.default_control_type = self.config['controller']['selected_type'] self.default_params = self.config['controller']['Real'][ self.default_control_type] self.redisClient.set(CONTROLLER_CONTROL_TYPE_KEY, self.default_control_type) self.redisClient.set(CONTROLLER_CONTROL_PARAMS_KEY, json.dumps(self.default_params)) self.pb_ee_pos_log = [] self.pb_ee_ori_log = [] self.pb_ee_v_log = [] self.pb_ee_w_log = [] # Set initial redis keys self._linear_jacobian = None self._angular_jacobian = None self._jacobian = None self._calc_mass_matrix() self._calc_jacobian() self.update_redis() self.update_model() self.controlType = self.get_control_type() self.control_dict = self.get_controller_params() self.controller = self.make_controller_from_redis( self.controlType, self.control_dict) self.redisClient.set(ROBOT_CMD_TSTAMP_KEY, time.time()) self.redisClient.set(ROBOT_SET_GRIPPER_CMD_KEY, 0.1) self.redisClient.set(ROBOT_SET_GRIPPER_CMD_TSTAMP_KEY, time.time()) self.last_cmd_tstamp = self.get_cmd_tstamp() self.last_gripper_cmd_tstamp = self.get_gripper_cmd_tstamp() self.prev_gripper_state = self.des_gripper_state rospy.logdebug('Control Interface initialized') # TIMING TEST ONLY self.timelog_start = open('cmd_tstamp.txt', 'w') self.timelog_end = open('cmd_set_time.txt', 'w') self.controller_times = [] self.loop_times = [] self.cmd_end_time = []
import tf.transformations as tr import tf import rospy from std_msgs.msg import Bool, Int32, Float64 from geometry_msgs.msg import Pose, Point, Quaternion from urdf_parser_py.urdf import URDF from pykdl_utils.kdl_kinematics import KDLKinematics import PyKDL import os os.chdir("/home/irobot/catkin_ws/src/ddpg/scripts") rospy.init_node('pose_acquisition') limb = intera_interface.Limb("right") head_display = intera_interface.HeadDisplay() head_display.display_image('/home/irobot/Downloads' + "/Cute.png") cuff = intera_interface.Cuff() tf_listenser = tf.TransformListener() endpt_ori = [0.0, 0.0, 0.0, 0.0] endpt_pos = [0.0, 0.0, 0.0] position_x = list() position_y = list() position_z = list() orient_x = list() orient_y = list() orient_z = list() orient_w = list() roll = list() pitch = list()
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 display_image(): global g_image_path head_display = intera_interface.HeadDisplay() head_display.display_image(g_image_path)
def main(): # Initialize the node rospy.init_node('MoveCartesian') # Initialize interfaces limb = intera_interface.Limb('right') head_display = intera_interface.HeadDisplay() try: gripper = intera_interface.Gripper('right_gripper') gripper.calibrate() nsteps = 5.0 # Increase it for a finer motion dgripper = gripper.MAX_POSITION / nsteps except ValueError: rospy.logerr("Could not detect a gripper") return # Set the path for images folder = str(packages.get_pkg_dir('sawyer_utec')) + '/images/' # Diplay the UTEC logo in the robot head head_display.display_image(folder + 'up1.jpg', False, 1.0) # Move arm to the initial position limb.move_to_neutral() jangles_neutral = limb.joint_angles() if (False): print jangles_neutral # Open the gripper gripper.open() # =================== # Motion for object 1 # =================== # PRIMER PISO # VASO 1 MOD # pose_init1 = ((0.6, 0.50, 0.03), quat_init) # pose_final1 = ((0.6, -0.5, 0.02), quat_final) # VASO 2 MOD # pose_initial2 = ((0.6, 0.40, 0.03), quat_init) # pose_final2 = ((0.6, -0.42, 0.02), quat_final) # VASO 3 # pose_initial3 = ((0.6, 0.32, 0.03), quat_init) # pose_final3 = ((0.6, -0.34, 0.02), quat_final) # SEGUNDO PISO # VASO 4 # pose_initial4 = ((0.7, 0.5, 0.03), quat_init) # pose_final4 = ((0.6, -0.46, 0.12), quat_final) # VASO 5 # pose_initial5 = ((0.7, 0.4, 0.03), quat_init) # pose_final5 = ((0.6, -0.38, 0.12), quat_final) # VASO 6 # pose_initial = ((0.7, 0.4, 0.03), quat_init) # pose_final = ((0.6, -0.38, 0.12), quat_final) # Initial and final poses of the object quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) offset_table_z = -0.02 # vaso 1 pose_initial1 = ((0.6, 0.50, 0.03 - offset_table_z), quat_init) quat_final = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) pose_final1 = ((0.6, -0.5, 0.02 - offset_table_z), quat_final) # Gripper opening (0*dgripper [closed] to nsteps*dgripper [open]) gripper_opening = 3.5 * dgripper # Offset in z (from the desired position) for pre-grasping z_pre_grasp = 0.20 pick_place(limb, gripper, pose_initial1, pose_final1, gripper_opening, z_pre_grasp, jangles_neutral) # # vaso 2 # quat_init2 = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) # pose_initial2 = ((0.6, 0.40, 0.03 - offset_table_z), quat_init) # quat_final2 = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) # pose_final2 = ((0.6, -0.42, 0.02 - offset_table_z), quat_final) # # Gripper opening (0*dgripper [closed] to nsteps*dgripper [open]) # gripper_opening = 3.5*dgripper # # Offset in z (from the desired position) for pre-grasping # z_pre_grasp = 0.20 # pick_place(limb, gripper, pose_initial2, pose_final2, gripper_opening, z_pre_grasp, jangles_neutral) # # vaso 3 # quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) # pose_initial3 = ((0.6, 0.32, 0.03 - offset_table_z), quat_init) # quat_final = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) # pose_final3 = ((0.6, -0.34, 0.02 - offset_table_z), quat_final) # # Gripper opening (0*dgripper [closed] to nsteps*dgripper [open]) # gripper_opening = 3.5*dgripper # # Offset in z (from the desired position) for pre-grasping # z_pre_grasp = 0.20 # pick_place(limb, gripper, pose_initial3, pose_final3, gripper_opening, z_pre_grasp, jangles_neutral) # # vaso 4 # quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) # pose_initial4 = ((0.7, 0.5, 0.03 - offset_table_z), quat_init) # quat_final = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) # pose_final4 = ((0.6, -0.46, 0.12 - offset_table_z), quat_final) # # Gripper opening (0*dgripper [closed] to nsteps*dgripper [open]) # gripper_opening = 3.5*dgripper # # Offset in z (from the desired position) for pre-grasping # z_pre_grasp = 0.20 # pick_place(limb, gripper, pose_initial4, pose_final4, gripper_opening, z_pre_grasp, jangles_neutral) # # vaso 5 # quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) # pose_initial5 = ((0.7, 0.4, 0.03 - offset_table_z), quat_init) # quat_final = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) # pose_final5 = ((0.6, -0.38, 0.12 - offset_table_z), quat_final) # # Gripper opening (0*dgripper [closed] to nsteps*dgripper [open]) # gripper_opening = 3.5*dgripper # # Offset in z (from the desired position) for pre-grasping # z_pre_grasp = 0.20 # pick_place(limb, gripper, pose_initial5, pose_final5, gripper_opening, z_pre_grasp, jangles_neutral) # #vaso 6 # quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) # pose_initial = ((0.7, 0.32, 0.03 - offset_table_z), quat_init) # quat_final = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) # pose_final = ((0.6, -0.42, 0.22 - offset_table_z), quat_final) # # Gripper opening (0*dgripper [closed] to nsteps*dgripper [open]) # gripper_opening = 3.5*dgripper # # Offset in z (from the desired position) for pre-grasping # z_pre_grasp = 0.20 # pick_place(limb, gripper, pose_initial, pose_final, gripper_opening, z_pre_grasp, jangles_neutral) limb.move_to_neutral() jangles_neutral = limb.joint_angles() if (False): print jangles_neutral # Display another face head_display.display_image(folder + 'sleep1.png', False, 1.0)
def main(): # Initialize the node rospy.init_node('DemoVasos') # Initialize interfaces limb = intera_interface.Limb('right') head_display = intera_interface.HeadDisplay() try: gripper = intera_interface.Gripper('right_gripper') gripper.calibrate() nsteps = 5.0 # Increase it for a finer motion dgripper = gripper.MAX_POSITION / nsteps except ValueError: rospy.logerr("Could not detect a gripper") return # Set the path for images folder = str(packages.get_pkg_dir('sawyer_utec')) + '/images/' # Diplay the UTEC logo in the robot head head_display.display_image(folder + 'frente.jpg', False, 1.0) # Set camera and tag searcher camera_setup() tags = tag_searcher() # Move arm to the initial position limb.move_to_neutral() jangles_neutral = limb.joint_angles() if (False): print jangles_neutral # Open the gripper gripper.open() # Initial and final poses of the object quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) quat_rot = quaternionFromAxisAngle(90.0, (0.0, 0.0, 1.0)) quat_init = quaternionMult(quat_init, quat_rot) offset_table_z = -0.05 # vaso 1 pose_initial1 = ((0.6, 0.4, 0.03 - offset_table_z), quat_init) quat_tmp = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) quat_final = quaternionMult(quat_tmp, quaternionFromAxisAngle(90.0, (0.0, 0.0, 1.0))) pose_final1 = ((0.6, -0.5, 0.02 - offset_table_z), quat_final) # Gripper opening (0*dgripper [closed] to nsteps*dgripper [open]) gripper_opening = 3.5 * dgripper # Offset in z (from the desired position) for pre-grasping z_pre_grasp = 0.17 pick_place_autonomous(limb, gripper, pose_initial1, pose_final1, gripper_opening, z_pre_grasp, jangles_neutral, tags, head_display) # vaso 2 quat_init2 = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) pose_initial2 = ((0.6, 0.40, 0.03 - offset_table_z), quat_init) quat_tmp = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) quat_final2 = quaternionMult( quat_tmp, quaternionFromAxisAngle(90.0, (0.0, 0.0, 1.0))) # pose_final2 = ((0.6, -0.42, 0.02 - offset_table_z), quat_final2) pose_final2 = ((0.6, -0.38, 0.02 - offset_table_z), quat_final2) # Gripper opening (0*dgripper [closed] to nsteps*dgripper [open]) gripper_opening = 3.5 * dgripper # Offset in z (from the desired position) for pre-grasping z_pre_grasp = 0.17 pick_place_autonomous(limb, gripper, pose_initial2, pose_final2, gripper_opening, z_pre_grasp, jangles_neutral, tags, head_display) # vaso 3 quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) pose_initial3 = ((0.6, 0.40, 0.03 - offset_table_z), quat_init) quat_tmp = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) quat_final3 = quaternionMult( quat_tmp, quaternionFromAxisAngle(90.0, (0.0, 0.0, 1.0))) # pose_final3 = ((0.6, -0.34, 0.02 - offset_table_z), quat_final3) pose_final3 = ((0.6, -0.26, 0.02 - offset_table_z), quat_final3) # Gripper opening (0*dgripper [closed] to nsteps*dgripper [open]) gripper_opening = 3.5 * dgripper # Offset in z (from the desired position) for pre-grasping z_pre_grasp = 0.17 pick_place_autonomous(limb, gripper, pose_initial3, pose_final3, gripper_opening, z_pre_grasp, jangles_neutral, tags, head_display) # vaso 4 quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) pose_initial4 = ((0.6, 0.40, 0.03 - offset_table_z), quat_init) quat_tmp = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) quat_final4 = quaternionMult( quat_tmp, quaternionFromAxisAngle(90.0, (0.0, 0.0, 1.0))) # pose_final4 = ((0.6, -0.46, 0.12 - offset_table_z), quat_final4) pose_final4 = ((0.5, -0.5, 0.02 - offset_table_z), quat_final4) # Gripper opening (0*dgripper [closed] to nsteps*dgripper [open]) gripper_opening = 3.5 * dgripper # Offset in z (from the desired position) for pre-grasping z_pre_grasp = 0.17 pick_place_autonomous(limb, gripper, pose_initial4, pose_final4, gripper_opening, z_pre_grasp, jangles_neutral, tags, head_display) # vaso 5 quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) pose_initial5 = ((0.6, 0.40, 0.03 - offset_table_z), quat_init) quat_tmp = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) quat_final5 = quaternionMult( quat_tmp, quaternionFromAxisAngle(90.0, (0.0, 0.0, 1.0))) #pose_final5 = ((0.6, -0.38, 0.12 - offset_table_z), quat_final5) pose_final5 = ((0.5, -0.38, 0.02 - offset_table_z), quat_final5) # Gripper opening (0*dgripper [closed] to nsteps*dgripper [open]) gripper_opening = 3.5 * dgripper # Offset in z (from the desired position) for pre-grasping z_pre_grasp = 0.17 pick_place_autonomous(limb, gripper, pose_initial5, pose_final5, gripper_opening, z_pre_grasp, jangles_neutral, tags, head_display) #vaso 6 quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) pose_initial = ((0.6, 0.40, 0.03 - offset_table_z), quat_init) quat_tmp = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0)) quat_final6 = quaternionMult( quat_tmp, quaternionFromAxisAngle(90.0, (0.0, 0.0, 1.0))) # pose_final = ((0.6, -0.42, 0.22 - offset_table_z), quat_final6) pose_final = ((0.5, -0.26, 0.02 - offset_table_z), quat_final6) # Gripper opening (0*dgripper [closed] to nsteps*dgripper [open]) gripper_opening = 3.5 * dgripper # Offset in z (from the desired position) for pre-grasping z_pre_grasp = 0.17 pick_place_autonomous(limb, gripper, pose_initial, pose_final, gripper_opening, z_pre_grasp, jangles_neutral, tags, head_display) limb.move_to_neutral() jangles_neutral = limb.joint_angles() if (False): print jangles_neutral # Display another face head_display.display_image(folder + 'sleep1.png', False, 1.0)
def init_head_dis(self): head_display = intera_interface.HeadDisplay() head_display.display_image("bob.jpg")
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()