def execute_move(self, pos): rospy.loginfo('moving') # Read in pose data. Adjust the height to be above the block and the length so that the laser sees the table instead of the block pos.position.z += .1 pos.position.x += .005 q = [pos.orientation.w, pos.orientation.x, pos.orientation.y, pos.orientation.z] p =[[pos.position.x],[pos.position.y],[pos.position.z]] # Convert quaternion data to rotation matrix R = quat_to_so3(q); # Form transformation matrix robot = URDF.from_parameter_server() base_link = robot.get_root() kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base') # Create seed with current position q0 = kdl_kin.random_joint_angles() limb_interface = baxter_interface.limb.Limb('right') limb_interface.set_joint_position_speed(.3) current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()] for ind in range(len(q0)): q0[ind] = current_angles[ind] pose = kdl_kin.forward(q0) pose[0:3,0:3] = R pose[0:3,3] = p # Solve for joint angles, iterating if no solution is found seed = 0.3 q_ik = kdl_kin.inverse(pose, q0+seed) while q_ik == None: seed += 0.3 q_ik = kdl_kin.inverse(pose, q0+seed) rospy.loginfo(q_ik) # Calculate the joint trajectory with a quintic time scaling q_list = JointTrajectory(q0,q_ik,1,50,5) # Iterate through list for q in q_list: # Format joint angles as limb joint angle assignment angles = limb_interface.joint_angles() for ind, joint in enumerate(limb_interface.joint_names()): angles[joint] = q[ind] rospy.sleep(.07) # Send joint move command limb_interface.set_joint_positions(angles) # Publish state and hand position rospy.sleep(1) rospy.loginfo(4) self._pub_state.publish(4) rospy.loginfo(pos) self._pub_hand.publish(pos) self._done = True print('Done')
def execute_move(self, pos): rospy.loginfo('moving') # Read in pose data q = [pos.orientation.w, pos.orientation.x, pos.orientation.y, pos.orientation.z] p =[[pos.position.x],[pos.position.y],[pos.position.z]] # Convert quaternion data to rotation matrix R = quat_to_so3(q); # Form transformation matrix robot = URDF.from_parameter_server() base_link = robot.get_root() kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base') # Create seed with current position q0 = kdl_kin.random_joint_angles() limb_interface = baxter_interface.limb.Limb('right') current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()] for ind in range(len(q0)): q0[ind] = current_angles[ind] pose = kdl_kin.forward(q0) Xstart = copy(np.asarray(pose)) pose[0:3,0:3] = R pose[0:3,3] = p Xend = copy(np.asarray(pose)) # Compute straight-line trajectory for path N = 500 Xlist = CartesianTrajectory(Xstart, Xend, 1, N, 5) thList = np.empty((N,7)) thList[0] = q0; for i in range(N-1): # Solve for joint angles seed = 0 q_ik = kdl_kin.inverse(Xlist[i+1], thList[i]) while q_ik == None: seed += 0.3 q_ik = kdl_kin.inverse(pose, q0+seed) thList[i+1] = q_ik # rospy.loginfo(q_ik) # # Solve for joint angles # seed = 0.3 # q_ik = kdl_kin.inverse(pose, q0+seed) # while q_ik == None: # seed += 0.3 # q_ik = kdl_kin.inverse(pose, q0+seed) # rospy.loginfo(q_ik) # q_list = JointTrajectory(q0,q_ik,1,100,5) # for q in q_list: # Format joint angles as limb joint angle assignment angles = limb_interface.joint_angles() angle_count = 0 for ind, joint in enumerate(limb_interface.joint_names()): # if fabs(angles[joint] - q_ik[ind]) < .05: # angle_count += 1 angles[joint] = q_ik[ind] # rospy.loginfo(angles) rospy.sleep(.003) # Send joint move command rospy.loginfo('move to object') # rospy.loginfo(angle_count) # if angle_count > 4: # nothing = True; # else: limb_interface.set_joint_position_speed(.3) limb_interface.set_joint_positions(angles) self._done = True print('Done')
def execute_move(self, pos): rospy.loginfo('moving') # Read in pose data q = [ pos.orientation.w, pos.orientation.x, pos.orientation.y, pos.orientation.z ] p = [[pos.position.x], [pos.position.y], [pos.position.z]] # Convert quaternion data to rotation matrix R = quat_to_so3(q) # Form transformation matrix robot = URDF.from_parameter_server() base_link = robot.get_root() kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base') # Create seed with current position q0 = kdl_kin.random_joint_angles() limb_interface = baxter_interface.limb.Limb('right') current_angles = [ limb_interface.joint_angle(joint) for joint in limb_interface.joint_names() ] for ind in range(len(q0)): q0[ind] = current_angles[ind] pose = kdl_kin.forward(q0) Xstart = copy(np.asarray(pose)) pose[0:3, 0:3] = R pose[0:3, 3] = p Xend = copy(np.asarray(pose)) # Compute straight-line trajectory for path N = 500 Xlist = CartesianTrajectory(Xstart, Xend, 1, N, 5) thList = np.empty((N, 7)) thList[0] = q0 for i in range(N - 1): # Solve for joint angles seed = 0 q_ik = kdl_kin.inverse(Xlist[i + 1], thList[i]) while q_ik == None: seed += 0.3 q_ik = kdl_kin.inverse(pose, q0 + seed) thList[i + 1] = q_ik # rospy.loginfo(q_ik) # # Solve for joint angles # seed = 0.3 # q_ik = kdl_kin.inverse(pose, q0+seed) # while q_ik == None: # seed += 0.3 # q_ik = kdl_kin.inverse(pose, q0+seed) # rospy.loginfo(q_ik) # q_list = JointTrajectory(q0,q_ik,1,100,5) # for q in q_list: # Format joint angles as limb joint angle assignment angles = limb_interface.joint_angles() angle_count = 0 for ind, joint in enumerate(limb_interface.joint_names()): # if fabs(angles[joint] - q_ik[ind]) < .05: # angle_count += 1 angles[joint] = q_ik[ind] # rospy.loginfo(angles) rospy.sleep(.003) # Send joint move command rospy.loginfo('move to object') # rospy.loginfo(angle_count) # if angle_count > 4: # nothing = True; # else: limb_interface.set_joint_position_speed(.3) limb_interface.set_joint_positions(angles) self._done = True print('Done')
def main_func(): #Initialise Node and ROS variables rospy.init_node('tag_select', anonymous=False) rate = rospy.Rate(10) # 10hz listener = tf.TransformListener() #rtime = rospy.Time(0) #Initialize variables and empty lists trans = [] rot = [] dist = [] id_list = [] #tag_pose = Pose() tag_found = False global state ###Define Subsribers and Publishers #set callback for state rospy.Subscriber('state',Int16,set_state_callback) rospy.Subscriber("/cameras/right_hand_camera/image", Image,xdisplay_pub) state_pub = rospy.Publisher('state', Int16, queue_size=10, latch=True) tag_pub = rospy.Publisher('block_position', Pose, queue_size = 10, latch=True) goal_pub = rospy.Publisher('goal', Int16, queue_size = 10, latch=True) #Sleep Before entering loop rospy.sleep(.5) ''' try: (transWC,rotWC) = listener.lookupTransform('base', 'right_hand_camera', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue ''' ####Main ROS loop to run node until shutdown while not rospy.is_shutdown(): #IF statement to print current state if state ==1: print "Entered State :", state print "Is tag_found?:", tag_found ###Main State Machine Loop - Only run while state_callback is 1 while state == 1:# and tag_found == False: rospy.loginfo("Entered State 1") rtime = rospy.Time.now() trans = [] rot = [] dist = [] id_list = [] tag_found = False rospy.sleep(.15) print state ##For loop to search through tf frames of markers for n in range(1,7): marker_id = 'ar_marker_%d'%n #tag_found = listener.frameExists(marker_id) #print "Marker found is:", 'ar_marker_%d'%n #Check to see if the frames exists for the marker if listener.frameExists("/right_hand_camera") and listener.frameExists(marker_id): #set the time for the latest lookup rtime1 = listener.getLatestCommonTime("/right_hand_camera", marker_id) #check to see if this time is greater that time at beginning of loop if rtime1 > rtime: #set the tag_found to true tag_found = True rtime1 = rtime print "Marker list: AR_Marker", n, "exist:", tag_found ##when a frame is found for a particular Marker "n" add to list ##only 1 addition per "n" in for if tag_found == True: #print "looking at tag:",n #(transWC,rotWC) = listener.lookupTransform('base', 'right_hand_camera', rtime) posCB,quatCB = listener.lookupTransform('right_hand_camera', marker_id, rospy.Time(0)) #print "posCB:", posCB #print "quatCB", quatCB trans.append(posCB) rot.append(quatCB) dist.append(np.linalg.norm(posCB)) id_list.append(n) #print marker_id, ":", posCB tag_found = False ##IF Tag list is not empty we have a tag if len(dist)>0: #set tag_found tag_found = True #choose the closest tag found (if more than one) tag_index = np.argmin(dist) tag_selected = id_list[tag_index] tag_transform = (trans[tag_index],rot[tag_index]) print "tag index is", tag_index #get tag and world frames into correct variables posCB, quatCB = tag_transform posWC,quatWC = listener.lookupTransform('base', 'right_hand_camera', rospy.Time(0)) print "posCB from tf:", posCB #create proper format for pos and quat variables for functions posCB = np.array([posCB[0], posCB[1], posCB[2]]) posWC = np.array([posWC[0], posWC[1], posWC[2]]) quatCB = np.array([quatCB[3],quatCB[0],quatCB[1],quatCB[2]]) quatWC = np.array([quatWC[3],quatWC[0],quatWC[1],quatWC[2]]) #convert quat to Rotations rotWC = quat_to_so3(quatWC) rotCB = quat_to_so3(quatCB) #Build Transform SE3 matrix gWC = RpToTrans(rotWC,posWC) gCB = RpToTrans(rotCB,posCB) gWB = gWC.dot(gCB) #Reorient Orientation so that gripper is correct beta = pi gpick = gWB.dot(np.array([[cos(beta),0, -sin(beta),0],[0,1,0,0],[sin(beta),0, cos(beta),0],[0,0,0,1]])) #Recreate quatWB and rotWB (Block in world frame) from pickup Transform rotWB, posWB = TransToRp(gpick) quatWB = so3_to_quat(rotWB) #Create poses and orientations for block in world bpos = Pose() bpos.position.x = posWB[0] bpos.position.y = posWB[1] bpos.position.z = -.1489 #posWB[2] bpos.orientation.x = .99 bpos.orientation.y = -.024 bpos.orientation.z = .024 bpos.orientation.w = .0133 #print statements print "PosWB:", posWB print "Distance is:", dist[np.argmin(dist)] ##PUBLISH TAG if it Exists if tag_found == True: #Set locally and publish new state if tag_selected < 3: goal_pub.publish(1) else: goal_pub.publish(2) rospy.loginfo(2) state_pub.publish(2) #publish tage Pose tag_pub.publish(bpos) print "tag selected", tag_selected print state del posCB,quatCB,posWC,quatWC,posWB,quatWB ###Part of Main While rate.sleep()
def main_func(): #img = Image() rospy.init_node('tag_select', anonymous=False) rate = rospy.Rate(10) # 10hz count = 0 first_count = True listener = tf.TransformListener() trans = [] rot = [] dist = [] id_list = [] tag_pose = Pose() tag_found = False state = 1 rtime = rospy.Time(0) rospy.Subscriber('state',Int16,set_state_callback) # while listener.frameExists('base') == False: # try: # (transWC,rotWC) = listener.lookupTransform('base', 'right_hand_camera', rospy.Time(0)) # except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): # continue rospy.sleep(1) while not rospy.is_shutdown(): #if count%10==0: rospy.Subscriber("/cameras/right_hand_camera/image", Image,xdisplay_pub,count) #tag_found, tag_transform, WCtransform = multi_marker(listener,tag_found) while tag_found == False: #posCB, quatCB, posWC, quatWC = multi_marker(listener) for n in range(1,7): marker_id = 'ar_marker_%d'%n marker_found = listener.frameExists(marker_id) if marker_found == True: #print "looking at tag:",n #(transWC,rotWC) = listener.lookupTransform('base', 'right_hand_camera', rtime) posCB,quatCB = listener.lookupTransform('right_hand_camera', marker_id, rtime) print "posCB:", posCB print "quatCB", quatCB trans.append(posCB) rot.append(quatCB) dist.append(np.linalg.norm(posCB)) id_list.append(n) print marker_id, ":", posCB if len(dist)>0: tag_index = np.argmin(dist) print "tag index is", tag_index tag_selected = id_list[tag_index] tag_transform = (trans[tag_index],rot[tag_index]) tag_found = True #listener.waitForTransform('base', 'right_hand_camera', rospy.Time(0), rospy.Duration(.5)) posCB, quatCB = tag_transform posWC,quatWC = listener.lookupTransform('base', 'right_hand_camera', rospy.Time(0)) print "posCB after if:", posCB print "quatCB acter if:", quatCB print "posWC", posWC print "quatWC", quatWC posCB = np.array([posCB[0], posCB[1], posCB[2]]) posWC = np.array([posWC[0], posWC[1], posWC[2]]) quatCB = np.array([quatCB[3],quatCB[0],quatCB[1],quatCB[2]]) quatWC = np.array([quatWC[3],quatWC[0],quatWC[1],quatWC[2]]) # print tag_transform print "Distance is:", dist[np.argmin(dist)] count += 1 #if tag_found == True: #pub_tag_pose(tag_transform, WCtransform) #print tag_transform rotWC = quat_to_so3(quatWC) rotCB = quat_to_so3(quatCB) gWC = RpToTrans(rotWC,posWC) print "gWC is: ", gWC gCB = RpToTrans(rotCB,posCB) gWB = gWC.dot(gCB) beta = pi gpick = gWB.dot(np.array([[cos(beta),0, -sin(beta),0],[0,1,0,0],[sin(beta),0, cos(beta),0],[0,0,0,1]])) print "gWB is: ", gWB print "gCB is: ", gCB rotWB, posWB = TransToRp(gpick) quatWB = so3_to_quat(rotWB) print "quatWB", quatWB print "PosWB:", posWB bpos = Pose() bpos.position.x = posWB[0] bpos.position.y = posWB[1] bpos.position.z = posWB[2] bpos.orientation.x = quatWB[1] bpos.orientation.y = quatWB[2] bpos.orientation.z = quatWB[3] bpos.orientation.w = quatWB[0] tag_found = False state_pub = rospy.Publisher('state', Int16, queue_size=10) tag_pub = rospy.Publisher('block_position', Pose, queue_size = 10) for i in range(10): # state_pub = rospy.Publisher('state', Int16, queue_size=10) state_pub.publish(2) # tag_pub = rospy.Publisher('block_position', Pose, queue_size = 10) tag_pub.publish(bpos) print "tag selected", tag_selected #rospy.sleep(1) #break print state rate.sleep()
def main_func(): #Initialise Node and ROS variables rospy.init_node('tag_select', anonymous=False) rate = rospy.Rate(10) # 10hz listener = tf.TransformListener() #rtime = rospy.Time(0) #Initialize variables and empty lists trans = [] rot = [] dist = [] id_list = [] #tag_pose = Pose() tag_found = False global state ###Define Subsribers and Publishers #set callback for state rospy.Subscriber('state', Int16, set_state_callback) rospy.Subscriber("/cameras/right_hand_camera/image", Image, xdisplay_pub) state_pub = rospy.Publisher('state', Int16, queue_size=10, latch=True) tag_pub = rospy.Publisher('block_position', Pose, queue_size=10, latch=True) goal_pub = rospy.Publisher('goal', Int16, queue_size=10, latch=True) #Sleep Before entering loop rospy.sleep(.5) ''' try: (transWC,rotWC) = listener.lookupTransform('base', 'right_hand_camera', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue ''' ####Main ROS loop to run node until shutdown while not rospy.is_shutdown(): #IF statement to print current state if state == 1: print "Entered State :", state print "Is tag_found?:", tag_found ###Main State Machine Loop - Only run while state_callback is 1 while state == 1: # and tag_found == False: rospy.loginfo("Entered State 1") rtime = rospy.Time.now() trans = [] rot = [] dist = [] id_list = [] tag_found = False rospy.sleep(.15) print state ##For loop to search through tf frames of markers for n in range(1, 7): marker_id = 'ar_marker_%d' % n #tag_found = listener.frameExists(marker_id) #print "Marker found is:", 'ar_marker_%d'%n #Check to see if the frames exists for the marker if listener.frameExists("/right_hand_camera" ) and listener.frameExists(marker_id): #set the time for the latest lookup rtime1 = listener.getLatestCommonTime( "/right_hand_camera", marker_id) #check to see if this time is greater that time at beginning of loop if rtime1 > rtime: #set the tag_found to true tag_found = True rtime1 = rtime print "Marker list: AR_Marker", n, "exist:", tag_found ##when a frame is found for a particular Marker "n" add to list ##only 1 addition per "n" in for if tag_found == True: #print "looking at tag:",n #(transWC,rotWC) = listener.lookupTransform('base', 'right_hand_camera', rtime) posCB, quatCB = listener.lookupTransform( 'right_hand_camera', marker_id, rospy.Time(0)) #print "posCB:", posCB #print "quatCB", quatCB trans.append(posCB) rot.append(quatCB) dist.append(np.linalg.norm(posCB)) id_list.append(n) #print marker_id, ":", posCB tag_found = False ##IF Tag list is not empty we have a tag if len(dist) > 0: #set tag_found tag_found = True #choose the closest tag found (if more than one) tag_index = np.argmin(dist) tag_selected = id_list[tag_index] tag_transform = (trans[tag_index], rot[tag_index]) print "tag index is", tag_index #get tag and world frames into correct variables posCB, quatCB = tag_transform posWC, quatWC = listener.lookupTransform( 'base', 'right_hand_camera', rospy.Time(0)) print "posCB from tf:", posCB #create proper format for pos and quat variables for functions posCB = np.array([posCB[0], posCB[1], posCB[2]]) posWC = np.array([posWC[0], posWC[1], posWC[2]]) quatCB = np.array([quatCB[3], quatCB[0], quatCB[1], quatCB[2]]) quatWC = np.array([quatWC[3], quatWC[0], quatWC[1], quatWC[2]]) #convert quat to Rotations rotWC = quat_to_so3(quatWC) rotCB = quat_to_so3(quatCB) #Build Transform SE3 matrix gWC = RpToTrans(rotWC, posWC) gCB = RpToTrans(rotCB, posCB) gWB = gWC.dot(gCB) #Reorient Orientation so that gripper is correct beta = pi gpick = gWB.dot( np.array([[cos(beta), 0, -sin(beta), 0], [0, 1, 0, 0], [sin(beta), 0, cos(beta), 0], [0, 0, 0, 1]])) #Recreate quatWB and rotWB (Block in world frame) from pickup Transform rotWB, posWB = TransToRp(gpick) quatWB = so3_to_quat(rotWB) #Create poses and orientations for block in world bpos = Pose() bpos.position.x = posWB[0] bpos.position.y = posWB[1] bpos.position.z = -.1489 #posWB[2] bpos.orientation.x = .99 bpos.orientation.y = -.024 bpos.orientation.z = .024 bpos.orientation.w = .0133 #print statements print "PosWB:", posWB print "Distance is:", dist[np.argmin(dist)] ##PUBLISH TAG if it Exists if tag_found == True: #Set locally and publish new state if tag_selected < 3: goal_pub.publish(1) else: goal_pub.publish(2) rospy.loginfo(2) state_pub.publish(2) #publish tage Pose tag_pub.publish(bpos) print "tag selected", tag_selected print state del posCB, quatCB, posWC, quatWC, posWB, quatWB ###Part of Main While rate.sleep()
def execute_move(self, pos): rospy.loginfo('moving') # Read in pose data. Adjust the height to be above the block and the length so that the laser sees the table instead of the block pos.position.z += .1 pos.position.x += .005 q = [ pos.orientation.w, pos.orientation.x, pos.orientation.y, pos.orientation.z ] p = [[pos.position.x], [pos.position.y], [pos.position.z]] # Convert quaternion data to rotation matrix R = quat_to_so3(q) # Form transformation matrix robot = URDF.from_parameter_server() base_link = robot.get_root() kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base') # Create seed with current position q0 = kdl_kin.random_joint_angles() limb_interface = baxter_interface.limb.Limb('right') limb_interface.set_joint_position_speed(.3) current_angles = [ limb_interface.joint_angle(joint) for joint in limb_interface.joint_names() ] for ind in range(len(q0)): q0[ind] = current_angles[ind] pose = kdl_kin.forward(q0) pose[0:3, 0:3] = R pose[0:3, 3] = p # Solve for joint angles, iterating if no solution is found seed = 0.3 q_ik = kdl_kin.inverse(pose, q0 + seed) while q_ik == None: seed += 0.3 q_ik = kdl_kin.inverse(pose, q0 + seed) rospy.loginfo(q_ik) # Calculate the joint trajectory with a quintic time scaling q_list = JointTrajectory(q0, q_ik, 1, 50, 5) # Iterate through list for q in q_list: # Format joint angles as limb joint angle assignment angles = limb_interface.joint_angles() for ind, joint in enumerate(limb_interface.joint_names()): angles[joint] = q[ind] rospy.sleep(.07) # Send joint move command limb_interface.set_joint_positions(angles) # Publish state and hand position rospy.sleep(1) rospy.loginfo(4) self._pub_state.publish(4) rospy.loginfo(pos) self._pub_hand.publish(pos) self._done = True print('Done')