def __init__(self): rospy.init_node("baxter_test") self._rs = baxter_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled self._rs.enable() self._rate = 5.0 #Hz #IK self.ns = "ExternalTools/left/PositionKinematicsNode/IKService" self.hdr = Header(stamp = rospy.Time(0), frame_id = "base") self.iksvc = rospy.ServiceProxy(self.ns, SolvePositionIK) self.ikreq = SolvePositionIKRequest() self._left_arm = baxter_interface.limb.Limb("left") self._right_arm = baxter_interface.limb.Limb("right") self._left_joint_names = self._left_arm.joint_names() self._right_joint_names = self._right_arm.joint_names() self._left_angles = [self._left_arm.joint_angle(joint) for joint in self._left_joint_names] self._right_angles = [self._right_arm.joint_angle(joint) for joint in self._right_joint_names]
def __init__(self, limb, verbose=True): self._limb_name = limb # string self._verbose = verbose # bool self._limb = baxter_interface.Limb(limb) self._gripper = baxter_interface.Gripper(limb) self._limb.set_joint_position_speed(0.1) self._gripper.calibrate() ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK) rospy.wait_for_service(ns, 5.0) # verify robot is enabled print("Getting robot state... ") self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable()
def __init__(self): self._limb = baxter_interface.limb.Limb("right") self._angles = self._limb.joint_angles() # open left gripper #self.leftGripper =baxter_interface.Gripper('left') #self.leftGripper.calibrate() #self.leftGripper.open() # open Right gripper self.rightGripper = baxter_interface.Gripper('right') self.rightGripper.calibrate() self.rightGripper.open() # set control params print("Getting robot state...") self._rs = baxter_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot..") self._rs.enable()
def __init__(self, limb, hover_distance=0.15, verbose=True): self._limb_name = limb # string self._hover_distance = hover_distance # in meters self._verbose = verbose # bool self._limb = baxter_interface.Limb(limb) self.joint_space = len(self._limb.joint_angles()) self._gripper = baxter_interface.Gripper(limb) ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK) rospy.wait_for_service(ns, 5.0) # verify robot is enabled print("Getting robot state... ") self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() self.ctr = 0 self.canSubscriber = True
def __init__(self): # initialize interfaces rs = baxter_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled self.leftGripper = baxter_interface.Gripper('left', CHECK_VERSION) self.rightGripper = baxter_interface.Gripper('right', CHECK_VERSION) # MANUAL JOINT CONTROL self.leftLimb = baxter_interface.Limb('left') self.rightLimb = baxter_interface.Limb('right') self.manualJointAccuracy = baxter_interface.settings.JOINT_ANGLE_TOLERANCE self.manualJointTimeout = 20.0 # JOINT SPEEDS self.manualJointSpeed = 0.3 self.leftLimb.set_joint_position_speed(self.manualJointSpeed) self.rightLimb.set_joint_position_speed(self.manualJointSpeed)
def __init__(self): """ 'Waves' right arm by driving the joint velocities to sinusoid functions """ self._pub_rate = rospy.Publisher('/robot/joint_state_publish_rate', UInt16) self._left_arm = baxter_interface.limb.Limb("left") self._right_arm = baxter_interface.limb.Limb("right") self._right_joint_names = self._right_arm.joint_names() self._left_joint_names = self._left_arm.joint_names() self._head = baxter_interface.Head() # set joint state publishing to 500Hz self._rs = baxter_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled self._rs.enable() self._pub_rate.publish(500)
def __init__(self, limb, hover_distance=0.15, verbose=True): self._limb_name = limb # string self._hover_distance = hover_distance # in meters self._verbose = verbose # bool self._limb = baxter_interface.Limb(limb) self._gripper = baxter_interface.Gripper(limb) ns = "ExternalTools/" + self._limb_name + "/PositionKinematicsNode/IKService" self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK) rospy.wait_for_service(ns, 5.0) # verify robot is enabled print("Getting robot state... ") self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() self.block_detected = False self.block_poses = [] self.pose = Pose()
def __init__(self): self._speak_ctr = PollySpeech() self._mbot_relay = MultiBotInterface() self._face_ctr = FaceController() #enable robot enable_robot = baxter_interface.RobotEnable() enable_robot.enable() rospack = rospkg.RosPack() self._package_path = rospack.get_path( 'hri19_multirobot_transition_study') #to deal with SST rospy.Subscriber('stt', Speech, self._stt_callback, queue_size=1) self._stt_toggle = rospy.ServiceProxy('toggle_stt', SetBool) self._last_sentence = "" self._stt_condition = threading.Condition() self._stt_event = threading.Event() self._pause_flag = True self._nlu_engine = SnipsNLU() self._head = baxter_interface.Head() self._running = True #memory for each session self._temp_memory = dict() self._memory_stack = list() self._index_stack = list() self._index = 0 """ The following are the list of actions/behaviors that exist for the robot, each action's name directly correspond to the same name of the function """ self._operation_list = [ 'speak', 'move_to_posture', 'move_head', 'change_face', 'natural_language_understanding', 'wait_response', 'wait', 'yes_no_question', 'signal_robot', 'wait_for_signal', 'run_file', 'choice_question', 'load_param' ] rospy.loginfo("RUN Complete Initiailization")
def main(): print("Initializing node... ") rospy.init_node("set_joint_positions") #################Resposible for setting joint positions for Baxter ############################################ print("Getting robot state... ") rs = baxter_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled def clean_shutdown(): print("\nExiting example...") if not init_state: print("Disabling robot...") rs.disable() print("Enabling robot... ") rs.enable() #right_e1 is special. joint limit [-0.05] is the reason. keeping it zero for now #left_e1 is special. joint limit [-0.05] is the reason. keeping it zero for now # joint_name_left=["left_s0", "left_s1", "left_e0", "left_e1", "left_w0", "left_w1", "left_w2"] # joint_name_right=["right_e0", "right_e1", "right_s0", "right_s1", "right_w0", "right_w1", "right_w2"] ################################################################################################################### # joint_name_left=["left_e0", "left_e1", "left_s0", "left_s1", "left_w0", "left_w1", "left_w2"] # joint_desired_state_right=[0.7410703227630888, 1.5362719506658014, 0.01001108907777759, -0.7006024655803085, 0.9735489859081357, 1.447864944629445, -0.4520477771263609] # joint_desired_state_left=[-0.02477285952504804, 1.5454976873355646, -0.34985937977411385, -0.862626895167157, -1.3115418478918244, 1.8487306917056667, -0.892602931497076] ################################################################################################################### joint_name_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2'] joint_desired_state_left= [0.041363752507206364, 1.9348352917089535, -0.2726798000286923, -0.5658538514940457, -1.1000522708348006, 1.7430249026235884, -0.1936378991920895] joint_name_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] joint_desired_state_right = [0.6068619915741271, 1.911764079997937, -0.13185736132604742, -0.4816721820621366, 0.790763174233601, 1.304727973182544, -1.0247054998116605] joint_name_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2'] joint_desired_state_left= [0.4345000581685434, 1.4078108680818384, -1.117505003974524, -0.8383205005793786, -0.35128160042575973, 1.0530778108833365, 0.20401944478876002] joint_name_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] joint_desired_state_right = [-0.33824276372873374, 1.5082866096883332, 1.035053536625683, -0.9276748814737039, 0.25080585881926515, 1.0895098545956152, -0.07209709703061444] setJointPositions(joint_name_right, joint_name_left, joint_desired_state_right, joint_desired_state_left) ###################################################################################################################### #################Resposible for getting joint positions for Baxter from /robot/joint_states topic ############################################ joint_states_current_right, joint_states_current_left=getCurrentJointState(joint_name_right, joint_name_left) printSetPositionResult(joint_desired_state_right, joint_states_current_right, joint_desired_state_left, joint_states_current_left,joint_name_right, joint_name_left)
def __init__(self, limb, reconfig_server): self._dyn = reconfig_server # control parameters self._rate = 1000.0 # Hz self._missed_cmds = 20.0 # Missed cycles before triggering timeout self.joint_names = ['left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2'] # define start and end time to calcualte velocity self._end = time() self._start = time() # create our limb instance self._limb = baxter_interface.Limb(limb) self._start_angles = dict() # self._dof_names = {'x': 1, 'y': 2, 'z': 3} self._dof_names = ['x', 'y', 'z'] # initialize parameters self._springs = dict() self._damping = dict() self._start_pos = [0, 0, 0] self._last_pos = self._start_pos self._kin = baxter_kinematics('left') # gravity compensation self._gravity_comp_effort = dict() self._gravity_comp_effort_ok = False gravity_comp_topic = '/robot/limb/left/gravity_compensation_torques' self._gravity_comp_sub = rospy.Subscriber( gravity_comp_topic, SEAJointState, self._on_gravity_comp, queue_size=1, tcp_nodelay=True) # create cuff disable publisher cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction' self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1) # verify robot is enabled print("Getting robot state... ") self._rs = baxter_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() print("Running. Ctrl-c to quit") self.safe_file = open('mani.csv', 'w')
def __init__(self, arm, distance): global image_directory baxter_interface.RobotEnable().enable() # arm ("left" or "right") self.limb = arm self.distance = distance self.limb_interface = baxter_interface.Limb(self.limb) self.gripper = baxter_interface.Gripper(arm) self.gripper.calibrate() self.other_limb_interface = baxter_interface.Limb("left") self.baxter_ik_move("left", (0.25, 0.50, 0.2, math.pi, 0.0, 0.0)) self.limb_interface.set_joint_position_speed(0.1) self.other_limb_interface.set_joint_position_speed(0.1) #self.open_camera(arm) self.bridge = CvBridge() # image directory self.image_dir = image_directory # flag to control saving of analysis images self.save_images = True # required position accuracy in metres # self.ball_tolerance = 0.005 # self.tray_tolerance = 0.05 # number of balls found # self.balls_found = 0 # start positions self.origin_x = 0.6 # x = front back 0.50 0.70(good) self.origin_y = -0.1 # y = left right 0.30 -0.1(good) self.origin_z = 0 # z = up down 0.15 self.roll = -1.0 * math.pi # roll = horizontal self.pitch = 0.0 * math.pi # pitch = vertical self.yaw = 0.0 * math.pi # yaw = rotation self.pose = [self.origin_x, self.origin_y, self.origin_z, \ self.roll, self.pitch, self.yaw]
def __init__(self): # save the output messages into .txt file in order to draw plot #self.saveFile = open('record.txt', 'w') #self.saveFile1 = open('record1.txt', 'w') self.limb_interface = baxter_interface.Limb("left") # start positions self.x = 0.62 # x = front back self.y = 0.14 # y = left right self.z = 0.14 # z = up down self.roll = -1.0 * math.pi # roll = horizontal self.pitch = 0.0 * math.pi # pitch = vertical self.yaw = 0.0 * math.pi # yaw = rotation self.pose = [self.x, self.y, self.z, self.roll, self.pitch, self.yaw] print(self.pose) # Enable the actuators baxter_interface.RobotEnable().enable() # set speed as a ratio of maximum speed self.limb_interface.set_joint_position_speed(0.5) # height of table self.table = -0.077 # camera parameter # This is a very interesting argument. Actually this is a simplification of the camera projection transformation. # This parameter works only when the camera is vertical to the detection surface! self.cam_calib = 0.0025 # meters per pixel at 1 meter. self.width = 960 # Camera resolution self.height = 600 # bridge to use opencv to process images self.bridge = CvBridge() # subscribe image message from webcam self.image_sub = rospy.Subscriber("/cameras/left_hand_camera/image", Image, self.callback) # setup topic for images with detection results self.image_pub = rospy.Publisher('/image_bridge', Image, latch=True, queue_size=10) self.XL_coo = rospy.Publisher('/Xcoordinate_left', Float32, latch=True, queue_size=10) self.YL_coo = rospy.Publisher('/Ycoordinate_left', Float32, latch=True, queue_size=10)
def __init__(self, shape, desired_position, limb, hover_distance=0.15, verbose=False): self._limb_name = limb # string self._shape_name = shape # string self._desired_position = desired_position # list[x,y] (integer) self._hover_distance = hover_distance # in meters self._verbose = verbose # bool self._limb = baxter_interface.limb.Limb(limb) self._limb_joint_names = self._limb.joint_names() self.prev_time = 0.0 self.control_effort_x = 0.0 self.control_effort_y = 0.0 self.object_position = Object() self.hover_position = Pose() self.diff_old = 0.0 self.zposi = 0.0 self.step = 0 self._rate = 10 #10Hz self.display_pub = rospy.Publisher('/robot/xdisplay', Image, queue_size=60) self.pub_grasp_now = rospy.Publisher("pump_on", Empty, queue_size=1) self.pub_release_now = rospy.Publisher("pump_off", Empty, queue_size=1) self.sub = rospy.Subscriber('/detected_image', Image, self.republish, None, 1) #msg_grasp_now = Bool(False) #self.pub_grasp_now.publish(msg_grasp_now) ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK) rospy.wait_for_service(ns, 5.0) # verify robot is enabled print("Getting robot state... ") self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() print("Moving to neutral pose...") self._limb.move_to_neutral()
def main(): # arg_fmt = argparse.RawDescriptionHelpFormatter # parser = argparse.ArgumentParser(formatter_class=arg_fmt, # description=main.__doc__) # parser.add_argument( # '-l', '--limb', choices=['left', 'right'], required=True, # help="the limb to test" # ) # args = parser.parse_args(rospy.myargv()[1:]) print("Getting robot state... ") rospy.init_node("IKSolver") rs = baxter_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled limb = baxter_interface.Limb("right") right = baxter_interface.Gripper('right', CHECK_VERSION) # return ik_test(args.limb) goToOrigin(limb) current_pose = limb.endpoint_pose() print current_pose print limb.joint_angles() running = True while (running): x = raw_input("X Value?\n") y = raw_input("Y Value?\n") z = raw_input("Z Value?\n") joints = solveIK('right', float(x), float(y), float(z), current_pose['orientation'].x, current_pose['orientation'].y, current_pose['orientation'].z, current_pose['orientation'].w) if (x == " " or y == " " or z == " "): running = False else: if (joints): print "moving" limb.move_to_joint_positions(joints)
def __init__(self, verbose=False): if not ProxyTrajectoryClient._is_initialized: ProxyTrajectoryClient._is_initialized = True Logger.loginfo( "Initializing proxy FollowJointTrajectory client...") ProxyTrajectoryClient._client = ProxyActionClient({ ProxyTrajectoryClient._action_topic: FollowJointTrajectoryAction }) ProxyTrajectoryClient._verbose = verbose # enable the IK Service ik_ns = "ExternalTools/" + ProxyTrajectoryClient._limb + "/PositionKinematicsNode/IKService" ProxyTrajectoryClient._iksvc = rospy.ServiceProxy( ik_ns, SolvePositionIK) rospy.wait_for_service(ik_ns, 5.0) # enable the Baxter ProxyTrajectoryClient._rs = baxter_interface.RobotEnable( baxter_interface.CHECK_VERSION) ProxyTrajectoryClient._init_state = ProxyTrajectoryClient._rs.state( ).enabled print("Enabling robot... ") ProxyTrajectoryClient._rs.enable() # enable baxter limb interface ProxyTrajectoryClient._baxter_limb_interface = baxter_interface.limb.Limb( ProxyTrajectoryClient._limb) # Get the names from joints ProxyTrajectoryClient._joint_name = [ joint for joint in ProxyTrajectoryClient._baxter_limb_interface.joint_names() ] ProxyTrajectoryClient._gripper = baxter_interface.Gripper( ProxyTrajectoryClient._limb) self._goal = FollowJointTrajectoryGoal() self._result = None self._goal_time_tolerance = rospy.Time(0.1) self._goal.goal_time_tolerance = self._goal_time_tolerance
def __init__(self, arm): self.limb = arm self.limb_interface = baxter_interface.Limb(arm) self.gripper = baxter_interface.Gripper(arm) moveit_commander.roscpp_initialize(sys.argv) self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.tolerance = 0.005 self.pos_x = 0.50 self.pos_y = 0.00 self.pos_z = 0.05 self.or_x = 0.0 self.or_y = -1.0 self.or_z = 0.0 self.or_w = 0.0 self.pose = [self.pos_x, self.pos_y, self.pos_z, self.or_x, self.or_y, self.or_z, self.or_w] self.hough_accumulator = 30 self.hough_min_radius = 35 self.hough_max_radius = 70 self.cam_calib = 0.0004 self.cam_x_offset = 0.045 self.cam_y_offset = -0.01 self.width = 960 self.height = 600 self.cv_image = cv.CreateImage((self.width, self.height), 8, 3) baxter_interface.RobotEnable().enable() self.limb_interface.set_joint_position_speed(0.5) self.gripper.calibrate() self.reset_camera(self.limb, self.width, self.height) self.subscribe_to_camera(self.limb) self.distance = 0
def __init__(self, limb, verbose=True): # Initialize parameters self._limb_name = limb self._verbose = False self._limb = baxter_interface.Limb(limb) self._gripper = baxter_interface.Gripper(limb) self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION) self._init_state = self._rs.state().enabled left = baxter_interface.Gripper('left', baxter_interface.CHECK_VERSION) if self._verbose: print("Getting robot state... ") print("Enabling robot... ") self._rs.enable() if self._verbose: print("Calibrating gripper...") left.calibrate() # Node cycle rate (in Hz). self.loop_rate = rospy.Rate(100)
def main(limb): print("Initializing node... ") rospy.init_node("rethink_rsdk_joint_trajectory_controller_test") print("Getting robot state... ") rs = baxter_interface.RobotEnable() print("Enabling robot... ") rs.enable() print("Running. Ctrl-c to quit") positions = { 'left': [-0.11, -0.62, -1.15, 1.32, 0.80, 1.27, 2.39], 'right': [0.11, -0.62, 1.15, 1.32, -0.80, 1.27, -2.39], } p1 = positions[limb] traj = Trajectory(limb) traj.add_point(p1, 7.0) traj.add_point([x * 0.75 for x in p1], 9.0) traj.add_point([x * 1.25 for x in p1], 12.0) traj.start() traj.wait()
def __init__(self): print(' initializing Tf2AnglesController node ') rospy.init_node('Tf2AnglesController', anonymous=True) # Enable the Robot self.__robot = baxter_interface.RobotEnable(CHECK_VERSION) # Robot Limbs self.__left_limb = baxter_interface.limb.Limb('left') self.__right_limb = baxter_interface.limb.Limb('right') self.__rate = rospy.Rate(10) self.__command_joint_angles = dict() self.__enable_robot_and_move_to_neutral_pose() self.__setup_subscribers() self.__setup_command_joint_angles()
def main(): rospy.init_node("rsdk_test") rs = baxter_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled def clean_shutdown(): if not init_state: rs.disable() rospy.on_shutdown(clean_shutdown) rs.enable() print('1. velocity \n 2. path') c = raw_input() if c == 1: run_velocity_loop() else if c == 2: run_path_loop()
def __init__(self): """ 'Wobbles' both arms by commanding joint velocities sinusoidally. """ self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate', UInt16) self._left_arm = baxter_interface.limb.Limb("left") self._right_arm = baxter_interface.limb.Limb("right") self._left_joint_names = self._left_arm.joint_names() self._right_joint_names = self._right_arm.joint_names() print("Getting robot state... ") self._rs = baxter_interface.RobotEnable() self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() # set joint state publishing to 100Hz self._pub_rate.publish(100)
def main(): rospy.init_node("pick_and_place_service") parser = argparse.ArgumentParser( description='Process Pick and Place Command Line Arguments') parser.add_argument('--save', '-s', type=str) parser.add_argument('--read', '-r', type=str) args = parser.parse_args() rs = baxter_interface.RobotEnable() print("Enabling Robot") rs.enable() pp = PickPlace('right') # Calibrate Pickup Locations if args.read: print "Reading Position File: " + args.read pp.ReadCalibration(args.read) else: pp.CalibrateObjects() # Post Params pp.PostParameters() # Save Calibration to File if args.save: print "Saving Calibration File: " + args.save pp.SaveCalibration(args.save) # Advertise Service s = rospy.Service('pick_and_place_object', pick_and_place, pp.PickAndPlaceObject) s_2 = rospy.Service('pick_and_place_check', pick_and_place, pp.PickAndPlaceCheck) s_3 = rospy.Service('pick_and_place_state', pick_and_place_state, pp.PickAndPlaceState) s_4 = rospy.Service('pick_and_place_stop', pick_and_place_stop, pp.PickAndPlaceStop) print "READY to PICK and Place" rospy.spin()
def main(): global data_s, mutex t = threading.Thread(target=receive) t.setDaemon(True) t.start() print("Initializing node... ") rospy.init_node('moveit_demo') rospy.sleep(1) rs = baxter_interface.RobotEnable(CHECK_VERSION) limb = baxter_interface.Limb("right") # limb.move_to_neutral() moveit = MoveItDemo() print "Waiting to receive messages..." while True: if data_s in [ "fight", "fight0", "handshake", "fetch", "hello", "salute" ]: moveit.execute(data_s) #execute action print "reset..." #reset pose rs.reset() print "move to natual..." limb.move_to_neutral() if mutex.acquire(): #clear command data_s = "" mutex.release() # if data_s == "exit": # moveit_commander.roscpp_shutdown() # moveit_commander.os._exit(0) # break UDPSock.close() os._exit(0)
def image_callback(ros_img): pattern = cv2.imread("/home/crl7/ros_ws/marker.jpg") rs = baxter_interface.RobotEnable(CHECK_VERSION) #get robot state init_state = rs.state().enabled #enable robot rs.enable() #left arm initial state left = baxter_interface.Limb('left') initial_left = left.endpoint_pose()['position'][:] + left.endpoint_pose()['orientation'][:] Pose_state_left = [x for x in initial_left] limb = 'left' ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
def listener(): rospy.init_node('end_effector_trajectory_client', anonymous=True) fine = False rate = rospy.Rate(1) while not fine | rospy.is_shutdown(): try: rs = baxter_interface.RobotEnable() rs.enable() fine = True except (OSError): rospy.logwarn("Can not enable robot.Will keep trying...") rate.sleep() if rospy.is_shutdown(): return rospy.Subscriber("end_effector_command_solution", JointCommand, callback) # spin() simply keeps python from exiting until this node is stopped rospy.spin()
def __init__(self, limb, hover_distance=0.15, verbose=True): self._limb_name = limb # string self._hover_distance = hover_distance # in meters self._verbose = verbose # bool self.cap_status = '1.0' #string self.status = 'working' #string self._limb = baxter_interface.Limb(limb) self._gripper = baxter_interface.Gripper(limb) ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK) rospy.wait_for_service(ns, 5.0) # verify robot is enabled print("Getting robot state... ") self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() self.cap_sub = rospy.Subscriber('capflag', Float64, self.callback) #initialize listener
def __init__(self, limb, amplification=1.0): """ Puppets one arm with the other. @param limb: the control arm used to puppet the other @param amplification: factor by which to amplify the arm movement """ puppet_arm = {"left": "right", "right": "left"} self._control_limb = limb self._puppet_limb = puppet_arm[limb] self._control_arm = baxter_interface.limb.Limb(self._control_limb) self._puppet_arm = baxter_interface.limb.Limb(self._puppet_limb) self._amp = amplification print("Getting robot state... ") self._rs = baxter_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable()
def main(): # arg_fmt = argparse.RawDescriptionHelpFormatter # parser = argparse.ArgumentParser(formatter_class=arg_fmt, # description=main.__doc__) # parser.add_argument( # '-l', '--limb', choices=['left', 'right'], required=True, # help="the limb to test" # ) # args = parser.parse_args(rospy.myargv()[1:]) print("Getting robot state... ") rospy.init_node("IKSolver") rs = baxter_interface.RobotEnable(CHECK_VERSION) init_state = rs.state().enabled limb = baxter_interface.Limb("right") right = baxter_interface.Gripper('right', CHECK_VERSION) # return ik_test(args.limb) goToOrigin(limb) current_pose = limb.endpoint_pose() print current_pose print limb.joint_angles() x = input("X Value?\n") y = input("Y Value?\n") z = input("Z Value?\n") # q1 = input("Q1 Value?\n") # q2 = input("Q2 Value?\n") # q3 = input("Q3 Value?\n") # q4 = input("Q4 Value?\n") # limb.move_to_neutral() # joints = solveIK('right', float(x), float(y), float(z), float(q1), float(q2), float(q3), float(q4)); joints = solveIK('right', x, y, z, current_pose['orientation'].x, current_pose['orientation'].y, current_pose['orientation'].z, current_pose['orientation'].w) if (joints): limb.move_to_joint_positions(joints)
def main(): parser = argparse.ArgumentParser() required = parser.add_argument_group('required arguments') required.add_argument('-l', '--limb', required=True, choices=['left', 'right'], help='Tare the specified limb') args = parser.parse_args(rospy.myargv()[1:]) limb = args.limb print("Initializing node...") rospy.init_node('rsdk_tare_%s' % (limb, )) print("Preparing to tare...") gripper_warn = ("\nIMPORTANT: Make sure to remove grippers and other" " attachments before running tare.\n") print(gripper_warn) if not gripper_removed(args.limb): return 1 rs = baxter_interface.RobotEnable(CHECK_VERSION) rs.enable() tt = Tare(limb) rospy.loginfo("Running tare on %s limb" % (limb, )) error = None try: tt.run() except Exception as e: error = e.strerror finally: try: rs.disable() except Exception: pass if error == None: rospy.loginfo("Tare finished") else: rospy.logerr("Tare failed: %s" % (error, )) return 0 if error == None else 1
def __init__(self): self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate', UInt16, queue_size=10) self._left_arm = baxter_interface.limb.Limb("left") self._right_arm = baxter_interface.limb.Limb("right") self._left_joint_names = self._left_arm.joint_names() self._right_joint_names = self._right_arm.joint_names() # control parameters self._rate = 500.0 # Hz print("Getting robot state... ") self._rs = baxter_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() # set joint state publishing to 500Hz self._pub_rate.publish(self._rate)