def __init__(self, limb_name, trajectory, offset, radius): # crate a limb instance control the baxter arm self.limb = Limb(limb_name) # numpy joint angle trajectory of nx7 shape self.trajectory = trajectory # index variable to keep track of current row in numpy array self.trajectory_index = 0 # store the joint names since # it will be used later while commanding the limb self.joint_names = self.limb.joint_names() # define a service called 'move_arm_to_waypoint' self.move_arm_to_waypoint_service = rospy.Service( 'move_arm_to_waypoint', Trigger, self.handle_move_arm_to_waypoint) # define another service called 'get_ee_pose' self.get_ee_position_service = rospy.Service( 'get_ee_position', GetEEPosition, self.handle_get_ee_position_service) # flag to set when trajectory is finished self.trajectory_finished = False # define 4x4 transformation for marker wrt end-effector # considering no rotation in 'marker_wrt_ee' transformation matrix self.marker_wrt_ee = np.eye(4) self.marker_wrt_ee[2, -1] = offset + radius # in z direction only
def __init__(self, usegripper = True): rospy.init_node("Baxter") threading.Thread(target=self.__jointtraj_server).start() self.trajrgt = Trajectory("right") self.trajlft = Trajectory("left") if usegripper: self.__right_hand = Gripper("right") self.__left_hand = Gripper("left") self.__right_hand.calibrate() self.__left_hand.calibrate() self.__right_limb = Limb("right") self.__left_limb = Limb("left") self.baxter = RobotEnable() self.__enabled = self.baxter.state().enabled self.enable_baxter() self.img = None for camname in ['head_camera', 'left_hand_camera', 'right_hand_camera']: try: cam = CameraController(camname) # cam.resolution = (1280, 800) cam.resolution = (320, 200) except: pass print("baxter opened")
def __init__(self): self.centerx = 365 self.centery = 120 self.coefx = 0.1/(526-369) self.coefy = 0.1/(237-90) self.count = 0 self.hdr = Header(stamp=rospy.Time.now(), frame_id='base') self.gripper = Gripper("right") self.gripper.calibrate() self.gripper.set_moving_force(0.1) rospy.sleep(0.5) self.gripper.set_holding_force(0.1) rospy.sleep(0.5) self.busy = False self.gripper_distance = 100 self.subscribe_gripper() self.robotx = -1 self.roboty = -1 self.framenumber = 0 self.history = np.arange(0,20)*-1 self.newPosition = True self.bowlcamera = None self.kin = baxter_kinematics('right') self.J = self.kin.jacobian() self.J_inv = self.kin.jacobian_pseudo_inverse() self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32) self.control_arm = Limb("right") self.control_joint_names = self.control_arm.joint_names() self.dx = 0 self.dy = 0 self.distance = 1000 self.finish = False self.found = 0 ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
def __init__(self, mode, arm_mode, step=5): self.mode = mode self.arm_mode = arm_mode self._step = step self._right_limb = Limb('right') self._left_limb = Limb('left') self._subscribe()
def set_joints( target_angles_right, target_angles_left): right = Limb("right") left = Limb("left") reach_right = False reach_left = False while not reach_right or not reach_left: right.set_joint_positions(target_angles_right) left.set_joint_positions(target_angles_left) current_angles_right = right.joint_angles() current_angles_left = left.joint_angles() for k, v in current_angles_right.iteritems(): if abs(target_angles_right[k] - v) > 0.01: reach_right = False break reach_right = True for k, v in current_angles_left.iteritems(): if abs(target_angles_left[k] - v) > 0.01: reach_left = False break reach_left = True
def __init__(self, starting_poss=None, push_thresh=10, mode='one_arm', arm_mode='first'): """ Initialises parameters and moves the arm to a neutral position. """ self.push_thresh = push_thresh self._right_neutral_pos = starting_poss[0] self._left_neutral_pos = starting_poss[1] self._mode = mode self._arm_mode = arm_mode rospy.loginfo("Creating interface and calibrating gripper") self._right_limb = Limb('right') self._left_limb = Limb('left') self._right_gripper = Gripper('right', CHECK_VERSION) self._right_gripper.calibrate() self._left_gripper = Gripper('left', CHECK_VERSION) self._left_gripper.calibrate() self._is_right_fist_closed = False self._is_left_fist_closed = False rospy.loginfo("Moving to neutral position") self.move_to_neutral() rospy.loginfo("Initialising PoseGenerator") self._pg = PoseGenerator(self._mode, self._arm_mode) self._sub_right_gesture = rospy.Subscriber( "/low_myo/gesture", UInt8, self._right_gesture_callback) self._sub_left_gesture = rospy.Subscriber("/top_myo/gesture", UInt8, self._left_gesture_callback) self._last_data = None self._pg.calibrate()
def __init__(self, name, rate=100, fk='robot', ik='trac', default_kv_max=1., default_ka_max=0.5): """ :param name: 'left' or 'right' :param rate: Rate of the control loop for execution of motions :param fk: Name of the Forward Kinematics solver, "robot", "kdl", "trac" or "ros" :param ik: Name of the Inverse Kinematics solver, "robot", "kdl", "trac" or "ros" :param default_kv_max: Default K maximum for velocity :param default_ka_max: Default K maximum for acceleration """ Limb.__init__(self, name) self._world = 'base' self.kv_max = default_kv_max self.ka_max = default_ka_max self._gripper = Gripper(name) self._rate = rospy.Rate(rate) self._tf_listener = TransformListener() self.recorder = Recorder(name) # Kinematics services: names and services (if relevant) self._kinematics_names = {'fk': {'ros': 'compute_fk'}, 'ik': {'ros': 'compute_ik', 'robot': 'ExternalTools/{}/PositionKinematicsNode/IKService'.format(name), 'trac': 'trac_ik_{}'.format(name)}} self._kinematics_services = {'fk': {'ros': {'service': rospy.ServiceProxy(self._kinematics_names['fk']['ros'], GetPositionFK), 'func': self._get_fk_ros}, 'kdl': {'func': self._get_fk_pykdl}, 'robot': {'func': self._get_fk_robot}}, 'ik': {'ros': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['ros'], GetPositionIK), 'func': self._get_ik_ros}, 'robot': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['robot'], SolvePositionIK), 'func': self._get_ik_robot}, 'trac': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['trac'], GetConstrainedPositionIK), 'func': self._get_ik_trac}, 'kdl': {'func': self._get_ik_pykdl}}} self._selected_ik = ik self._selected_fk = fk # Kinematics services: PyKDL self._kinematics_pykdl = baxter_kinematics(name) if self._selected_ik in self._kinematics_names['ik']: rospy.wait_for_service(self._kinematics_names['ik'][self._selected_ik]) if self._selected_fk in self._kinematics_names['fk']: rospy.wait_for_service(self._kinematics_names['fk'][self._selected_fk]) # Execution attributes rospy.Subscriber('/robot/limb/{}/collision_detection_state'.format(name), CollisionDetectionState, self._cb_collision, queue_size=1) rospy.Subscriber('/robot/digital_io/{}_lower_cuff/state'.format(name), DigitalIOState, self._cb_dig_io, queue_size=1) self._stop_reason = '' # 'cuff' or 'collision' could cause a trajectory to be stopped self._stop_lock = Lock() action_server_name = "/robot/limb/{}/follow_joint_trajectory".format(self.name) self.client = SimpleActionClient(action_server_name, FollowJointTrajectoryAction) self._display_traj = rospy.Publisher("/move_group/display_planned_path", DisplayTrajectory, queue_size=1) self._gripper.calibrate() self.client.wait_for_server()
def __init__(self): self._tf = tf.TransformListener() self._rs = RobotEnable() self._left_arm = Limb('left') self._left_arm.set_joint_position_speed(0.5) self._right_arm = Limb('right') self._right_arm.set_joint_position_speed(0.5) self._left_arm_neutral = None self._right_arm_neutral = None self._left_iksvc = rospy.ServiceProxy(Mimic.IKSVC_LEFT_URI, SolvePositionIK) self._right_iksvc = rospy.ServiceProxy(Mimic.IKSVC_RIGHT_URI, SolvePositionIK) self._left_camera = CameraController('left_hand_camera') self._right_camera = CameraController('right_hand_camera') self._head_camera = CameraController('head_camera') self._left_camera.close() self._right_camera.close() self._head_camera.close() self._head_camera.resolution = CameraController.MODES[0] self._head_camera.open() self._head_camera_sub = rospy.Subscriber( '/cameras/head_camera/image', Image, self._head_camera_sub_callback) self._xdisplay_pub = rospy.Publisher('/robot/xdisplay', Image, latch=True) self._out_of_range = False self._ik_smooth = 4 self._ik_rate = 200 self._avg_window = 1000 self._r_trans_prev = [] self._l_trans_prev = [] self._r_ik_prev = [] self._l_ik_prev = [] self._joint_update_pub = rospy.Publisher( '/robot/joint_state_publish_rate', UInt16) self._joint_update_pub.publish(self._ik_rate) self._mimic_timer = None
def mover_setup(self): limb = Limb("right") # Right arm planner self.planner = PathPlanner("right_arm") # Left arm planner self.planner_left = PathPlanner("left_arm") place_holder = {'images': [], 'camera_infos': []} #Create the function used to call the service compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) count = 0 print(limb.endpoint_pose()) calibration_points = [] if ROBOT == "sawyer": Kp = 0.2 * np.array([0.4, 2, 1.7, 1.5, 2, 2, 3]) Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8]) Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6]) Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) else: Kp = 0.45 * np.array([0.8, 2.5, 1.7, 2.2, 2.4, 3, 4]) Kd = 0.015 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8]) Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6]) Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Controller for right arm self.c = Controller(Kp, Ki, Kd, Kw, Limb('right')) self.orien_const = OrientationConstraint() self.orien_const.link_name = "right_gripper"; self.orien_const.header.frame_id = "base"; self.orien_const.orientation.y = -1.0; self.orien_const.absolute_x_axis_tolerance = 0.1; self.orien_const.absolute_y_axis_tolerance = 0.1; self.orien_const.absolute_z_axis_tolerance = 0.1; self.orien_const.weight = 1.0; box = PoseStamped() box.header.frame_id = "base" box.pose.position.x = self.box.pose.position.x box.pose.position.y = self.box.pose.position.y # box.pose.position.z = self.box.pose.position.z box.pose.position.z = self.conveyor_z # box.pose.position.x = 0.5 # box.pose.position.y = 0.0 # box.pose.position.z = 0.0 box.pose.orientation.x = 0.0 box.pose.orientation.y = 0.0 box.pose.orientation.z = 0.0 box.pose.orientation.w = 1.0 self.planner.add_box_obstacle((100, 100, 0.00001), "box", box) # Controller for left arm self.c_left = Controller(Kp, Ki, Kd, Kw, Limb('left'))
def __init__(self): rospy.init_node('vision_server_right') self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True) self.busy = False self.dx = 0 self.dy = 0 self.avg_dx = -1 self.avg_dy = -1 self.H = homography() self.framenumber = 0 self.history_x = np.arange(0, 10) * -1 self.history_y = np.arange(0, 10) * -1 self.bowlcamera = None self.newPosition = True self.foundBowl = 0 self.centerx = 400 #self.centerx = 250 self.centery = 150 #self.centery = 190 self.coefx = 0.1 / (526 - 369) self.coefy = 0.1 / (237 - 90) self.v_joint = np.arange(7) self.v_end = np.arange(6) self.cmd = {} self.found = False self.finish = 0 self.distance = 10 self.gripper = Gripper("right") #self.gripper.calibrate() cv2.namedWindow('image') self.np_image = np.zeros((400, 640, 3), np.uint8) cv2.imshow('image', self.np_image) #self._set_threshold() s = rospy.Service('vision_server_vertical', Vision, self.handle_vision_req) camera_topic = '/cameras/right_hand_camera/image' self.right_camera = rospy.Subscriber(camera_topic, Image, self._on_camera) ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance) print "\nReady to use right hand vision server\n" self.kin = baxter_kinematics('right') self.J = self.kin.jacobian() self.J_inv = self.kin.jacobian_pseudo_inverse() self.jointVelocity = np.asarray([1, 2, 3, 4, 5, 6, 7], np.float32) self.control_arm = Limb("right") self.control_joint_names = self.control_arm.joint_names()
def __init__(self): self._tf = tf.TransformListener() self._rs = RobotEnable() self._left_arm = Limb('left') self._left_arm.set_joint_position_speed(0.5) self._right_arm = Limb('right') self._right_arm.set_joint_position_speed(0.5) self._left_arm_neutral = None self._right_arm_neutral = None self._left_iksvc = rospy.ServiceProxy( Mimic.IKSVC_LEFT_URI, SolvePositionIK) self._right_iksvc = rospy.ServiceProxy( Mimic.IKSVC_RIGHT_URI, SolvePositionIK) self._left_camera = CameraController('left_hand_camera') self._right_camera = CameraController('right_hand_camera') self._head_camera = CameraController('head_camera') self._left_camera.close() self._right_camera.close() self._head_camera.close() self._head_camera.resolution = CameraController.MODES[0] self._head_camera.open() self._head_camera_sub = rospy.Subscriber('/cameras/head_camera/image', Image, self._head_camera_sub_callback) self._xdisplay_pub = rospy.Publisher('/robot/xdisplay', Image, latch=True) self._out_of_range = False self._ik_smooth = 4 self._ik_rate = 200 self._avg_window = 1000 self._r_trans_prev = [] self._l_trans_prev = [] self._r_ik_prev = [] self._l_ik_prev = [] self._joint_update_pub = rospy.Publisher('/robot/joint_state_publish_rate', UInt16) self._joint_update_pub.publish(self._ik_rate) self._mimic_timer = None
def main(): joint_states = { 'right_initial': { 'right_e0': 2.498, 'right_e1': 2.158, 'right_s0': 0.826, 'right_s1': 0.366, 'right_w0': 2.809, 'right_w1': 1.867, 'right_w2': 0.411, }, 'left_initial': { 'left_e0': -1.738, 'left_e1': 1.828, 'left_s0': 0.247, 'left_s1': 0.257, 'left_w0': 0.0721, 'left_w1': -0.818, 'left_w2': 1.826, }, } rospy.init_node('demo', anonymous=True) hdr = Header(stamp=rospy.Time.now(), frame_id='base') arm_right = Limb("right") kin_right = baxter_kinematics('right') arm_left = Limb("left") kin_left = baxter_kinematics('left') set_joints(target_angles_right=joint_states['right_initial']) # right arm movement ik_move(hdr, arm_right, kin_right, target_dx=0.35, arm_position='right', speedx=0.05) ik_move(hdr, arm_right, kin_right, target_dy=0.1, arm_position='right', speedy=0.1) ik_move(hdr, arm_right, kin_right, target_dx=-0.35, arm_position='right', speedx=0.1)
def __init__(self): self._left_arm = Limb('left') self._left_arm.set_joint_position_speed(0.3) self._right_arm = Limb('right') self._right_arm.set_joint_position_speed(0.3) self._left_iksvc = rospy.ServiceProxy(_IKSVC_LEFT_URI, SolvePositionIK) self._right_iksvc = rospy.ServiceProxy(_IKSVC_RIGHT_URI, SolvePositionIK) self._joint_update_pub = rospy.Publisher( '/robot/joint_state_publish_rate', UInt16) self._joint_update_pub.publish(250)
def __init__(self): self.rightg = Gripper('right') self.rightg.set_holding_force(100) self.leftg = Gripper('left') self.right = Limb('right') self.left = Limb('left') self.head = Head() self.angles= list() rospy.Subscriber("command", String, self.command) rospy.spin()
class TrajectoryWaypointsRecorder(): def __init__(self, limb_name, file_name): self.limb = Limb(limb_name) self.trajectory = [] self.file_name = file_name self.file_header = self.limb.joint_names() self.dt = rospy.Duration(secs=2) self.previous_time = rospy.Time.now() self.limb_nav = Navigator(limb_name) self.limb_nav.button2_changed.connect(self.limb_nav_button_pressed) rospy.on_shutdown(self.save_data) rospy.loginfo('Press Ctrl + C to exit') rospy.spin() def save_data(self): file_header = ','.join(x for x in self.file_header) # src: http://wiki.ros.org/Packages#Client_Library_Support full_path = os.path.join( rospkg.RosPack().get_path('multiple_kinect_baxter_calibration'), 'files', self.file_name) np.savetxt(full_path, self.trajectory, header=file_header, delimiter=',', fmt='%.4f', comments='') rospy.loginfo("Trajectory have been successfully saved to \n'%s'\n" % full_path) def limb_nav_button_pressed(self, state): now = rospy.Time.now() if (now - self.previous_time > self.dt) and state: self.limb_nav.inner_led = state self.record() elif not state: self.limb_nav.inner_led = state self.previous_time = now def record(self): joint_angles_dict = self.limb.joint_angles() joint_angles = [ joint_angles_dict[joint_name] for joint_name in self.file_header ] self.trajectory.append(joint_angles) rospy.loginfo('%d samples collected.' % len(self.trajectory))
def return_to_init(dict_keys,arm_name): from baxter_interface import Limb ''' Joint angles that give us desired initial position: {'left_w0': -0.029529130166794215, 'left_w1': 0.08436894333369775, 'left_w2': -0.08782040010643993, 'left_e0': 0.10009224640952324, 'left_e1': 0.03604854851530722, 'left_s0': -0.8061069040337849, 'left_s1': -0.13690778531877318} ''' init_pos=[-0.8061069040337849,-0.13690778531877318,0.10009224640952324,0.03604854851530722, -0.029529130166794215,0.08436894333369775,-0.08782040010643993] init_pos_dict=convert_list_to_dict(dict_keys,init_pos) arm=Limb(arm_name) t_end=time.time()+4 # Loop timer (in seconds) while time.time()<t_end: arm.set_joint_positions(init_pos_dict)
def record(): rospy.init_node('record') RobotEnable(CHECK_VERSION).enable() baxter_limb = Limb(limb_name) while True: initial_time = rospy.get_time() forces_vec3 = baxter_limb.endpoint_effort()['force'] forces = np.array([forces_vec3.x, forces_vec3.y, forces_vec3.z]) print "===========\nEndpoint Forces: \nF_x: {x} \nF_y: {y} \nF_z: {z}\n===========\n".format(forces_vec3) after_time = rospy.get_time() rospy.sleep(dt - (after_time - initial_time))
def __init__(self, calibrate_grippers=True): self._baxter_state = RobotEnable() self._left = Limb(LEFT) self._right = Limb(RIGHT) self._limbs = {LEFT: self._left, RIGHT: self._right} self._head = Head() self._left_gripper, self._right_gripper = Gripper(LEFT), Gripper(RIGHT) if calibrate_grippers: self.calibrate() self._left_ikservice = IKService(LEFT) self._right_ikservice = IKService(RIGHT)
def __init__(self, limb_name, file_name): self.limb = Limb(limb_name) self.trajectory = [] self.file_name = file_name self.file_header = self.limb.joint_names() self.dt = rospy.Duration(secs=2) self.previous_time = rospy.Time.now() self.limb_nav = Navigator(limb_name) self.limb_nav.button2_changed.connect(self.limb_nav_button_pressed) rospy.on_shutdown(self.save_data) rospy.loginfo('Press Ctrl + C to exit') rospy.spin()
def initBaxterObjects(): global rightGripper global leftGripper global rightMover global leftMover rightGripper = Gripper('right', versioned = False) leftGripper = Gripper('left', versioned = False) rightMover = Limb('right') leftMover = Limb('left') # Initialize based on which is over the blocks global numBlocks global isOneArmSolution gridToCartesian.initToBaxter(rightMover)
def stir_salad(self,stir_joint_angles): self.errorhandle("ladle") initial_pose = get_current_pose(self.hdr,self.control_arm) initial_angles = Limb("right").joint_angles() set_joints(target_angles_right = stir_joint_angles) upstraight_pose = get_current_pose(self.hdr,self.control_arm) ready_pose = copy.deepcopy(upstraight_pose) x,y = self.moveToBowl() ready_pose.pose.position.x = x + 0.13 ready_pose.pose.position.y = y - 0.15 ik_move_to_pose(self.control_arm,self.kin_right,ready_pose) stir_pose = copy.deepcopy(ready_pose) stir_pose.pose.position.z -= 0.24 ik_move_to_pose(self.control_arm,self.kin_right,stir_pose) initial_stir_pose = get_current_pose(self.hdr,self.control_arm) initial_y = initial_stir_pose.pose.position.y initial_x = initial_stir_pose.pose.position.x current_pose = copy.deepcopy(initial_stir_pose) for i in range(6000): current_pose.pose.position.y = initial_y + math.sin(i*0.004)*0.07 current_pose.pose.position.x = initial_x - (0.07-math.cos(i*0.004)*0.07) ik_pykdl(self.control_arm,self.kin_right,current_pose) ik_move_to_pose(self.control_arm,self.kin_right,ready_pose) ik_move_to_pose(self.control_arm,self.kin_right,upstraight_pose) set_joints(target_angles_right = initial_angles)
def __init__(self, side_name): """ Constructor. @param side_name: which arm side we are refering to 'left' or 'right'. """ self.__side_name = side_name # MoveIt interface to a group of arm joints. # Either left arm joint group or right arm joint group. self.moveit_limb = MoveGroupCommander('{}_arm'.format(side_name)) # MoveIt limb setting. self.moveit_limb.set_end_effector_link('{}_gripper'.format(side_name)) self.moveit_limb.set_planner_id('RRTConnectKConfigDefault') self.moveit_limb.set_goal_position_tolerance(0.01) self.moveit_limb.set_goal_orientation_tolerance(0.01) # MoveIt does not provide support for Baxter gripper. # Thus we use Baxter SDK gripper instead. self.gripper = Gripper(side_name, CHECK_VERSION) self.gripper.calibrate() self.baxter_sdk_limb = Limb(side_name)
def endpoint_pose(self): """ Returns the pose of the end effector :return: [[x, y, z], [x, y, z, w]] """ pose = Limb.endpoint_pose(self) return [[pose['position'].x, pose['position'].y, pose['position'].z], [pose['orientation'].x, pose['orientation'].y, pose['orientation'].z, pose['orientation'].w]]
def __init__(self): self._left_arm = Limb('left') self._left_arm.set_joint_position_speed(0.3) self._right_arm = Limb('right') self._right_arm.set_joint_position_speed(0.3) self._left_iksvc = rospy.ServiceProxy( _IKSVC_LEFT_URI, SolvePositionIK) self._right_iksvc = rospy.ServiceProxy( _IKSVC_RIGHT_URI, SolvePositionIK) self._joint_update_pub = rospy.Publisher( '/robot/joint_state_publish_rate', UInt16) self._joint_update_pub.publish(250)
def set_joints( target_angles_right = None, target_angles_left = None,timeout= 40000): right = Limb("right") left = Limb("left") if target_angles_right == None: reach_right = True else: reach_right = False if target_angles_left == None: reach_left = True else: reach_left = False time = 0 while not reach_right or not reach_left: if target_angles_right: right.set_joint_positions(target_angles_right) if target_angles_left: left.set_joint_positions(target_angles_left) current_angles_right = right.joint_angles() current_angles_left = left.joint_angles() if reach_right == False: for k, v in current_angles_right.iteritems(): if abs(target_angles_right[k] - v) > 0.01: reach_right = False break reach_right = True if reach_left == False: for k, v in current_angles_left.iteritems(): if abs(target_angles_left[k] - v) > 0.01: reach_left = False break reach_left = True time+=1 if time > timeout: print "Time out" break
class Baxter_Interactive(object): def __init__(self, arm): #Vector to record poses self.recorded = [] self.arm = arm #enable Baxter self._en = RobotEnable() self._en.enable() #use DigitalIO to connect to gripper buttons self._close_button = DigitalIO(self.arm + '_upper_button') self._open_button = DigitalIO(self.arm + '_lower_button') #set up gripper self._gripper = Gripper(self.arm) self._gripper.calibrate() #set up navigator self._navigator = Navigator(self.arm) #set up limb self._limb = Limb(self.arm) self._limb.set_joint_position_speed(0.5) self._limb.move_to_neutral() print 'Ready to record...' def recorder(self): doneRecording = False while not doneRecording: if self._navigator.button0: self.recorded.append(self._limb.joint_angles()) print 'Waypoint Added' rospy.sleep(1) if self._close_button.state: self.recorded.append('CLOSE') self._gripper.close() print 'Gripper Closed' rospy.sleep(1) if self._open_button.state: self.recorded.append('OPEN') self._gripper.open() print 'Gripper Opened' rospy.sleep(1) if self._navigator.button1: print 'END RECORDING' doneRecording = True rospy.sleep(3) while doneRecording: for waypoint in self.recorded: if waypoint == 'OPEN': self._gripper.open() rospy.sleep(1) elif waypoint == 'CLOSE': self._gripper.close() rospy.sleep(1) else: self._limb.move_to_joint_positions(waypoint)
def clean_shutdown(self): hdr = Header(stamp=rospy.Time.now(), frame_id='base') arm = Limb("right") # ik_move(hdr,arm, x = 0.82 , y = -0.2) # ik_move(hdr,arm, z = -0.136) # self.gripper.open() # rospy.sleep(0.5) # ik_move(hdr,arm, target_dx= -0.2) print "Demo finished" rospy.signal_shutdown("Done")
def endpoint_pose(self): """ Returns the pose of the end effector :return: [[x, y, z], [x, y, z, w]] """ pose = Limb.endpoint_pose(self) return [[pose['position'].x, pose['position'].y, pose['position'].z], [ pose['orientation'].x, pose['orientation'].y, pose['orientation'].z, pose['orientation'].w ]]
class track(): def __init__(self): self.centerx = 365 self.centery = 120 self.coefx = 0.1/(526-369) self.coefy = 0.1/(237-90) self.count = 0 self.hdr = Header(stamp=rospy.Time.now(), frame_id='base') self.gripper = Gripper("right") self.gripper.calibrate() self.gripper.set_moving_force(0.1) rospy.sleep(0.5) self.gripper.set_holding_force(0.1) rospy.sleep(0.5) self.busy = False self.gripper_distance = 100 self.subscribe_gripper() self.robotx = -1 self.roboty = -1 self.framenumber = 0 self.history = np.arange(0,20)*-1 self.newPosition = True self.bowlcamera = None self.kin = baxter_kinematics('right') self.J = self.kin.jacobian() self.J_inv = self.kin.jacobian_pseudo_inverse() self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32) self.control_arm = Limb("right") self.control_joint_names = self.control_arm.joint_names() self.dx = 0 self.dy = 0 self.distance = 1000 self.finish = False self.found = 0 ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance) def _read_distance(self,data): self.distance = data.range def vision_request_right(self, controlID, requestID): try: rospy.wait_for_service('vision_server_vertical') vision_server_req = rospy.ServiceProxy('vision_server_vertical', VisionVertical) return vision_server_req(controlID, requestID) except (rospy.ServiceException,rospy.ROSInterruptException), e: print "Service call failed: %s" % e self.clean_shutdown_service()
def __init__(self, arm): #Vector to record poses self.recorded = [] self.arm = arm #enable Baxter self._en = RobotEnable() self._en.enable() #use DigitalIO to connect to gripper buttons self._close_button = DigitalIO(self.arm + '_upper_button') self._open_button = DigitalIO(self.arm + '_lower_button') #set up gripper self._gripper = Gripper(self.arm) self._gripper.calibrate() #set up navigator self._navigator = Navigator(self.arm) #set up limb self._limb = Limb(self.arm) self._limb.set_joint_position_speed(0.5) self._limb.move_to_neutral() print 'Ready to record...'
def pour_dressing(self, dressing_angles): self.errorhandle("dressing") initial_pose = get_current_pose(self.hdr, self.control_arm) initial_angles = Limb("right").joint_angles() # set_joints(target_angles_right = dressing_angles) ready_pose = get_current_pose(self.hdr, self.control_arm) #x,y = self.moveToBowl() x = self.pour_x y = self.pour_y ready_pose.pose.position.x = x ready_pose.pose.position.y = y - 0.15 ik_move_to_pose(self.control_arm, self.kin_right, ready_pose)
def __init__(self, sub_topic, pub_topic): self.planner = None self.limb = None self.executed = False self.planner = PathPlanner("right_arm") self.limb = Limb("right") # self.orien_const = OrientationConstraint() # self.orien_const.link_name = "right_gripper" # self.orien_const.header.frame_id = "base" # self.orien_const.orientation.z = 0 # self.orien_const.orientation.y = 1 # self.orien_const.orientation.w = 0 # self.orien_const.absolute_x_axis_tolerance = 0.1 # self.orien_const.absolute_y_axis_tolerance = 0.1 # self.orien_const.absolute_z_axis_tolerance = 0.1 # self.orien_const.weight = 1.0 size = [1, 2, 2] obstacle_pose = PoseStamped() obstacle_pose.header.frame_id = "base" obstacle_pose.pose.position.x = -1 obstacle_pose.pose.position.y = 0 obstacle_pose.pose.position.z = 0 obstacle_pose.pose.orientation.x = 0 obstacle_pose.pose.orientation.y = 0 obstacle_pose.pose.orientation.z = 0 obstacle_pose.pose.orientation.w = 1 self.planner.add_box_obstacle(size, "wall", obstacle_pose) size = [.75, 1, 1] obstacle_pose = PoseStamped() obstacle_pose.header.frame_id = "base" obstacle_pose.pose.position.x = 1 obstacle_pose.pose.position.y = 0 obstacle_pose.pose.position.z = -.4 obstacle_pose.pose.orientation.x = 0 obstacle_pose.pose.orientation.y = 0 obstacle_pose.pose.orientation.z = 0 obstacle_pose.pose.orientation.w = 1 self.planner.add_box_obstacle(size, "table", obstacle_pose) self.pub = rospy.Publisher(pub_topic, Bool) self.sub = rospy.Subscriber(sub_topic, PoseStamped, self.callback)
def __init__(self, arm): self.as_goal = {'left': baxterGoal(), 'right': baxterGoal()} self.as_feed = {'left': baxterFeedback(), 'right': baxterFeedback()} self.as_res = {'left': baxterResult(), 'right': baxterResult()} self.action_server_left = actionlib.SimpleActionServer( "baxter_action_server_left", baxterAction, self.execute_left, auto_start=False) self.action_server_right = actionlib.SimpleActionServer( "baxter_action_server_right", baxterAction, self.execute_right, auto_start=False) self.left_arm = Limb('left') self.right_arm = Limb('right') self.left_gripper = Gripper('left') self.right_gripper = Gripper('right') self.left_gripper.calibrate() self.arm = arm
def __init__(self): rospy.init_node('vision_server_right') self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True) self.busy = False self.dx = 0 self.dy = 0 self.avg_dx = -1 self.avg_dy = -1 self.H = homography() self.framenumber = 0 self.history_x = np.arange(0,10)*-1 self.history_y = np.arange(0,10)*-1 self.bowlcamera = None self.newPosition = True self.foundBowl = 0 self.centerx = 400 #self.centerx = 250 self.centery = 150 #self.centery = 190 self.coefx = 0.1/(526-369) self.coefy = 0.1/(237-90) self.v_joint = np.arange(7) self.v_end = np.arange(6) self.cmd = {} self.found = False self.finish = 0 self.distance = 10 self.gripper = Gripper("right") #self.gripper.calibrate() cv2.namedWindow('image') self.np_image = np.zeros((400,640,3), np.uint8) cv2.imshow('image',self.np_image) #self._set_threshold() s = rospy.Service('vision_server_vertical', Vision, self.handle_vision_req) camera_topic = '/cameras/right_hand_camera/image' self.right_camera = rospy.Subscriber(camera_topic, Image, self._on_camera) ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance) print "\nReady to use right hand vision server\n" self.kin = baxter_kinematics('right') self.J = self.kin.jacobian() self.J_inv = self.kin.jacobian_pseudo_inverse() self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32) self.control_arm = Limb("right") self.control_joint_names = self.control_arm.joint_names()
def create_arms(self): """ Create arm interface objects for Baxter. An arm consists of a Limb and its Gripper. """ # create arm objects if self.sim: self.left_arm = pybullet_interface.Limb(self.baxter_id, "left", self.config) # self.left_arm.gripper = pybullet_interface.Gripper("left") self.right_arm = pybullet_interface.Limb(self.baxter_id, "right", self.config) # self.right_arm.gripper = pybullet_interface.Gripper("right") else: self.left_arm = Limb("left") self.left_arm.gripper = Gripper("left") self.left_arm.kin = baxter_kinematics("left") self.right_arm = Limb("right") self.right_arm.gripper = Gripper("right") self.right_arm.kin = baxter_kinematics("right") return
class Baxter_Deli(object): def __init__(self): self.rightg = Gripper('right') self.rightg.set_holding_force(100) self.leftg = Gripper('left') self.right = Limb('right') self.left = Limb('left') self.head = Head() self.angles= list() rospy.Subscriber("command", String, self.command) rospy.spin() def command(self, comm): if comm.data == "left": self.angles.append(self.left.joint_angles()) elif comm.data == "right": self.angles.append(self.right.joint_angles()) elif comm.data == "done": print self.angles
def main(): joint_states = { # 'observe':{ # 'right_e0': -0.365, # 'right_e1': 1.061, # 'right_s0': 0.920, # 'right_s1': -0.539, # 'right_w0': 0.350, # 'right_w1': 1.105, # 'right_w2': -0.221, # }, 'observe_vertical': { 'right_e0': 0.2784175126831055, 'right_e1': 1.8561167512207033, 'right_s0': -0.1464951650756836, 'right_s1': -0.5292233712158203, 'right_w0': -0.9572040105468751, 'right_w1': -1.200723460345459, 'right_w2': 0.004985437554931641, }, } roscpp_initialize(sys.argv) rospy.init_node('demo', anonymous=True) hdr = Header(stamp=rospy.Time.now(), frame_id='base') arm = Limb("right") kin = baxter_kinematics('right') #set_joints(target_angles_right = joint_states['observe_midpoint'],target_angles_left = joint_states['observe_left'],timeout= 100000) tracker = track() rospy.on_shutdown(tracker.clean_shutdown) #pour tomato set_joints(target_angles_right=joint_states['observe_vertical']) initial_pose = get_current_pose(hdr, arm) #warm up the tracker after initial start up tracker.track(initial_pose, hdr, arm, id=1) ik_move(hdr, arm, kin, z=0.47) ik_move(hdr, arm, kin, target_dx=0.3, speedx=0.1) #ik_move(hdr,arm, kin, target_dz = -0.08, speedz = 0.1) ik_move(hdr, arm, kin, z=0.35, speedz=0.1) ik_move(hdr, arm, kin, x=0.75, speedx=0.1)
def main(): """ Calculate best PID constans using simulated annealing :return: """ rospy.init_node('pid_calibration') enabler = RobotEnable() enabler.enable() limb = Limb('left') init_params = {'left_s0': {'kp': 80.0, 'ki': 0.29, 'kd': 1.4}, 'left_s1': {'kp': 100.0, 'ki': 1.0, 'kd': 1.0}, 'left_e0': {'kp': 10.0, 'ki': 2.1, 'kd': 0.12}, 'left_e1': {'kp': 16.0, 'ki': 2.1, 'kd': 0.26}, 'left_w0': {'kp': 2.0, 'ki': 0.1, 'kd': 0.5}, 'left_w1': {'kp': 1.9, 'ki': 1.71, 'kd': 0.15}, 'left_w2': {'kp': 1.8, 'ki': 2.31, 'kd': 0.11}} init_error = calculate_error(init_params) best = simulated_annealing(init_params, init_error, calculate_error, neighbour, acceptance, stop_condition, 100.0, temperature_update=lambda x: 0.95*x, callback=callback) print best
ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') ikreq.pose_stamp.append(pose) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) return 0 if (resp.isValid[0]): #print("SUCCESS - Valid Joint Solution Found:") # Format solution into Limb API-compatible dictionary limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position)) arm = Limb("right") arm.set_joint_positions(limb_joints) return 1 #rospy.sleep(0.05) else: print("INVALID POSE - No Valid Joint Solution Found.") return 0 def ik_pykdl(arm, kin, pose, arm_position = 'right'): position = pose.pose.position orientation = pose.pose.orientation pos = [position.x,position.y,position.z] rot = [orientation.x,orientation.y,orientation.z,orientation.w] joint_angles = kin.inverse_kinematics(pos,rot) if joint_angles:
#holding force: 40 #replace cookie with drink #individually wrapped triangle sandwich will not do #!/usr/bin/env python import rospy, baxter_interface from baxter_interface import Gripper, Head, Limb rospy.init_node('deli_Baxter') rightg = Gripper('right') rightg.set_holding_force(100) leftg = Gripper('left') right = Limb('right') left = Limb('left') head = Head() head.set_pan(0.0, speed = 0.8, timeout = 0.0) Gripper.calibrate(rightg) Gripper.calibrate(leftg) neutral_right ={'right_s0': 0.5971020216843973, 'right_s1': -0.4237621926533455, 'right_w0': 0.4226117070624315, 'right_w1': 0.8471408901097197, 'right_w2': -0.9725438195193523, 'right_e0': -0.40727189918357737, 'right_e1': 1.161990446823201} neutral_left = {'left_w0': -0.3006602344255411, 'left_w1': 0.5549175500175484, 'left_w2': 3.050704291907117, 'left_e0': 0.5161845351234418, 'left_e1': 1.1984224905354794, 'left_s0': -0.8904758473674826, 'left_s1': -0.38387869216832476} right.move_to_joint_positions(neutral_right) left.move_to_joint_positions(neutral_left)
class Mimic(object): IKSVC_LEFT_URI = 'ExternalTools/left/PositionKinematicsNode/IKService' IKSVC_RIGHT_URI = 'ExternalTools/right/PositionKinematicsNode/IKService' def __init__(self): self._tf = tf.TransformListener() self._rs = RobotEnable() self._left_arm = Limb('left') self._left_arm.set_joint_position_speed(0.5) self._right_arm = Limb('right') self._right_arm.set_joint_position_speed(0.5) self._left_arm_neutral = None self._right_arm_neutral = None self._left_iksvc = rospy.ServiceProxy( Mimic.IKSVC_LEFT_URI, SolvePositionIK) self._right_iksvc = rospy.ServiceProxy( Mimic.IKSVC_RIGHT_URI, SolvePositionIK) self._left_camera = CameraController('left_hand_camera') self._right_camera = CameraController('right_hand_camera') self._head_camera = CameraController('head_camera') self._left_camera.close() self._right_camera.close() self._head_camera.close() self._head_camera.resolution = CameraController.MODES[0] self._head_camera.open() self._head_camera_sub = rospy.Subscriber('/cameras/head_camera/image', Image, self._head_camera_sub_callback) self._xdisplay_pub = rospy.Publisher('/robot/xdisplay', Image, latch=True) self._out_of_range = False self._ik_smooth = 4 self._ik_rate = 200 self._avg_window = 1000 self._r_trans_prev = [] self._l_trans_prev = [] self._r_ik_prev = [] self._l_ik_prev = [] self._joint_update_pub = rospy.Publisher('/robot/joint_state_publish_rate', UInt16) self._joint_update_pub.publish(self._ik_rate) self._mimic_timer = None ############################################################################ def start(self): self._rs.enable() self._reset() self._mimic_timer = rospy.Timer(rospy.Duration(1.0 / self._ik_rate), self._mimic_callback) rate = rospy.Rate(self._avg_window) while not rospy.is_shutdown(): try: (r_trans,r_rot) = self._tf.lookupTransform( '/skeleton/user_1/right_hand', '/skeleton/user_1/torso', rospy.Time(0)) (l_trans,l_rot) = self._tf.lookupTransform( '/skeleton/user_1/left_hand', '/skeleton/user_1/torso', rospy.Time(0)) self._l_trans_prev.append(l_trans) if (len(self._l_trans_prev) > self._avg_window): self._l_trans_prev.pop(0) self._r_trans_prev.append(r_trans) if (len(self._r_trans_prev) > self._avg_window): self._r_trans_prev.pop(0) except: self._out_of_range = True rate.sleep() continue rate.sleep() def _reset(self): if self._mimic_timer: self._mimic_timer.shutdown() self._left_arm.move_to_neutral() self._left_arm_neutral = self._left_arm.joint_angles() self._right_arm.move_to_neutral() self._right_arm_neutral = self._right_arm.joint_angles() print(self._left_arm_neutral) print(self._right_arm_neutral) def shutdown(self): self._reset() ############################################################################ def _mimic_callback(self, event): try: l_trans = ( sum(map(lambda x: x[0], self._l_trans_prev)) / self._avg_window, sum(map(lambda x: x[1], self._l_trans_prev)) / self._avg_window, sum(map(lambda x: x[2], self._l_trans_prev)) / self._avg_window) r_trans = ( sum(map(lambda x: x[0], self._r_trans_prev)) / self._avg_window, sum(map(lambda x: x[1], self._r_trans_prev)) / self._avg_window, sum(map(lambda x: x[2], self._r_trans_prev)) / self._avg_window) rh_x = clamp(0.65 + (l_trans[2] / 1.5), 0.5, 0.9) rh_y = clamp(-l_trans[0], -0.8, 0) rh_z = clamp(-l_trans[1], -0.1, 0.8) lh_x = clamp(0.65 + (r_trans[2] / 1.5), 0.5, 0.9) lh_y = clamp(-r_trans[0], 0, 0.8) lh_z = clamp(-r_trans[1], -0.1, 0.8) self.set_left_coords(lh_x, lh_y, lh_z) self.set_right_coords(rh_x, rh_y, rh_z) except: return def _head_camera_sub_callback(self, data): orig_img = cvbr.imgmsg_to_cv2(data, 'rgb8') img = cv2.cvtColor(orig_img, cv2.COLOR_RGB2GRAY) img = cv2.equalizeHist(img) img = cv2.GaussianBlur(img, (5, 5), 0) img = cv2.Canny(img, 100, 200) img = cv2.cvtColor(img, cv2.COLOR_GRAY2RGB) img = cv2.addWeighted(img, 0.75, orig_img, 0.25, 0) img = cv2.resize(img, (1024, 768)) if self._out_of_range: cv2.putText(img, 'Out of range!', (50, 768-50), cv2.FONT_HERSHEY_SIMPLEX, 2, (255, 0, 0), 4) img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR) img = cvbr.cv2_to_imgmsg(img) self._xdisplay_pub.publish(img) ############################################################################ def set_left_coords(self, x, y, z, er=math.pi * -1, ep=math.pi * 0.5, ey=math.pi * -1): self._set_arm_coords(self._left_iksvc, self._left_arm, x, y, z, er, ep, ey, self._get_neutral_joint_state(self._left_arm_neutral), self._l_ik_prev) def set_right_coords(self, x, y, z, er=math.pi * -1, ep=math.pi * 0.5, ey=math.pi * -1): self._set_arm_coords(self._right_iksvc, self._right_arm, x, y, z, er, ep, ey, self._get_neutral_joint_state(self._right_arm_neutral), self._r_ik_prev) ############################################################################ def _set_arm_coords(self, iksvc, limb, x, y, z, er, ep, ey, njs, prev): resp = self._get_ik(iksvc, x, y, z, er, ep, ey, njs) positions = resp[0] isValid = resp[1] self._out_of_range = not isValid prev.append(positions) if (len(prev) > self._ik_smooth): prev.pop(0) smooth_positions = {} for joint_name in positions: smooth_positions[joint_name] = \ sum(map(lambda x: x[joint_name], prev)) / self._ik_smooth limb.set_joint_positions(smooth_positions) def _get_ik(self, iksvc, x, y, z, er, ep, ey, njs): q = quaternion_from_euler(er, ep, ey) pose = PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id='base'), pose=Pose( position=Point( x=x, y=y, z=z, ), orientation=Quaternion(q[0], q[1], q[2], q[3]) ), ) ikreq = SolvePositionIKRequest() ikreq.pose_stamp.append(pose) ikreq.seed_angles.append(njs) iksvc.wait_for_service(1.0) resp = iksvc(ikreq) positions = dict(zip(resp.joints[0].name, resp.joints[0].position)) return (positions, resp.isValid[0]) def _get_neutral_joint_state(self, njs): js = JointState() js.header = Header(stamp=rospy.Time.now(), frame_id='base') for j in njs: js.name.append(j) js.position.append(njs[j]) return js
class vision_server(): def __init__(self): rospy.init_node('vision_server_right') self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True) self.busy = False self.dx = 0 self.dy = 0 self.avg_dx = -1 self.avg_dy = -1 self.H = homography() self.framenumber = 0 self.history_x = np.arange(0,10)*-1 self.history_y = np.arange(0,10)*-1 self.bowlcamera = None self.newPosition = True self.foundBowl = 0 self.centerx = 400 #self.centerx = 250 self.centery = 150 #self.centery = 190 self.coefx = 0.1/(526-369) self.coefy = 0.1/(237-90) self.v_joint = np.arange(7) self.v_end = np.arange(6) self.cmd = {} self.found = False self.finish = 0 self.distance = 10 self.gripper = Gripper("right") #self.gripper.calibrate() cv2.namedWindow('image') self.np_image = np.zeros((400,640,3), np.uint8) cv2.imshow('image',self.np_image) #self._set_threshold() s = rospy.Service('vision_server_vertical', Vision, self.handle_vision_req) camera_topic = '/cameras/right_hand_camera/image' self.right_camera = rospy.Subscriber(camera_topic, Image, self._on_camera) ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance) print "\nReady to use right hand vision server\n" self.kin = baxter_kinematics('right') self.J = self.kin.jacobian() self.J_inv = self.kin.jacobian_pseudo_inverse() self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32) self.control_arm = Limb("right") self.control_joint_names = self.control_arm.joint_names() #print np.dot(self.J,self.jointVelocity) def _read_distance(self,data): self.distance = data.range def _set_threshold(self): cv2.createTrackbar('Min R(H)','image',0,255,nothing) cv2.createTrackbar('Max R(H)','image',255,255,nothing) cv2.createTrackbar('Min G(S)','image',0,255,nothing) cv2.createTrackbar('Max G(S)','image',255,255,nothing) cv2.createTrackbar('Min B(V)','image',0,255,nothing) cv2.createTrackbar('Max B(V)','image',255,255,nothing) def get_joint_velocity(self): cmd = {} velocity_list = np.asarray([1,2,3,4,5,6,7],np.float32) for idx, name in enumerate(self.control_joint_names): v = self.control_arm.joint_velocity( self.control_joint_names[idx]) velocity_list[idx] = v cmd[name] = v return velocity_list def list_to_dic(self,ls): cmd = {} for idx, name in enumerate(self.control_joint_names): v = ls.item(idx) cmd[name] = v return cmd def PID(self): Kp = 0.5 vy = -Kp*self.dx vx = -Kp*self.dy return vx,vy def _on_camera(self, data): self.busy = True self.framenumber += 1 index = self.framenumber % 10 cv2.namedWindow('image') cv_image = cv_bridge.CvBridge().imgmsg_to_cv(data, "bgr8") np_image = np.asarray(cv_image) image_after_process, mask = self.image_process(np_image) self.history_x[index] = self.dx*100 self.history_y[index] = self.dy*100 self.avg_dx = np.average(self.history_x)/100 self.avg_dy = np.average(self.history_y)/100 # if abs(self.dx)<0.01 and abs(self.dy)<0.01: # self.found = True # else: # self.found = False vx, vy = self.PID() self.v_end = np.asarray([(1-((abs(vx)+abs(vy))*5))*0.03,vy,vx,0,0,0],np.float32) #self.v_end = np.asarray([0,0,0,0,0,0.05],np.float32) self.v_joint = np.dot(self.J_inv,self.v_end) self.cmd = self.list_to_dic(self.v_joint) self.busy = False cv2.imshow('image',image_after_process) cv2.waitKey(1) def image_process(self,img): # min_r = cv2.getTrackbarPos('Min R(H)','image') # max_r = cv2.getTrackbarPos('Max R(H)','image') # min_g = cv2.getTrackbarPos('Min G(S)','image') # max_g = cv2.getTrackbarPos('Max G(S)','image') # min_b = cv2.getTrackbarPos('Min B(V)','image') # max_b = cv2.getTrackbarPos('Max B(V)','image') # min_r = 0 # max_r = 57 # min_g = 87 # max_g = 181 # min_b = 37 # max_b = 70 min_r = 0 max_r = 58 min_g = 86 max_g = 255 min_b = 0 max_b = 255 min_color = (min_r, min_g, min_b) max_color = (max_r, max_g, max_b) #img = cv2.flip(img, flipCode = -1) hsv_img = cv2.cvtColor(img,cv2.COLOR_BGR2HSV) mask = cv2.inRange(hsv_img, min_color, max_color) result = cv2.bitwise_and(img, img, mask = mask) mask_bool = np.asarray(mask, bool) label_img = morphology.remove_small_objects(mask_bool, 1000, connectivity = 1) objects = measure.regionprops(label_img) if objects != []: self.found = True target = objects[0] box = target.bbox cv2.rectangle(img,(box[1],box[0]),(box[3],box[2]),(0,255,0),3) dx_pixel=target.centroid[1]-self.centerx dy_pixel=target.centroid[0]-self.centery dx=dx_pixel*self.coefx dy=dy_pixel*self.coefy angle = target.orientation cv2.circle(img,(int(target.centroid[1]),int(target.centroid[0])),10,(0,0,255),-1) self.dx = dx self.dy = dy #print self.dx,self.dy else: self.found = False self.dx = 0 self.dy = 0 return img, mask_bool def handle_vision_req(self, req): #resp = VisionResponse(req.requestID, 0, self.avg_dx, self.avg_dy) self.finish = 0 if self.found == True: if self.distance > 0.07: self.control_arm.set_joint_velocities(self.cmd) #rospy.sleep(0.02) else: self.finish = 1 resp = VisionResponse(req.requestID, self.finish, self.dx, self.dy) return resp def clean_shutdown(self): print "Server finished" cv2.imwrite('box_img.png', self.blank_image) rospy.signal_shutdown("Done") sys.exit()
def __init__(self): self.centerx = 365 self.centery = 120 self.coefx = 0.1/(526-369) self.coefy = 0.1/(237-90) self.count = 0 self.hdr = Header(stamp=rospy.Time.now(), frame_id='base') self.gripper_left = Gripper("left") self.gripper_left.calibrate() self.gripper_left.set_moving_force(0.01) rospy.sleep(0.5) self.gripper_left.set_holding_force(0.01) rospy.sleep(0.5) self.gripper_right = Gripper("right") self.gripper_right.calibrate() rospy.sleep(1) self.busy = False self.gripper_distance = 100 self.subscribe_gripper() self.robotx = -1 self.roboty = -1 self.framenumber = 0 self.history = np.arange(0,20)*-1 self.newPosition = True self.bowlcamera = None self.kin_right = baxter_kinematics('right') self.kin_left = baxter_kinematics('left') self.J = self.kin_right.jacobian() self.J_inv = self.kin_right.jacobian_pseudo_inverse() self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32) self.control_arm = Limb("right") self.left_arm = Limb("left") self.control_joint_names = self.control_arm.joint_names() self.dx = 0 self.dy = 0 self.distance = 1000 self.finish = False self.found = 0 self.pour_x = 0 self.pour_y = 0 ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance) self.joint_states = { 'observe':{ 'right_e0': -0.631, 'right_e1': 0.870, 'right_s0': 0.742, 'right_s1': -0.6087, 'right_w0': 0.508, 'right_w1': 1.386, 'right_w2': -0.5538, }, 'observe_ladle':{ 'right_e0': -0.829, 'right_e1': 0.831, 'right_s0': 0.678, 'right_s1': -0.53, 'right_w0': 0.716, 'right_w1': 1.466, 'right_w2': -0.8099, }, 'observe_left':{ 'left_w0': 2.761932405432129, 'left_w1': -1.5700293346069336, 'left_w2': -0.8893253607604981, 'left_e0': -0.9805972175354004, 'left_e1': 1.8300390778564455, 'left_s0': 1.4783739826354982, 'left_s1': -0.9503010970092775, }, 'stir':{ 'right_e0': -0.179, 'right_e1': 1.403, 'right_s0': 0.381, 'right_s1': -0.655, 'right_w0': 1.3, 'right_w1': 2.04, 'right_w2': 0.612, }, 'observe_vertical':{ 'right_e0': 0.699, 'right_e1': 1.451, 'right_s0': -1.689, 'right_s1': 0.516, 'right_w0': 0.204, 'right_w1': 0.935, 'right_w2': -2.706, }, 'observe_midpoint':{ 'right_e0': -0.606, 'right_e1': 0.968, 'right_s0': 0.0, 'right_s1': -0.645, 'right_w0': 0.465, 'right_w1': 1.343, 'right_w2': -0.437, }, 'dressing':{ 'right_e0': 0.967, 'right_e1': 1.386, 'right_s0': -0.348, 'right_s1': -0.155, 'right_w0': 0.264, 'right_w1': 1.521, 'right_w2': -2.199, }, }
import sys import time import rospy from std_msgs.msg import UInt32 from msg_types import * import baxter_interface from baxter_interface import Gripper, Head, Limb rospy.init_node('run_condition') voice_pub = rospy.Publisher('/voice', UInt32, queue_size=10) time.sleep(1) rightg = Gripper('right') rightg.set_holding_force(50) leftg = Gripper('left') right = Limb('right') left = Limb('left') head = Head() neutral_right = {'right_s0': 0.476301034638421, 'right_s1': -0.5606699779721187, 'right_w0': 0.07094661143970038, 'right_w1': 0.7037136864424336, 'right_w2': -0.28033498898605935, 'right_e0': -0.16566992509162468, 'right_e1': 1.3077186216723151} neutral_left = {'left_w0': -0.15339807878854136, 'left_w1': 0.34552917247118947, 'left_w2': 3.0158062289827234, 'left_e0': 0.18676216092504913, 'left_e1': 1.5715633171886063, 'left_s0': -0.5836796897904, 'left_s1': -0.6845389265938658} right.move_to_joint_positions(neutral_right) left.move_to_joint_positions(neutral_left) head.set_pan(0.0, speed = 0.8, timeout = 0.0) Gripper.calibrate(rightg) Gripper.calibrate(leftg) # neutral_right = {'right_s0': 0.476301034638421, 'right_s1': -0.5606699779721187, 'right_w0': 0.07094661143970038, 'right_w1': 0.7037136864424336, 'right_w2': -0.28033498898605935, 'right_e0': -0.16566992509162468, 'right_e1': 1.3077186216723151}
def __init__(self): self._left_arm = Limb('left') self._left_arm.set_joint_position_speed(0.6) self._right_arm = Limb('right') self._right_arm.set_joint_position_speed(0.6)
class IK(object): def __init__(self): self._left_arm = Limb('left') self._left_arm.set_joint_position_speed(0.6) self._right_arm = Limb('right') self._right_arm.set_joint_position_speed(0.6) def neutral(self): self._left_arm.move_to_neutral() self._right_arm.move_to_neutral() def ik_calculate(self,limb,pos,rot): ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') #rotation -0.5 is perp and -1.0 is parallel for rot[2] quat = transformations.quaternion_from_euler(rot[0], rot[1], rot[2]) pose = PoseStamped( header=hdr, pose=Pose( position=Point( x=pos[0], #depth y=pos[1], #lateral z=pos[2], #height ), orientation=Quaternion( quat[0], quat[1], quat[2], quat[3], ), ), ) ikreq.pose_stamp.append(pose) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) return 1 # Check if result valid, and type of seed ultimately used to get solution # convert rospy's string representation of uint8[]'s to int's resp_seeds = struct.unpack('<%dB' % len(resp.result_type), resp.result_type) if (resp_seeds[0] != resp.RESULT_INVALID): seed_str = { ikreq.SEED_USER: '******', ikreq.SEED_CURRENT: 'Current Joint Angles', ikreq.SEED_NS_MAP: 'Nullspace Setpoints', }.get(resp_seeds[0], 'None') #print("SUCCESS - Valid Joint Solution Found from Seed Type: %s" % # (seed_str,)) # Format solution into Limb API-compatible dictionary limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position)) return limb_joints else: pass #print pose #print("INVALID POSE - No Valid Joint Solution Found.") return 0
class track(): def __init__(self): self.centerx = 365 self.centery = 120 self.coefx = 0.1/(526-369) self.coefy = 0.1/(237-90) self.count = 0 self.hdr = Header(stamp=rospy.Time.now(), frame_id='base') self.gripper = Gripper("right") self.gripper.calibrate() rospy.sleep(2) # self.gripper.set_moving_force(0.1) # rospy.sleep(0.5) # self.gripper.set_holding_force(0.1) # rospy.sleep(0.5) self.busy = False self.gripper_distance = 100 self.subscribe_gripper() self.robotx = -1 self.roboty = -1 self.framenumber = 0 self.history = np.arange(0,20)*-1 self.newPosition = True self.bowlcamera = None self.kin = baxter_kinematics('right') self.J = self.kin.jacobian() self.J_inv = self.kin.jacobian_pseudo_inverse() self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32) self.control_arm = Limb("right") self.control_joint_names = self.control_arm.joint_names() self.dx = 0 self.dy = 0 self.distance = 1000 self.finish = False self.found = 0 ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance) self.joint_states = { # 'observe':{ # 'right_e0': -0.365, # 'right_e1': 1.061, # 'right_s0': 0.920, # 'right_s1': -0.539, # 'right_w0': 0.350, # 'right_w1': 1.105, # 'right_w2': -0.221, # }, 'observe':{ 'right_e0': -0.631, 'right_e1': 0.870, 'right_s0': 0.742, 'right_s1': -0.6087, 'right_w0': 0.508, 'right_w1': 1.386, 'right_w2': -0.5538, }, 'observe_vertical':{ 'right_e0': 0.699, 'right_e1': 1.451, 'right_s0': -1.689, 'right_s1': 0.516, 'right_w0': 0.204, 'right_w1': 0.935, 'right_w2': -2.706, }, 'observe_ladle':{ 'right_e0': -0.829, 'right_e1': 0.831, 'right_s0': 0.678, 'right_s1': -0.53, 'right_w0': 0.716, 'right_w1': 1.466, 'right_w2': -0.8099, }, } def _read_distance(self,data): self.distance = data.range def vision_request_right(self, controlID, requestID): try: rospy.wait_for_service('vision_server_vertical') vision_server_req = rospy.ServiceProxy('vision_server_vertical', VisionVertical) return vision_server_req(controlID, requestID) except (rospy.ServiceException,rospy.ROSInterruptException), e: print "Service call failed: %s" % e self.clean_shutdown_service()
ikreq.pose_stamp.append(poses['right']) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) return 1 if (resp.isValid[0]): print("SUCCESS - Valid Joint Solution Found:") # Format solution into Limb API-compatible dictionary #limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position)) #print limb_joints arm = Limb('right') i = 0 while i < 300: arm.set_joint_positions(limb_joints) i += 1 rospy.sleep(0.01) else: print("INVALID POSE - No Valid Joint Solution Found.") return 0
def __init__(self): self.centerx = 365 self.centery = 120 self.coefx = 0.1/(526-369) self.coefy = 0.1/(237-90) self.count = 0 self.hdr = Header(stamp=rospy.Time.now(), frame_id='base') self.gripper = Gripper("right") #self.gripper.calibrate() rospy.sleep(2) # self.gripper.set_moving_force(0.1) # rospy.sleep(0.5) # self.gripper.set_holding_force(0.1) # rospy.sleep(0.5) self.busy = False self.gripper_distance = 100 self.subscribe_gripper() self.robotx = -1 self.roboty = -1 self.framenumber = 0 self.history = np.arange(0,20)*-1 self.newPosition = True self.bowlcamera = None self.kin = baxter_kinematics('right') self.J = self.kin.jacobian() self.J_inv = self.kin.jacobian_pseudo_inverse() self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32) self.control_arm = Limb("right") self.control_joint_names = self.control_arm.joint_names() self.dx = 0 self.dy = 0 self.distance = 1000 self.finish = False self.found = 0 ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance) self.joint_states = { # 'observe':{ # 'right_e0': -0.365, # 'right_e1': 1.061, # 'right_s0': 0.920, # 'right_s1': -0.539, # 'right_w0': 0.350, # 'right_w1': 1.105, # 'right_w2': -0.221, # }, 'observe':{ 'right_e0': -0.631, 'right_e1': 0.870, 'right_s0': 0.742, 'right_s1': -0.6087, 'right_w0': 0.508, 'right_w1': 1.386, 'right_w2': -0.5538, }, 'observe_vertical':{ 'right_e0': 0.699, 'right_e1': 1.451, 'right_s0': -1.689, 'right_s1': 0.516, 'right_w0': 0.204, 'right_w1': 0.935, 'right_w2': -2.706, }, }
class IKHelper(object): """An abstraction layer for using Baxter's built in IK service.""" ############################################################################ def __init__(self): self._left_arm = Limb('left') self._left_arm.set_joint_position_speed(0.3) self._right_arm = Limb('right') self._right_arm.set_joint_position_speed(0.3) self._left_iksvc = rospy.ServiceProxy( _IKSVC_LEFT_URI, SolvePositionIK) self._right_iksvc = rospy.ServiceProxy( _IKSVC_RIGHT_URI, SolvePositionIK) self._joint_update_pub = rospy.Publisher( '/robot/joint_state_publish_rate', UInt16) self._joint_update_pub.publish(250) ############################################################################ def reset(self): """Reset both arms to their neutral positions.""" self._left_arm.move_to_neutral() self._right_arm.move_to_neutral() def set_left(self, pos, rot=(0, math.pi, math.pi *0.5), wait=False): """Set the endpoint of the left arm to the supplied coordinates. Arguments: pos -- Position in space in (x, y, z) format. rot -- Rotation in space in (r, p, y) format. (defaults to pointing downwards.) Keyword arguments: wait -- If True, method will block until in position. (default False) """ self._set_arm(self._left_iksvc, self._left_arm, pos, rot, wait) def set_left(self, pos, rot=(0, math.pi, math.pi *0.5), wait=False): """Set the endpoint of the right arm to the supplied coordinates. Arguments: pos -- Position in space in (x, y, z) format. rot -- Rotation in space in (r, p, y) format. (defaults to pointing downwards.) Keyword arguments: wait -- If True, method will block until in position. (default False) """ self._set_arm(self._right_iksvc, self._right_arm, pos, rot, wait) def get_left(self): """Return the current endpoint pose of the left arm.""" return self._left_arm.endpoint_pose()['position'] def get_right(self): """Return the current endpoint pose of the left arm.""" return self._right_arm.endpoint_pose()['position'] def get_left_velocity(self): """Return the current endpoint velocity of the left arm.""" return self._left_arm.endpoint_velocity()['linear'] def get_right_velocity(self): """Return the current endpoint velocity of the right arm.""" return self._right_arm.endpoint_velocity()['linear'] def get_left_force(self): """Return the current endpoint force on the left arm.""" return self._left_arm.endpoint_effort()['force'] def get_right_force(self): """Return the current endpoint force on the right arm.""" return self._right_arm.endpoint_effort()['force'] ############################################################################ def _set_arm(self, iksvc, limb, pos, rot, wait): resp = self._get_ik(iksvc, pos, rot) positions = resp[0] isValid = resp[1] if not isValid: print('invalid: {0} {1} {2}'.format(x, y, z)) if not wait: limb.set_joint_positions(positions) else: limb.move_to_joint_positions(positions) def _get_ik(self, iksvc, pos, rot): q = quaternion_from_euler(rot[0], rot[1], rot[2]) pose = PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id='base'), pose=Pose( position=Point( x=pos[0], y=pos[1], z=pos[2], ), orientation=Quaternion(q[0], q[1], q[2], q[3]) ), ) ikreq = SolvePositionIKRequest() ikreq.pose_stamp.append(pose) iksvc.wait_for_service(5.0) resp = iksvc(ikreq) positions = dict(zip(resp.joints[0].name, resp.joints[0].position)) return (positions, resp.isValid[0])