def __init__(self): """ Init BaxterRobot object """ # Init the limbs of the robot rospy.init_node("inverse_kinematics_task") self._limb_left = baxter_interface.Limb("left") self._limb_right = baxter_interface.Limb("right") # Init grippers of the robot self._gripper_left = baxter_interface.Gripper( "left", baxter_interface.CHECK_VERSION) self._gripper_right = baxter_interface.Gripper( "right", baxter_interface.CHECK_VERSION) self._gripper_left.calibrate() self._gripper_right.calibrate() self._gripper_left_pos = 100.0 self._gripper_right_pos = 100.0 # Create Navigator I/O self._navigator_left = baxter_interface.Navigator("left") self._navigator_right = baxter_interface.Navigator("right") # State of the robot self.state = 0 # Init the robot rospy.loginfo("Getting robot state... ") self._rs = baxter_interface.RobotEnable() self._init_state = self._rs.state().enabled rospy.loginfo("Enabling robot... ") self._rs.enable()
def __init__(self, limb, filename, speed, accuracy): # Create baxter_interface limb instance self._arm = limb self.file=filename self._limb = baxter_interface.Limb(self._arm) self.result = [False, ''] # Parameters which will describe joint position moves self._speed = speed self._accuracy = accuracy #activate all limbs self._limb_left = baxter_interface.Limb("left") self._limb_right = baxter_interface.Limb("right") self._gripper_left = baxter_interface.Gripper("left", CHECK_VERSION) self._gripper_right = baxter_interface.Gripper("right", CHECK_VERSION) self._io_left_lower = baxter_interface.DigitalIO('left_lower_button') self._io_left_upper = baxter_interface.DigitalIO('left_upper_button') self._io_right_lower = baxter_interface.DigitalIO('right_lower_button') self._io_right_upper = baxter_interface.DigitalIO('right_upper_button') self._navigator_io = baxter_interface.Navigator("right") # Verify Grippers Have No Errors and are Calibrated if self._gripper_left.error(): self._gripper_left.reset() if self._gripper_right.error(): self._gripper_right.reset() if (not self._gripper_left.calibrated() and self._gripper_left.type() != 'custom'): self._gripper_left.calibrate() if (not self._gripper_right.calibrated() and self._gripper_right.type() != 'custom'): self._gripper_right.calibrate() # Recorded waypoints self._waypoints = list() # Recording state self._is_recording = False # Verify robot is enabled print("Getting robot state... ") self._rs = baxter_interface.RobotEnable() self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() # Create Navigator I/O self._navigator_io = baxter_interface.Navigator(self._arm)
def __init__(self, filename, rate): """ Records joint data to a file at a specified rate. """ self._filename = filename self._raw_rate = rate self._rate = rospy.Rate(rate) self._start_time = rospy.get_time() self._time_shift = 0 self._done = False self._pause = False #self._navigator_io = baxter_interface.Navigator("right") self._limb_left = baxter_interface.Limb("left") self._limb_right = baxter_interface.Limb("right") self._gripper_left = baxter_interface.Gripper("left", CHECK_VERSION) self._gripper_right = baxter_interface.Gripper("right", CHECK_VERSION) self._io_left_lower = baxter_interface.DigitalIO('left_lower_button') self._io_left_upper = baxter_interface.DigitalIO('left_upper_button') self._io_right_lower = baxter_interface.DigitalIO('right_lower_button') self._io_right_upper = baxter_interface.DigitalIO('right_upper_button') self._navigator_io = baxter_interface.Navigator("right") # Verify Grippers Have No Errors and are Calibrated if self._gripper_left.error(): self._gripper_left.reset() if self._gripper_right.error(): self._gripper_right.reset() if (not self._gripper_left.calibrated() and self._gripper_left.type() != 'custom'): self._gripper_left.calibrate() if (not self._gripper_right.calibrated() and self._gripper_right.type() != 'custom'): self._gripper_right.calibrate()
def __init__(self, limb, speed, accuracy): self.lastRotate = time.time() # Create baxter_interface limb instance self._arm = limb self._limb = baxter_interface.Limb(self._arm) # Parameters which will describe joint position moves self._speed = speed self._accuracy = accuracy # Recorded waypoints self._waypoints = list() self._suctionPoints = list() self._sucking = False self._right_gripper = baxter_interface.Gripper('right') # Recording state self._is_recording = False # Verify robot is enabled print("Getting robot state... ") self._rs = baxter_interface.RobotEnable() self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() # Create Navigator I/O self._navigator_io = baxter_interface.Navigator(self._arm) self._close_io = baxter_interface.DigitalIO('%s_upper_button' % (self._arm,)) # 'dash' btn self._open_io = baxter_interface.DigitalIO('%s_lower_button' % (self._arm,)) # 'circle' btn
def setButtonHandlers(arm): # inputs if arm == 'left': _close_io = baxter_interface.DigitalIO('%s_upper_button' % (arm,)) # 'dash' btn #_close_io.state_changed.connect(toggleLeftGripper) _open_io = baxter_interface.DigitalIO('%s_lower_button' % (arm,)) # 'circle' btn #_open_io.state_changed.connect(rotateLeftGripper) Lnav = (baxter_interface.Navigator('left')) #Lnav.button0_changed.connect(scrollWheel) #Lnav.wheel_changed.connect(wheel_moved) elif arm == 'right': pass #Rnav = (baxter_interface.Navigator('right')) #press 'ok' button to record #Rnav.button0_changed.connect(pickFlower) #press 'Rethink' button to record everything #Rnav.button2_changed.connect(recordNewFlowerPick) else: print 'uhm something went wrong here'
def blink(): navs = ( baxter_interface.Navigator('left'), baxter_interface.Navigator('right'), baxter_interface.Navigator('torso_left'), baxter_interface.Navigator('torso_right'),) print ("Blinking LED's for 10 seconds") rate = rospy.Rate(10) i = 0 while not rospy.is_shutdown() and i < 100: for nav in navs: nav.inner_led = not nav.inner_led nav.outer_led = not nav.outer_led rate.sleep() i += 1
def echo_input(): def b0_pressed(v): print("Button 0: %s" % (v, )) def b1_pressed(v): print("Button 1: %s" % (v, )) def b2_pressed(v): print("Button 2: %s" % (v, )) def wheel_moved(v): print("Wheel Increment: %d, New Value: %s" % (v, nav.wheel)) nav = baxter_interface.Navigator('left') nav.button0_changed.connect(b0_pressed) nav.button1_changed.connect(b1_pressed) nav.button2_changed.connect(b2_pressed) nav.wheel_changed.connect(wheel_moved) print("Press input buttons on the left 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, arm): self.arm = arm self.brazo = baxter_interface.Limb(self.arm) # Declaracion del boton self.boton_brazo = baxter_interface.Navigator(self.arm) # Gripper self.gripper = baxter_interface.Gripper(self.arm) # Calibracion self.gripper.calibrate() self.pos_caja = [1] * 6 self.pos_caja_vacia = [1] * 6 self.pos_caja_llena = [1] * 6 # Rotacion estandar self.rot = [math.pi, 0, -math.pi / 2] # Publicador a pantalla self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=10) # Datos de cajas self.num_cajas = 2 self.altura_cajas = 0.1 self.nivel = 0 self.contador = [False] * 6 self.lugar = 0
def __init__(self, intname) : self.intname =intname self.toggled = False self.blinking = False self.nav = baxter_interface.Navigator(intname) self.nav.button0_changed.connect(self.b0_pressed) self.nav.button1_changed.connect(self.b1_pressed) self.nav.button2_changed.connect(self.b2_pressed) self.b=[0,0,0]
def main(): def left_pressed(v): global pressed pressed = not pressed if pressed: print(left.joint_angles()) print def left_wheel_moved(v): print(left.joint_angles()) print def right_pressed(v): pressed = not pressed if pressed: print(right.joint_angles()) print def right_wheel_moved(v): print(right.joint_angles()) print rospy.init_node("navigator_test") left = baxter_interface.Limb('left') grip_left = baxter_interface.Gripper('left', CHECK_VERSION) nav_left = baxter_interface.Navigator('left') nav_right = baxter_interface.Navigator('right') nav_left.button0_changed.connect(left_pressed) nav_left.button1_changed.connect(left_pressed) nav_left.button2_changed.connect(left_pressed) nav_left.wheel_changed.connect(left_wheel_moved) nav_right.button0_changed.connect(right_pressed) nav_right.button1_changed.connect(right_pressed) nav_right.button2_changed.connect(right_pressed) nav_right.wheel_changed.connect(right_wheel_moved) while not rospy.is_shutdown(): rate = rospy.Rate(1) rate.sleep()
def save_posture(posture_name, button_control=True, arm=None, record_path="posture_records.yaml"): if button_control: left_nav = baxter_interface.Navigator('left') right_nav = baxter_interface.Navigator('right') while not left_nav.button0 and not right_nav.button0: rospy.sleep(0.1) #save the position left_joint_angles = baxter_interface.Limb('left').joint_angles() right_joint_angles = baxter_interface.Limb('right').joint_angles() posture_list = dict() #resolve path record_path = alloy.ros.resolve_res_path(record_path,"tbd_baxter_tools") #create the file at the root of `tbd_baxter_tools\res` if doesn't exist if record_path is None: record_path = os.path.join(alloy.ros.create_res_dir("tbd_baxter_tools"),"posture_records.yaml") #save them to some type of files if not os.path.exists(record_path): yaml.dump(posture_list,file(record_path,'w')) with open(record_path,'rw') as f: posture_list = yaml.load(f) if arm == 'right': posture_list[posture_name] = { 'right': right_joint_angles } elif arm == 'left': posture_list[posture_name] = { 'left': left_joint_angles, } else: posture_list[posture_name] = { 'left': left_joint_angles, 'right': right_joint_angles } yaml.dump(posture_list, file(record_path,'w'))
def __init__(self): """ Initialize control with Baxter """ # verify robot is enabled rospy.loginfo("Getting robot state... ") self._rs = baxter_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled rospy.loginfo("Enabling robot... ") self._rs.enable() rospy.on_shutdown(self.clean_shutdown) rospy.loginfo("Running...") # Get the body running self.head = baxter_interface.Head() self.right_arm = baxter_interface.Limb('right') self.left_arm = baxter_interface.Limb('left') self.left_gripper = baxter_interface.Gripper('left', CHECK_VERSION) self.right_gripper = baxter_interface.Gripper('right', CHECK_VERSION) self.left_navigator = baxter_interface.Navigator('left') self.right_navigator = baxter_interface.Navigator('right') self.left_torso_navigator = baxter_interface.Navigator('torso_left') self.right_torso_navigator = baxter_interface.Navigator('torso_right')
def __init__(self): self.goalPose = {} #Initialize ROS node rospy.init_node('Pick_N_Place', anonymous=True) #Enable Robot self.en = baxter_interface.RobotEnable() self.en.enable() print 'Robot Enabled...' #Initialize right arm and right gripper self.rightArm = baxter_interface.Limb('right') self.rightArm.set_joint_position_speed(0.5) self.rightArm.set_command_timeout(0.1) self.rightGripper = baxter_interface.Gripper('right') #instantiate range sensor self.rs = BaxterRangeSensor() #initialize right arm wheel button self.rightNavigator = baxter_interface.Navigator('right') #Blink LEDs self.rightNavigator.inner_led = False self.rightNavigator.outer_led = False for i in range(0, 6): self.rightNavigator.inner_led = not self.rightNavigator.inner_led self.rightNavigator.outer_led = not self.rightNavigator.outer_led rospy.sleep(0.5) #initialize baxter KDL self.KDL = baxter_kinematics('right') print '' #move arm to neutral position and calibrate gripper self.rightArm.move_to_joint_positions(neutralPose) self.rightGripper.calibrate() #Create z-height PID control self.zReg = PID.PID(Kp=0.2, Kd=0.1, Ki=0, scaleFactor=2) #create x PID control self.xReg = PID.PID(Kp=0.1, Kd=0.01, Ki=0, scaleFactor=0.005) #create y PID control self.yReg = PID.PID(Kp=0.1, Kd=0.01, Ki=0, scaleFactor=0.005) #subscribe to 'center' message (sent from CV module) and get center of object self.centerFromCv = rospy.Subscriber('/center', Point, self._get_center) self.centerX = 0 self.centerY = 0
def main_baxter(limb='right'): import baxter_interface from visual_mpc.envs.robot_envs.baxter.baxter_impedance import BaxterImpedanceController controller = BaxterImpedanceController('baxter', True, gripper_attached='none', limb=limb) 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 = baxter_interface.Navigator(limb) navigator.button0_changed.connect(print_eep) rospy.spin()
def main(): rospy.init_node('light_show') navs = ( baxter_interface.Navigator('left'), baxter_interface.Navigator('right'), baxter_interface.Navigator('torso_left'), baxter_interface.Navigator('torso_right'), ) rate = rospy.Rate(10) i = 0 for nav in navs: nav.inner_led = True nav.outer_led = False while not rospy.is_shutdown() and i < 10: for nav in navs: nav.inner_led = not nav.inner_led nav.outer_led = not nav.outer_led rate.sleep() i += 1 off_lights(navs)
def __init__(self, arm, separacion): self.arm = arm # Declaracion del boton self.boton_brazo = baxter_interface.Navigator(self.arm) # Gripper self.gripper = baxter_interface.Gripper(self.arm) # Calibracion self.gripper.calibrate() # Declaracion del brazo self.brazo = baxter_interface.Limb(self.arm) self.pos_ini = [1] * 6 self.pos_fin = [1] * 6 # Declaracion de caja self.caja = bandeja(0.160, 0.250, 3, 3, -0.2) self.separacion = separacion self.centros = [x[:] for x in self.caja.centros] # Rotacion estandar self.rot = [math.pi, 0, -math.pi / 2] # Publicador a pantalla self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=10) self.contador = [False] * self.caja.espacios self.lugar = 0 self.fila_max = 12 self.fila = 0 self.mov_caja = movimiento_cajas('right') self.definir_posicion() self.mov_caja.definir_posicion() self.publicador_a_pantalla('Simulador Produccion')
def __init__(self): self._done = False self._ur = 0.75 self._sub_ur = rospy.Subscriber('/trust/robot_states', RobotData, self._get_robot) self._pub_ur = rospy.Publisher('trust/ur_manual', Float64, queue_size=1) # Use navigator for start and stop of recording self._nav_right = baxter_interface.Navigator('right') self._nav_right.button0_changed.connect(self._nav_b0_pressed) self._nav_right.wheel_changed.connect(self._nav_wheel_changed) self._cvbridge = CvBridge() self._image = numpy.zeros((1024, 600, 3), numpy.uint8) self._pub_img = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1)
def __init__(self, limb, distance, loops, calibrate): # Create baxter_interface limb instance self._arm = limb self._limb = baxter_interface.Limb(self._arm) self._gripper = baxter_interface.Gripper(self._arm) self._cam = baxter_interface.CameraController(self._arm + '_hand_camera') self._cam.gain = 1 self._ik_srv = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" self._iksvc = rospy.ServiceProxy(self._ik_srv, SolvePositionIK) self._ikreq = SolvePositionIKRequest() self._hover_distance = distance self._num_loops = loops self._domino_source_approach = None self._domino_source = None self._domino_source_jp = None self._calibration_point = None # Recorded waypoints self._waypoints = list() self._source_point = None # Recording state self._is_recording = False # Verify robot is enabled print("Getting robot state... ") self._rs = baxter_interface.RobotEnable() self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() # Calibrate Gripper if (calibrate): self._gripper.calibrate() if not (self._gripper.calibrated() or self._gripper.calibrate()): rospy.logwarn("%s (%s) calibration failed.", self._gripper.name.capitalize(), self._gripper.type()) # Create Navigator I/O self._navigator_io = baxter_interface.Navigator(self._arm) self._overhead_orientation = Quaternion(x=-0.0249590815779, y=0.999649402929, z=0.00737916180073, w=0.00486450832011)
def __init__(self, filename, rate): """ Records joint data to a file at a specified rate. """ self._filename = filename self._raw_rate = rate self._rate = rospy.Rate(rate) self._start_time = rospy.get_time() self._done = False self._limb_left = baxter_interface.Limb("left") self._limb_right = baxter_interface.Limb("right") # forward kinematics self._limb_kin_left = baxter_pykdl.baxter_kinematics('left') self._limb_kin_right = baxter_pykdl.baxter_kinematics('right') self._arc_length_left = 0.0 self._arc_length_right = 0.0 self._gripper_left = baxter_interface.Gripper("left", CHECK_VERSION) self._gripper_right = baxter_interface.Gripper("right", CHECK_VERSION) self._io_left_lower = baxter_interface.DigitalIO('left_lower_button') self._io_left_upper = baxter_interface.DigitalIO('left_upper_button') self._io_right_lower = baxter_interface.DigitalIO('right_lower_button') self._io_right_upper = baxter_interface.DigitalIO('right_upper_button') self._io_left_lower.state_changed.connect(self._left_open_action) self._io_left_upper.state_changed.connect(self._left_close_action) # Use navigator for start and stop of recording self._nav_left = baxter_interface.Navigator('left') self._nav_left.button0_changed.connect(self._nav_b0_pressed) self._is_started = False self._pre_pos_left = None # Verify Grippers Have No Errors and are Calibrated if self._gripper_left.error(): self._gripper_left.reset() if self._gripper_right.error(): self._gripper_right.reset() if (not self._gripper_left.calibrated() and self._gripper_left.type() != 'custom'): self._gripper_left.calibrate() if (not self._gripper_right.calibrated() and self._gripper_right.type() != 'custom'): self._gripper_right.calibrate()
def __init__(self, arm, separacion): self.arm = arm # Declaracion del boton self.boton_brazo = baxter_interface.Navigator(self.arm) # Gripper self.gripper = baxter_interface.Gripper(self.arm) # Calibracion self.gripper.calibrate() # Declaracion del brazo self.brazo = baxter_interface.Limb(self.arm) self.pos_ini = [1] * 6 self.pos_fin = [1] * 6 # Largo de la pieza self.separacion = 0.0315 # Tablero self.caja = tablero_gato(0.38, 0.38, 3, 3) self.centros = [x[:] for x in self.caja.centros] # Rotacion estandar self.rot = [math.pi, 0, -math.pi / 2] # Publicador a pantalla self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=10) self.fila_max = 5 self.fila = 0 self.contador = [False] * self.caja.espacios self.lugar = 0
def __init__(self, arm, speed=0.3, accuracy=baxter_interface.JOINT_ANGLE_TOLERANCE): self._arm = arm self._speed = speed self._accuracy = accuracy self._limb = baxter_interface.Limb(self._arm) self._path = list() self._path_recording = True self._provide_assist = True # creating a navigator io object self._navigator_io = baxter_interface.Navigator(self._arm) # checking the robot state self._rs = baxter_interface.RobotEnable() self._init_state = self._rs.state().enabled # Enabling the robot if it not already enabled if not self._init_state: self._rs.enable()
def main(): def b0_pressed(v): print ("Button 0: %s" % (v,)) def b1_pressed(v): grip_left.open(block=True, timeout=2.0) def b2_pressed(v): grip_left.close(block=True, timeout=2.0) def wheel_moved(v): print ("Wheel Increment: %d, New Value: %s" % (v, nav.wheel)) angles = left.joint_angles() if v == 1: temp = angles['left_w2'] - 0.5 elif v == -1: temp = angles['left_w2'] + 0.5 pos = {'left_w2': temp} move_list(arm='left', p_list=[pos], timeout=0.05) rospy.init_node("navigator_test") left = baxter_interface.Limb('left') left.set_joint_position_speed(1.0) grip_left = baxter_interface.Gripper('left', CHECK_VERSION) nav = baxter_interface.Navigator('left') nav.button0_changed.connect(b0_pressed) nav.button1_changed.connect(b1_pressed) nav.button2_changed.connect(b2_pressed) nav.wheel_changed.connect(wheel_moved) while not rospy.is_shutdown(): rate = rospy.Rate(1) rate.sleep()
def __init__(self, limb, speed, accuracy): # Create baxter_interface limb instance self._arm = limb self._limb = baxter_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 = baxter_interface.RobotEnable() self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() # Create Navigator I/O self._navigator_io = baxter_interface.Navigator(self._arm)
def main(): rospy.init_node("control", anonymous=True) pub = rospy.Publisher('display_chatter', String, queue_size=40) #Display chatter publisher defined. # This variables fot helping that which action will be chosed indexOfEmotion = 0 isWakeUp = False isFollow = False emotion = 0 isEnable = False ''' Navigators are declared. -> Navigators on the arms are used for changing the emotions. Okay buttons of the arm navigators are used for enable or disable robot -> Navigators on the torses are unsed for the arm following action. Also they are used for two actions. (Sleep and wake up) ''' navLeftArm = baxter_interface.Navigator('left') navRightArm = baxter_interface.Navigator('right') navLeftTorso = baxter_interface.Navigator('torso_left') navRightTorso = baxter_interface.Navigator('torso_right') print "Controller is enable..." while not rospy.is_shutdown(): # Arm navigators okay button to enable and disable if navLeftTorso._state.buttons[0] or navRightTorso._state.buttons[0]: if not isEnable: pub.publish("enable") isEnable = True print "enable" else: pub.publish("disable") isEnable = False print "disable" #Left arm up button to wake up elif navLeftArm._state.buttons[1]: pub.publish("wake_up") isWakeUp = True print "wake_up" #Left arm down button to sleep elif navLeftArm._state.buttons[2]: pub.publish("sleep") isWakeUp = False print "sleep" #Right arm buttons to follow arms elif navRightArm._state.buttons[1]: if isFollow: pub.publish("arm_follow_off") isFollow = False print "arm_follow_off" else: pub.publish("dynamic_left_arm_follow_on") isFollow = True print "dynamic_left_arm_follow_on" elif navRightArm._state.buttons[2]: if isFollow: pub.publish("arm_follow_off") isFollow = False print "arm_follow_off" else: pub.publish("dynamic_right_arm_follow_on") isFollow = True print "dynamic_right_arm_follow_on" else: # Wheel Control currentEmotion = getEmotion(navLeftArm._state.wheel) if not emotion == currentEmotion and isWakeUp: pub.publish(commands[currentEmotion]) emotion = currentEmotion print commands[currentEmotion] print currentEmotion continue print "Wait for 0.3 secs " time.sleep(0.3) print "Okay:"
def __init__(self): print "Initializing Node" rospy.init_node('baxter_peripherals') self._running = True self._valid_limb_names = { 'left': 'left', 'l': 'left', 'right': 'right', 'r': 'right' } # gripper initialization self._grippers = { 'left': baxter_interface.Gripper('left'), 'right': baxter_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 = { 'left': baxter_interface.AnalogIO('left_hand_range'), 'right': baxter_interface.AnalogIO('right_hand_range') } # accelerometer initialization self._accelerometers = {'left': [0.0] * 3, '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 = baxter_interface.Head() # sonar initialization self._sonar_pointcloud = RR.RobotRaconteurNode.s.NewStructure( 'BaxterPeripheral_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 = {'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._suppress_collision_avoidance = {'left': False, 'right': False} 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 = {'left': False, 'right': False} 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 = {'left': False, 'right': False} 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 = {'left': False, 'right': False} 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(baxter_interface.Limb('left').joint_names() + \ baxter_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 = { 'left': baxter_interface.Navigator('left'), 'right': baxter_interface.Navigator('right'), 'torso_left': baxter_interface.Navigator('torso_left'), 'torso_right': baxter_interface.Navigator('torso_right') } # initialize frame transform self._listener = tf.TransformListener()
next_move = "PROBLEM" def callbackHint(v): global next_move, command, origin, destination # if button is pressed if v == True: name_of_file = "movement" + str(origin) + "-" + str( destination) + ".txt" movementLAPub.publish(name_of_file) feedbackPub.publish(command) if __name__ == "__main__": # create node rospy.init_node('provideHintAlgorithm', anonymous=True) # create subscriber to the valid move topic movesSub = rospy.Subscriber('/hanoi/nextMove', Moves, callbackNextMove) # create subscriber to the left arm button leftArmNav = baxter_interface.Navigator('left') # check for left arm button 1 press for YES HINT leftArmNav.button1_changed.connect(callbackHint) #prevents program from exiting, allowing subscribers and publishers to keep operating #in our case that is the camera subscriber and the image processing callback function rospy.spin()
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('baxter_peripherals') self._running = True self._valid_limb_names = { 'left': 'left', 'l': 'left', 'right': 'right', 'r': 'right' } # gripper initialization self._grippers = { 'left': baxter_interface.Gripper('left'), 'right': baxter_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 = { 'left': baxter_interface.AnalogIO('left_hand_range'), 'right': baxter_interface.AnalogIO('right_hand_range') } # accelerometer initialization self._accelerometers = {'left': [0.0] * 3, '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 = baxter_interface.Head() # suppressions 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._suppress_collision_avoidance = {'left': False, 'right': False} 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 = {'left': False, 'right': False} 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 = {'left': False, 'right': False} 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 = {'left': False, 'right': False} 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(baxter_interface.Limb('left').joint_names() + \ baxter_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) # hysteresis subscription self._hyst_lock = threading.Lock() self._hysteresis_torque = OrderedDict( zip(baxter_interface.Limb('left').joint_names() + \ baxter_interface.Limb('right').joint_names(), [0.0]*14)) rospy.Subscriber("/robot/limb/left/gravity_compensation_torques", SEAJointState, self.hyst_callback) rospy.Subscriber("/robot/limb/right/gravity_compensation_torques", SEAJointState, self.hyst_callback) #commanded position subscription self._commandPos_lock = threading.Lock() self._commanded_position = OrderedDict( zip(baxter_interface.Limb('left').joint_names() + \ baxter_interface.Limb('right').joint_names(), [0.0]*14)) rospy.Subscriber("/robot/limb/left/gravity_compensation_torques", SEAJointState, self.commandPos_callback) rospy.Subscriber("/robot/limb/right/gravity_compensation_torques", SEAJointState, self.commandPos_callback) #commanded effort subscription self._commandTor_lock = threading.Lock() self._commanded_torque = OrderedDict( zip(baxter_interface.Limb('left').joint_names() + \ baxter_interface.Limb('right').joint_names(), [0.0]*14)) rospy.Subscriber("/robot/limb/left/gravity_compensation_torques", SEAJointState, self.commandTor_callback) rospy.Subscriber("/robot/limb/right/gravity_compensation_torques", SEAJointState, self.commandTor_callback) #all gravity effort subscription self._allGrav_lock = threading.Lock() self._all_gravity_torque = OrderedDict( zip(baxter_interface.Limb('left').joint_names() + \ baxter_interface.Limb('right').joint_names(), [0.0]*14)) rospy.Subscriber("/robot/limb/left/gravity_compensation_torques", SEAJointState, self.allGrav_callback) rospy.Subscriber("/robot/limb/right/gravity_compensation_torques", SEAJointState, self.allGrav_callback) #crosstalk effort subscription self._crossTor_lock = threading.Lock() self._crosstalk_torque = OrderedDict( zip(baxter_interface.Limb('left').joint_names() + \ baxter_interface.Limb('right').joint_names(), [0.0]*14)) rospy.Subscriber("/robot/limb/left/gravity_compensation_torques", SEAJointState, self.crossTor_callback) rospy.Subscriber("/robot/limb/right/gravity_compensation_torques", SEAJointState, self.crossTor_callback) #hysteresis state subscription # self._hystState_lock = threading.Lock() #self._hysteresis_state = OrderedDict( # zip('hystState',[0.0])) #rospy.Subscriber("/robot/limb/left/gravity_compensation_torques", # SEAJointState, self.hystState_callback) #rospy.Subscriber("/robot/limb/right/gravity_compensation_torques", # SEAJointState, self.hystState_callback) # navigators self._navigators = { 'left': baxter_interface.Navigator('left'), 'right': baxter_interface.Navigator('right'), 'torso_left': baxter_interface.Navigator('torso_left'), 'torso_right': baxter_interface.Navigator('torso_right') }
""" import argparse import sys import rospy from std_msgs.msg import String import baxter_interface import time rospy.init_node('secondweek') #grab the left arm Llimb = baxter_interface.Limb('left') #set left limb to 'comfortable' position to get started with Langles = {'left_w0': 2.0793109556030274, 'left_w1': -1.4522963092712404, 'left_w2': -3.0533887547973633, 'left_e0': -0.17832526638793947, 'left_e1': 2.1414371774414063, 'left_s0': 0.22472818516845705, 'left_s1': -0.683388440222168} Llimb.move_to_joint_positions(Langles) Lnav = (baxter_interface.Navigator('right')) #grab the right arm Rlimb = baxter_interface.Limb('right') Rangles = {'right_s0': -0.12923788123168947, 'right_s1': -0.34361169609375003, 'right_w0': -1.1830826813049318, 'right_w1': 1.1347622865417482, 'right_w2': 2.5966459757263185, 'right_e0': 1.1581554935302736, 'right_e1': 1.3974564961669922} Rlimb.move_to_joint_positions(Rangles) Rnav = (baxter_interface.Navigator('right')) #this should import the grippers left and right right_gripper = baxter_interface.Gripper('right') left_gripper = baxter_interface.Gripper('left') #force calibration of left gripper left_gripper.calibrate()
def __init__(self, arm): self.boton_brazo = baxter_interface.Navigator(arm) self.brazo = baxter_interface.Limb(arm)