def callback(data): #run every time i see a TFMessage on tf_topic. global tf_listener global last_seen global rbt #header = data.header #print(data.transforms[0]) #print(dir(data.transforms[0])) tim = data.transforms[0] tim = tim.header.stamp #print(tim) #print('fo') if tf_listener.frameExists('ar_marker_63'): now = rospy.Time.now() #tf_listener.waitForTransform('ar_marker_63','left_gripper',now,rospy.Dure) try: (trans, rot) = tf_listener.lookupTransform('ar_marker_63', 'base', tim) (omega, theta) = eqf.quaternion_to_exp(rot) rbt = eqf.return_rbt(trans, rot) #print(rbt) #print("") #print("") #print("Period was " + str(time.time() - last_seen)) if last_seen - time.time() > 10: #print("updated rbt, i hadn't seen it in a while \n\r") #i haven't seen the tag in a while pass last_seen = time.time() #bring this down to 5hz, the camera rate. I don't wanna kill the processor on this... time.sleep(0.2) #time0 asks for the most recent one except: pass
def callback(data): #run every time i see a TFMessage on tf_topic. global tf_listener global last_seen global rbt #header = data.header #print(data.transforms[0]) #print(dir(data.transforms[0])) tim = data.transforms[0] tim = tim.header.stamp #print(tim) #print('fo') if tf_listener.frameExists('ar_marker_63'): now = rospy.Time.now() #tf_listener.waitForTransform('ar_marker_63','left_gripper',now,rospy.Dure) try: (trans,rot) = tf_listener.lookupTransform('ar_marker_63','base',tim) (omega,theta) = eqf.quaternion_to_exp(rot) rbt = eqf.return_rbt(trans,rot) #print(rbt) #print("") #print("") #print("Period was " + str(time.time() - last_seen)) if last_seen - time.time() > 10: #print("updated rbt, i hadn't seen it in a while \n\r") #i haven't seen the tag in a while pass last_seen = time.time() #bring this down to 5hz, the camera rate. I don't wanna kill the processor on this... time.sleep(0.2) #time0 asks for the most recent one except: pass
def movment_handle(data): #run every time i get a service call #print(data) global gripper_status #anything can be empty. #if incrimental is not empty, assume that this is to be incrimental if (len(data.trans) is not 3) and (len(data.trans) is not 0): return("ERROR: trans is of the wrong length") elif len(data.trans) is 3: trans = data.trans move = True else: move = True #assume that i'm staying at the same location loc = get_pose() trans = loc['trans'] if data.grip == 'True': gripper_status = False #actuate_gripper(False) #apparently, fase is actually what makes it suck. Weird. elif data.grip == 'False': gripper_status = True #actuate_gripper(True) else: #just let currnet state go on. pass if len(data.rot) is 0: #assume that no rotation was intended... aka, keep the current rotation I want pose = get_pose() rot = pose['rot'] else: rot = data.rot if len(rot) is not 4: return("Error. ROT should be a quaternion") if len(data.incrimental) is 0: incrimental = False else: incrimental = True if len(data.target_rot) > 0: target_rot = data.target_rot target_trans = data.target_trans target_rbt = eqf.return_rbt(target_trans,target_rot) else: target_rbt = np.eye(4) if len(data.keep_orient) is 0: keep_orient = False else: keep_orient = True if len(data.change_height) is 0: changeHeight = True else: changeHeight = False print "I am currently at " +str(get_pose()) if move and incrimental: print("Incrimental Move by " + str(trans)) #print("changeHeight?" + str(changeHeight)) incrimental_movement(trans,target_rbt,changeHeight = changeHeight, keep_oreint = keep_orient) elif move: print("Absolute Movement to " + str(trans) + "rot " + str(rot)) move_to_coord(trans,rot,keep_orient) #if bool() #construct a response. Currently, i return a string...1 resp = kinematics_requestResponse("I_did_stuff") print("\n\r") return resp
def closed_loop_pick( which_arm, arm, destination='othello_piece', ): #assume that i'm already close. global tf_listener global last_seen limb = baxter_interface.Limb(which_arm) while True: pose = get_pose(limb) execute = True exit = raw_input( "E to quit. K to start keyboard. D for dist to target. Otherwise, i'll move to 0 \n\r" ) if (exit == 'e') or (exit == 'E'): rospy.signal_shutdown("Shutdown signal recieved") return elif (exit == 'd') or (exit == 'D'): execute = False elif (exit == 'k') or (exit == 'K'): keyboard_ctrl(which_arm, arm, left_gripper) try: #now = rospy.Time.now() #tf_listener.waitForTransform('suction_cup','othello_piece',now,rospy.Duration(2.5)) #now = rospy.Time.now() last_time = tf_listener.getLatestCommonTime( 'suction_cup', 'othello_piece') (trans, rot) = tf_listener.lookupTransform('suction_cup', 'othello_piece', last_time) print("Saw the othello piece " + str(last_time.to_sec() - rospy.Time.now().to_sec()) + " secs ago \n\r") #get the RBT from the suction_cup to the base so incrimental movement can deal with it. last_time = tf_listener.getLatestCommonTime('suction_cup', 'base') (trans_base, rot_base) = tf_listener.lookupTransform('suction_cup', 'base', last_time) rbt_to_base = eqf.return_rbt(trans_base, rot_base) #print("time diff") #if time.time() - last_seen > 3: # print("Warning. lag is over 3 seconds") #last_seen = time.time() except KeyboardInterrupt: sys.exit() except: print("Could not find trans / rot \n\r") traceback.print_exc() else: print("Error between suction cup and the othello piece is \n\r") print(trans) print("") err_dist = np.linalg.norm(trans[:2]) #error in meters from print("XY ERROR IS " + str(err_dist)) if execute: if trans[2] > 0.05: #i'm very high away, just get closer... trans_goal = (.85 * trans[0], .85 * trans[1], trans[2] / 2) change_height = True keep_oreint = False elif err_dist > 0.01: #1cm, 1 inch. #final lateral alignment trans_goal = ( trans[0], trans[1], 0 ) #i want to move the suction cupt to 0,0,whatever height i'm at right now of hte othello piece. change_height = False keep_oreint = True else: #just go down % hit it trans_goal = (.5 * trans[0], .5 * trans[1], trans[2]) change_height = True keep_oreint = True print("I want to move by, in the suction cup frame, \n\r") print(str(trans_goal) + "\n\r") incrimental_movement(trans_goal[0], trans_goal[1], trans_goal[2], arm, which_arm, rbt=rbt_to_base, changeHeight=change_height, keep_oreint=keep_oreint)
def closed_loop_pick(which_arm,arm,destination='othello_piece',): #assume that i'm already close. global tf_listener global last_seen limb = baxter_interface.Limb(which_arm) while True: pose = get_pose(limb) execute = True exit = raw_input("E to quit. K to start keyboard. D for dist to target. Otherwise, i'll move to 0 \n\r") if (exit == 'e') or (exit == 'E'): rospy.signal_shutdown("Shutdown signal recieved") return elif(exit == 'd') or (exit == 'D'): execute = False elif(exit == 'k') or (exit == 'K'): keyboard_ctrl(which_arm,arm,left_gripper) try: #now = rospy.Time.now() #tf_listener.waitForTransform('suction_cup','othello_piece',now,rospy.Duration(2.5)) #now = rospy.Time.now() last_time = tf_listener.getLatestCommonTime('suction_cup','othello_piece') (trans,rot) = tf_listener.lookupTransform('suction_cup','othello_piece',last_time) print("Saw the othello piece " + str(last_time.to_sec() - rospy.Time.now().to_sec()) + " secs ago \n\r") #get the RBT from the suction_cup to the base so incrimental movement can deal with it. last_time = tf_listener.getLatestCommonTime('suction_cup','base') (trans_base,rot_base) = tf_listener.lookupTransform('suction_cup','base',last_time) rbt_to_base = eqf.return_rbt(trans_base,rot_base) #print("time diff") #if time.time() - last_seen > 3: # print("Warning. lag is over 3 seconds") #last_seen = time.time() except KeyboardInterrupt: sys.exit() except: print("Could not find trans / rot \n\r") traceback.print_exc() else: print("Error between suction cup and the othello piece is \n\r") print(trans) print("") err_dist = np.linalg.norm(trans[:2]) #error in meters from print("XY ERROR IS " +str(err_dist)) if execute: if trans[2] > 0.05: #i'm very high away, just get closer... trans_goal = (.85*trans[0],.85*trans[1],trans[2]/2) change_height = True keep_oreint = False elif err_dist > 0.01: #1cm, 1 inch. #final lateral alignment trans_goal = (trans[0],trans[1],0) #i want to move the suction cupt to 0,0,whatever height i'm at right now of hte othello piece. change_height = False keep_oreint = True else: #just go down % hit it trans_goal = (.5*trans[0],.5*trans[1],trans[2]) change_height = True keep_oreint = True print("I want to move by, in the suction cup frame, \n\r") print(str(trans_goal) + "\n\r") incrimental_movement(trans_goal[0],trans_goal[1],trans_goal[2], arm, which_arm,rbt = rbt_to_base,changeHeight = change_height,keep_oreint=keep_oreint)
def movment_handle(data): #run every time i get a service call #print(data) global gripper_status #anything can be empty. #if incrimental is not empty, assume that this is to be incrimental if (len(data.trans) is not 3) and (len(data.trans) is not 0): return ("ERROR: trans is of the wrong length") elif len(data.trans) is 3: trans = data.trans move = True else: move = True #assume that i'm staying at the same location loc = get_pose() trans = loc['trans'] if data.grip == 'True': gripper_status = False #actuate_gripper(False) #apparently, fase is actually what makes it suck. Weird. elif data.grip == 'False': gripper_status = True #actuate_gripper(True) else: #just let currnet state go on. pass if len(data.rot) is 0: #assume that no rotation was intended... aka, keep the current rotation I want pose = get_pose() rot = pose['rot'] else: rot = data.rot if len(rot) is not 4: return ("Error. ROT should be a quaternion") if len(data.incrimental) is 0: incrimental = False else: incrimental = True if len(data.target_rot) > 0: target_rot = data.target_rot target_trans = data.target_trans target_rbt = eqf.return_rbt(target_trans, target_rot) else: target_rbt = np.eye(4) if len(data.keep_orient) is 0: keep_orient = False else: keep_orient = True if len(data.change_height) is 0: changeHeight = True else: changeHeight = False print "I am currently at " + str(get_pose()) if move and incrimental: print("Incrimental Move by " + str(trans)) #print("changeHeight?" + str(changeHeight)) incrimental_movement(trans, target_rbt, changeHeight=changeHeight, keep_oreint=keep_orient) elif move: print("Absolute Movement to " + str(trans) + "rot " + str(rot)) move_to_coord(trans, rot, keep_orient) #if bool() #construct a response. Currently, i return a string...1 resp = kinematics_requestResponse("I_did_stuff") print("\n\r") return resp