def __init__(self, moveit=False, tolerance=0.008726646): baxter_interface.JOINT_ANGLE_TOLERANCE = tolerance self._left_limb = baxter_interface.Limb('left') self._right_limb = baxter_interface.Limb('right') self._left_gripper = baxter_interface.Gripper('left') self._left_gripper.set_holding_force(100) self._left_gripper.set_moving_force(100) self._right_gripper = baxter_interface.Gripper('right') self._right_gripper.set_holding_force(100) self._right_gripper.set_moving_force(100) left_ns = "ExternalTools/" + 'left' + "/PositionKinematicsNode/IKService" self.left_iksvc = rospy.ServiceProxy(left_ns, SolvePositionIK) rospy.wait_for_service(left_ns, 5.0) right_ns = "ExternalTools/" + 'right' + "/PositionKinematicsNode/IKService" self.right_iksvc = rospy.ServiceProxy(right_ns, SolvePositionIK) rospy.wait_for_service(right_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.server_pid_path = None if moveit: moveit_commander.roscpp_initialize(sys.argv) self.robot_commander = moveit_commander.RobotCommander() self.left_commander = moveit_commander.MoveGroupCommander( 'left_arm') self.right_commander = moveit_commander.MoveGroupCommander( 'right_arm') self.collision_proxy = rospy.ServiceProxy('/check_state_validity', GetStateValidity) self.collision_proxy.wait_for_service()
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/" + limb + "/PositionKinematicsNode/IKService" self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK) rospy.wait_for_service(ns, 2.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, limb, verbose=True): # Subscribers # Publishers # Initialize parameters self._limb_name = limb # string self._verbose = verbose # bool self._limb = baxter_interface.Limb(limb) self._gripper = baxter_interface.Gripper(limb) ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" 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 left = baxter_interface.Gripper('left', baxter_interface.CHECK_VERSION) print("Enabling robot... ") self._rs.enable() print("Calibrating gripper...") left.calibrate() # Node cycle rate (in Hz). self.loop_rate = rospy.Rate(15)
def target_pose_listener(): rospy.init_node('target_pose_listener', anonymous=True) #Subscribe to Baxter's left endpoint state and Visp autotracker messages rospy.Subscriber("/robot/limb/left/endpoint_state", EndpointState, getPoseEE) rospy.Subscriber("/visp_auto_tracker/object_position", PoseStamped, getPoseTag) rospy.Subscriber("/opencv/center_of_object", Point, getPositionObjectfromOpenCV) #Wait for left gripper's end effector pose while not first_flag: pass print "In main loop" #Move to home position move_to_home_pose = PoseStamped() move_to_home_pose.header = Header(stamp=rospy.Time.now(), frame_id='base') move_to_home_pose.pose.position = Point( x=0.8, y=0.3, z=0.3, ) move_to_home_pose.pose.orientation = Quaternion( x=0, y=1, z=0, w=0, ) BaxterMovement(move_to_home_pose) #Calibrate left gripper baxterleft = baxter_interface.Gripper('left') baxterleft.calibrate() #Run main part of loop while rospy is not shutdown while not rospy.is_shutdown(): #If no message is being published from QR code, moves towards object seen from OpenCV node while tag_msg.pose.position.x == 0: BaxterMovement(NewPoseUsingOpenCV(position_object_opencv)) #Otherwise, moves toward QR code BaxterMovement(NewPoseUsingQRcode(tag_msg)) rospy.spin()
def __init__(self,limb="right"): #create our action server clients self._right_client = actionlib.SimpleActionClient( 'robot/limb/right/follow_joint_trajectory', FollowJointTrajectoryAction, ) #verify joint trajectory action servers are available r_server_up = self._right_client.wait_for_server(rospy.Duration(10.0)) if not r_server_up: msg = ("Action server not available." " Verify action server availability.") rospy.logerr(msg) rospy.signal_shutdown(msg) sys.exit(1) #create our goal request self._limb_name = limb self._gripper = baxter_interface.Gripper(limb) self._r_goal = FollowJointTrajectoryGoal() #limb interface - current angles needed for start move self._r_arm = baxter_interface.Limb('right') #gripper interface - for gripper command playback #flag to signify the arm trajectories have begun executing self._arm_trajectory_started = False #reentrant lock to prevent same-thread lockout self._lock = threading.RLock() # Verify Grippers Have No Errors and are Calibrated #gripper goal trajectories # Timing offset to prevent gripper playback before trajectory has started self._slow_move_offset = 0.0 self._trajectory_start_offset = rospy.Duration(0.0) self._trajectory_actual_offset = rospy.Duration(0.0) #param namespace self._param_ns = '/rsdk_joint_trajectory_action_server/' #gripper control rate self._gripper_rate = 20.0 # Hz
def __init__(self,arm,posiciones): # Brazo a utilizar self.limb = arm self.limb_interface = baxter_interface.Limb(self.limb) # set speed as a ratio of maximum speed self.limb_interface.set_joint_position_speed(0.5) #self.other_limb_interface.set_joint_position_speed(0.5) # Gripper ("left" or "right") self.gripper = baxter_interface.Gripper(self.limb) # calibrate the gripper self.gripper.calibrate() # Laser self.laser = baxter_interface.AnalogIO('right_hand_range') self.dist = self.laser.state()/1000-0.105 # Parametros de camara self.cam_calibracion = 0.0025 # 0.0025 pixeles por metro a 1 metro de distancia. Factor de correccion self.cam_x_offset = 0 #0.04 # Correccion de camara por los gripper, self.cam_y_offset = -0.025 #-0.015 self.resolution = 1 self.width = 960 # 1280 640 960 self.height = 600 # 800 400 600 # Pose inicial self.x = posiciones[0][0] #0.4 self.y = posiciones[0][1] #-0.4 self.z = 0.0 # -0.1 self.roll = math.pi self.pitch = 0.0 self.yaw = 0.0 self.pose = [self.x, self.y, self.z, self.roll, self.pitch, self.yaw] self.mover_baxter('base',self.pose[:3],self.pose[3:6]) # Guardar cuadrilatero de busqueda self.x_pos=self.x+abs(self.x-posiciones[4][0]) self.x_neg=self.x-abs(self.x-posiciones[3][0]) self.y_pos=self.y+abs(self.y-posiciones[1][1]) self.y_neg=self.y-abs(self.y-posiciones[2][1]) # Posiciones contenedores self.Contenedor_1=posiciones[5] self.Contenedor_2=posiciones[6] self.Contenedor_3=posiciones[7]
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 ChangeGripper(action): if rospy.get_param('/gripper') == action: return True else: rospy.set_param('/gripper', action) gripper = bi.Gripper(rospy.get_param('/hand_mode')) rospy.sleep(1) if action == "open": gripper.open() return True elif action == "close": gripper.close() return True else: print("Please enter a valid gripper action.") return False
def __init__(self): super(MoveItPyPlanner_left, self).__init__() moveit_commander.roscpp_initialize(sys.argv) self.robot = moveit_commander.RobotCommander() self.group_names = self.robot.get_group_names() self.scene = moveit_commander.PlanningSceneInterface() self.group_name = "left_arm" self.group = moveit_commander.MoveGroupCommander(self.group_name) self.display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.left_gripper = baxter_interface.Gripper('left') self.box_name = '' self.eef_link = self.group.get_end_effector_link()
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, 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) print("Getting robot state... ") self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._robot = moveit_commander.RobotCommander() # This is an interface to one group of joints. In our case, we want to use the "right_arm". #We will use this to plan and execute motions self._group = moveit_commander.MoveGroupCommander(limb + "_arm") self._group.set_max_velocity_scaling_factor( 0.2) # This is to make Baxter move slower
def main(): global right_command_pub rospy.init_node("rsdk_ik_service_client") right_command_pub = rospy.Publisher("/robot/limb/right/joint_command", JointCommand, queue_size=10) limb = baxter_interface.Limb("right") gripper = baxter_interface.Gripper("right") #Calibrate your gripper here gripper.calibrate() point1 = Point( x=0.7, y=-0.15, z=-0.129) point2 = Point( x=0.7, y=-0.15, z=0.1) point3 = Point( x=0.7, y=-0.5, z=0.1) point4 = Point( x=0.7, y=-0.5, z=-0.129) gripper.open() rospy.sleep(.2) while(not rospy.is_shutdown()): move_joint(limb, point1) rospy.sleep(.2) gripper.close() rospy.sleep(.1) move_joint(limb, point2) rospy.sleep(.2) move_joint(limb, point3) rospy.sleep(.2) move_joint(limb, point4) rospy.sleep(.2) gripper.open() rospy.sleep(.1) move_joint(limb, point3) rospy.sleep(.2) move_joint(limb, point2) rospy.sleep(.2)
def __init__(self, limb, side): # TODO: Replace limb motion with moveit motions self.side = side # TODO: JB Added # self._limb = baxter_interface.Limb(limb) self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self._limb = moveit_commander.MoveGroupCommander(limb) self._limb.set_max_velocity_scaling_factor(0.4) circle_io = baxter_interface.DigitalIO(side + '_lower_button') dash_io = baxter_interface.DigitalIO(side + '_upper_button') self.calibrating = False self.object_calib = -1 self.objects = [ "teddy_bear", "orange", "book", "clock", "bottle", "scissors", "cup", "bowl" ] # self.objects = ['neutral', 'Cup', 'Tea', 'Sugar', 'Left_Bread', 'Right_Bread', 'Lettuce', 'Meat'] # self.objects = ['neutral', 'placemat', 'cup', 'plate', 'fork', 'spoon', 'knife', 'bowl', 'soda', 'wineglass'] # self.objects = ['neutral', 'cup', 'plate', 'bowl'] # self.objects = ['neutral', 'bowl'] # TODO: JB Added # self.object_pick_joint_angles = dict() self.object_pick_poses = dict() self.object_approach_poses = dict() # TODO: JB Added # self.object_place_joint_angles = dict() self.object_place_poses = dict() self._gripper = baxter_interface.Gripper(side) self._gripper.calibrate() self._gripper.set_holding_force(100.0) ik_srv = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" self._iksvc = rospy.ServiceProxy(ik_srv, SolvePositionIK) self._ikreq = SolvePositionIKRequest() circle_io.state_changed.connect(self.CirclePressed_) # Variables to manage threads self.work_thread = None self.stop = False # state variables self.state = STATE.IDLE
def __init__(self, limb, pick_hover = 0.15, place_hover = 0.25, verbose=True): self._limb_name = limb # string self._pick_hover = pick_hover # in meters self._place_hover = place_hover # in meters #how far to hover above brick before pick to prevent knock over self._verbose = verbose # bool 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()
def __init__(self): rospy.loginfo("Creating HandControl class") # Instatiating objects self.kb = kbhit.KBHit() rospy.on_shutdown(self.kb.set_normal_term) self.rh = baxter_interface.Gripper("right") rospy.sleep(1.0) self.run_state = ON # Always on self.gripper_state = OPEN self.gripper_state = CLOSED # Subscribers self.run_flag_sub = rospy.Subscriber('/run_status', Bool, self.run_cb) self.omni_white_sub = rospy.Subscriber('omni1_button', PhantomButtonEvent, self.omni_white_cb, queue_size=1) return
def __init__(self, gripper, scale_pos, reconfig_server): self._scale_pos = scale_pos self._dyn = reconfig_server self._ee = gripper + '_gripper' self._ns = 'robot/end_effector/' + self._ee + '/gripper_action' self._gripper = baxter_interface.Gripper(gripper, CHECK_VERSION) # Store Gripper Type self._type = self._gripper.type() if self._type == 'custom': msg = ("Stopping %s action server - %s gripper not capable of " "gripper actions" % (self._gripper.name, self._type)) rospy.logerr(msg) return # Verify Grippers Have No Errors and are Calibrated if self._gripper.error(): self._gripper.reset() if self._gripper.error(): msg = ("Stopping %s action server - Unable to clear error" % self._gripper.name) rospy.logerr(msg) return if not self._gripper.calibrated(): self._gripper.calibrate() if not self._gripper.calibrated(): msg = ("Stopping %s action server - Unable to calibrate" % self._gripper.name) rospy.logerr(msg) return # Action Server self._server = actionlib.SimpleActionServer( self._ns, GripperCommandAction, execute_cb=self._on_gripper_action, auto_start=False) self._action_name = rospy.get_name() self._server.start() # Action Feedback/Result self._fdbk = GripperCommandFeedback() self._result = GripperCommandResult() # Initialize Parameters self._prm = self._gripper.parameters() self._timeout = 5.0
def __init__(self, limb, speed = 1.5, hover_distance = 0.15, verbose=True): # fix the damned speed self._limb_name = limb # string self._hover_distance = hover_distance # in meters above self._verbose = verbose # bool self._limb = baxter_interface.Limb(limb) # selects limb self._limb.set_joint_position_speed(speed) # sets overall speed self._gripper = baxter_interface.Gripper(limb) # sets gripper left/right self._gripper.set_moving_force(100) # sets max gripper moving force 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, limb, name, ID): # Initialize Node rospy.init_node('grabber', anonymous=True) # Subscribe to sensor data rospy.Subscriber("/block_location", BlockArray, self.model_callback) # Contains a list of block locations for blocks visible by Baxter self._name = name self._ID = ID self._block_locations = [] self._check_for_blocks = False # Select limb (left or right) self._limb = baxter_interface.Limb(limb) # Set movement speed self._limb.set_joint_position_speed(0.05) # Initialize Gripper self._gripper = baxter_interface.Gripper(limb) # Initialize inverse kinematics service self._ik = rospy.ServiceProxy('/Baxter_IK_left/', BaxterIK) 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() # An orientation for gripper fingers to be overhead and parallel to an object - use these values for orientation when you query the IK service self._overhead_orientation = Quaternion( x=-0.0249590815779, y=0.999649402929, z=0.00737916180073, w=0.00486450832011) # Start position - use these angles for the robot's start position self._start_position = {'left_w0': 0.6699952259595108, 'left_w1': 1.030009435085784, 'left_w2': -0.4999997247485215, 'left_e0': -1.189968899785275, 'left_e1': 1.9400238130755056, 'left_s0': -0.08000397926829805, 'left_s1': -0.9999781166910306} # Position over the trash can - use these angles for the robot's position over the trash can self._trashcan_position = {'left_w0': 0.3699952259595108, 'left_w1': 1.030009435085784, 'left_w2': -0.999997247485215, 'left_e0': -1.189968899785275, 'left_e1': 1.9400238130755056, 'left_s0': 1.78000397926829805, 'left_s1': -0.9999781166910306}
def __init__(self, limb='right', hover_distance = 0.15, verbose=True, sequence=False): self._limb_name = limb self._hover_distance = hover_distance self._verbose = verbose # print debug messages? self._sequence = sequence # will run full demo once enabled self._limb = baxter_interface.Limb(limb) self._gripper = baxter_interface.Gripper(limb) self._gripper.set_moving_force(90) # use 90% of gripper force when moving self._gripper.set_holding_force(90) # use 90% of gripper force when holding self._iteration = 0 # which brick is being manipulated: begin at zero self._start_angles = { 'right_s0': 0.5448384680435581, 'right_s1': -1.3477369935496055, 'right_w0': 0.05326913893685781, 'right_w1': 1.5810761462006662, 'right_w2': -0.5013009260859533, 'right_e0': -0.2556930704776139, 'right_e1': 1.3379418806340722 } # Hardcoded pass angles for horizontal brick placement self._h_pass_angles = { 'right_s0': -0.46232466308834486, 'right_s1': -0.17581907048286283, 'right_w0': 0.05859510625342512, 'right_w1': 0.7853226620254743, 'right_w2': 1.8815493071424605, 'right_e0': 1.884863261937074, 'right_e1': 2.0108387683180178 } # Hardcoded pass angles for vertical brick placement self._v_pass_angles = { 'right_s0': 0.17441293379622724, 'right_s1': -0.9520368012619453, 'right_w0': -0.623071260093031, 'right_w1': 1.825515060165495, 'right_w2': 2.6252551125456414, 'right_e0': 1.7985084287013606, 'right_e1': 1.421872443080793 } self._hover_angles = None self._cpose = Pose() # calibration pose: empty until calibration function run self._cpose_angles = {} 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, tab): self.MAX_PLY = 32 self.pv_length = [0 for x in range(self.MAX_PLY)] self.INFINITY = 32000 self.init() self.tab = tab self.gripper = baxter_interface.Gripper("right") self.gripper.calibrate() self.gripper.open() self.limb = baxter_interface.Limb('right') self.limb2 = baxter_interface.Limb('left') self.piecePrise = Piece() self.castle_pos1 = Piece() self.castle_pos2 = Piece() self.en_passant = False self.boolc = False
def toggle_grip(self): gripper = baxter_interface.Gripper(self.limb_side) if self.grip_state == 0: print("Opening gripper") self.grip_state = 100 gripper.command_position(self.grip_state) elif self.grip_state == 100: print("Closing gripper") self.grip_state = 0 gripper.command_position(self.grip_state) time.sleep(0.5) position = gripper.position() print(' ' + str(position)) if position > 5: print(" Picked up object") else: print(" Didn't pick up object")
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, sim=False): """Hardware abstraction of the Baxter robot using the BaxterSDK interface. :param sim: Whether in Gazebo (True) or on real Baxter (False). """ name = 'main.baxter' self._logger = logging.getLogger(name) self._arms = ['left', 'right'] self._limbs = {a: baxter_interface.Limb(a) for a in self._arms} self._grippers = {a: baxter_interface.Gripper(a) for a in self._arms} self._grippers_pars = self._grippers['left'].valid_parameters() self._grippers_pars['moving_force'] = 40.0 self._grippers_pars['holding_force'] = 30.0 self._sensors = {a: baxter_interface.analog_io.AnalogIO('%s_hand_range' % a) for a in self._arms} # Cameras on the Baxter robot are tricky. Due to limited bandwidth # only two cameras can be operating at a time. # http://sdk.rethinkrobotics.com/wiki/Camera_Control_Tool # Default behavior on Baxter startup is for both of the hand cameras # to be in operation at a resolution of 320x200 at a frame rate of # 25 fps. We get their CameraControllers using the Baxter SDK ... self.cameras_d = {a: baxter_interface.CameraController('%s_hand_camera' % a, sim=sim) for a in self._arms} # ... and set their resolution to 1280x800 @ 14 fps. for arm in self._arms: self.cameras_d[arm].resolution = (1280, 800) self.cameras_d[arm].fps = 14.0 self.cameras_d[arm].exposure = settings.baxter_cam_exposure # We don't need the CameraControllers any more. Our own module will # do the remaining camera handling for us. self.cameras = {a: Camera(topic='/cameras/{}_hand_camera/image'.format(a), prefix=name) for a in self._arms} self._planner = SimplePlanner() self._rs = None self._init_state = None self.cam_offset = None self.range_offset = None self.z_table = None
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, 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, 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 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, 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 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 __init__(self): left = baxter_interface.Gripper('left') left.reboot() self.left_arm = baxter_interface.Limb('left') self.pub_rate = rospy.Publisher('/robot/joint_state_publish_rate', UInt16) self.pub_rate.publish(500) """ The following 6 dictionaries are positions for Baxter's left arm built such that each will put Baxter's arm over (for overposition dictionaries) one of the three bowls, or into (for inposition dictionaries) one of the bowls. """ self.overposition1 = dict( zip()) #We need to write all of these dictionaries. self.overposition2 = dict(zip()) self.overposition3 = dict(zip()) self.inposition1 = dict(zip()) self.inposition2 = dict(zip()) self.inposition3 = dict(zip())